/**
 * @file watchMain.cpp
 * @author Gustice
 * @brief ChaosWatch Application
 *
 * @copyright Copyright (c) 2024
 */

#include "watchMain.hpp"

static const char *TAG = "Watch";
static esp_eth_handle_t s_eth_handle = NULL;
static bool s_ethernet_is_connected = false;

extern "C" { // This switch allows the ROS C-implementation to find this main
void app_main(void);
}

struct DeviceState {
    int jumps;
    int pictogram;
    int profile;
    float speed = 1.0; // Rad/sec
    float accel = 1.0; // Rad/sec^2
    Color color{16, 16, 16};
};

DeviceState activeState;
enum class MoveType {
    Step,
    AllPictograms,
    Arbitrary,
};
struct MoveCommand {
    MoveType type;
    uint8_t deltaPic;
    uint8_t profile;
    Color color;
};
static fos::Queue<MoveCommand> commandQueue(8);


std::unique_ptr<Cli> shellPtr;

/// @brief Callback for update start
/// @param args callback arguments
void startUpdate(Cli::CArgs args) {
    ESP_LOGI(TAG, "Invoking Update");
    if (args.size() != 1) {
        shellPtr->write("Need URL to application'\n");
        return;
    }
    // @ToDo: Start Update here
}

/// @brief Callback for commands that are not directly wired
/// @param name command name
/// @param args callback arguments
void onCommand(std::string name, Cli::CArgs args) {
    ESP_LOGI(TAG, "Invoking: %s", name.c_str());
}

void showLogServerConfig(Cli::CArgs args) {
    ESP_LOGI(TAG, "on showLogServerConfig:");
    // auto &store = NvsApi::getInstance();
    // auto logger = store.getValue("syslogDst");
    // shellPtr->write(std::string{"syslog destination = '"} + logger + "'\n");
}

void setLogServerConfig(Cli::CArgs args) {
    ESP_LOGI(TAG, "on showLogServerConfig:");
    if (args.size() != 1) {
        shellPtr->write("Need IP address to syslog server'\n");
        return;
    }
    // auto &store = NvsApi::getInstance();
    // shellPtr->write(std::string{"setting syslog destination to = '"} + args[0] + "'\n");
    // store.setValue("syslogDst", args[0]);
}

/**
 * @brief Telnet socket via TCP/IP
 *
 * @details The ini-file for TeraTerm only changes CRReceive and CRSend values both to "=CR"
 * @param arg ignored task argument
 */
static void telnetSocketTask(int addrFamily) {
    ESP_LOGI(TAG, "Starting Telnet task");
    try {
        TelnetServer telnet(addrFamily);
        telnet.run(
            [](TcpSession &session) {
                shellPtr = std::make_unique<Cli>(session, [](std::string n, Cli::CArgs a) { onCommand(n, a); });
                shellPtr->addBinding(
                    {"update", "Start OTA-Update-Process",
                     [](Cli::CArgs a) { startUpdate(a); }});
                shellPtr->addBinding(
                    {"logserver_show", "Shows log server config ip",
                     [](Cli::CArgs a) { showLogServerConfig(a); }});
                shellPtr->addBinding(
                    {"logserver_set", "Set log server config ip",
                     [](Cli::CArgs a) { setLogServerConfig(a); }});
            },
            [](TcpSession &session) {
                shellPtr->process();
                Syslog::setupTask(session.getOther());
            });
    } catch (const std::exception &e) {
        ESP_LOGE("TCP", "%s", e.what());
    }

    vTaskDelete(NULL);
}


constexpr uint32_t DmxUniverse = 0;
constexpr int DmxChannelCount = 9;

uint8_t scaledRgbChannel(uint8_t value, float scale) {
    return static_cast<uint8_t>(value * scale);
}

/**
 * @brief Callback for ArtNet DMX-socket. Does not send data only updates the image
 *
 * @param data Pointer to dmx-values
 * @param size length of dmx-values
 */
static void UniverseCallback(const uint8_t *data, const uint16_t size) {
    int len = std::min(static_cast<int>(size), DmxChannelCount);
    ESP_LOGD(TAG, "Got DMX");
    ESP_LOG_BUFFER_HEXDUMP("DMX", data, len, ESP_LOG_DEBUG);
    Syslog::logv(Syslog::Severity::Debug, "Got DMX: r%0x g%0x b%0x i%0x  trig%i j%i p%i a%i w%i",
                 data[0], data[1], data[2], data[3], data[4], data[5], data[6], data[7], data[8]);

    float intens = (float)data[3] / UINT8_MAX;
    // activeState.color = Color{
    //     scaledRgbChannel(data[0], intens),
    //     scaledRgbChannel(data[1], intens),
    //     scaledRgbChannel(data[2], intens),
    //     0};

    static uint8_t lastTrig = 0;
    uint8_t thisTrig = data[4];
    if (thisTrig == lastTrig) {
        return;
    }
    lastTrig = thisTrig;
    uint8_t jumps = data[5];
    activeState.jumps = jumps;
    uint8_t profile = data[6];
    uint8_t accel = data[7];
    uint8_t velo = data[8];
    activeState.accel = (float)accel / 100 + 0.5; // min .5 / max 3
    activeState.speed = (float)velo / 100 + 0.5;  // min .5 / max 3
    activeState.profile = profile;

    Syslog::logv(Syslog::Severity::Notice, "Command detected step=%i prof=%i accel=%.1f velo=%.1f", jumps, profile, activeState.accel, activeState.speed);
    commandQueue.enqueue(std::make_unique<MoveCommand>(MoveType::Step, jumps, profile, activeState.color));
}

/**
 * @brief DMX-Socket for ArtNet TCP/IP
 *
 * @details Tested with [Q Light Controller+](https://www.qlcplus.org/)
 * @param args ignored task argument
 */
static void dmxSocketSenderTask(UdpSocket &socket) {
    try {
        ArtnetSender artnet(socket);
        std::array<uint8_t, 16> data;
        while (socket.isActive()) {
            data[0]++;
            artnet.send("", 0, data.data(), data.size());
            fos::Task::delayMs(500);
        }

    } catch (const std::exception &e) {
        ESP_LOGE("UDP", "%s", e.what());
    }
}

/**
 * @brief DMX-Socket for ArtNet TCP/IP
 *
 * @details Tested with [Q Light Controller+](https://www.qlcplus.org/)
 * @param args ignored task argument
 */
static void dmxSocketTask(int addrFamily) {

    ESP_LOGI(TAG, "Starting ArtNet task");
    constexpr uint16_t ArtNetPort = arx::artnet::DEFAULT_PORT;
    IpAdr address = addrFamily == AF_INET ? IpAdr(INADDR_ANY, ArtNetPort) : IpAdr(ArtNetPort);

    try {
        while (true) {
            UdpSocket socket(address);
            socket.bindTo();

            ArtnetReceiver artnet(socket);
            artnet.subscribe(DmxUniverse, UniverseCallback);
            artnet.parse();
            // auto msg = std::make_unique<Display::Message>(Display::Message::Type::UpdateInfo, "DMX-IP-Mode");
            // displayEvents.enqueue(std::move(msg));
            // deviceState.setNewState(DeviceState::Mode::Remote);
            // xTaskCreate(valueRefresherTask, "valueRefresher", 4096, nullptr, 5, NULL);
            // static TaskT<UdpSocket &> dmxSender(dmxSocketSenderTask, socket, "dmxSender", 4096);

            // parse artnet as long socket is active, then restart socket
            while (socket.isActive()) {
                ESP_LOGD(TAG, "Waiting for data");
                artnet.parse();
                // socket.write(data);
            }
            // deviceState.fallbackToLast();
        }

    } catch (const std::exception &e) {
        ESP_LOGE("UDP", "%s", e.what());
    }
}

void startServerTasks() {
    static int ipVersion = AF_INET;
    static fos::TaskT<int> telnetSocket(telnetSocketTask, ipVersion, "tcpServer", 4096);
    static fos::TaskT<int> dmxNode(dmxSocketTask, ipVersion, "dmxNode", 4096);
    // static Task otaUpdate(SystemFunctions::OtaTaskEntry, "otaTask", 8192);
    // initialise_mdns();
    // netbiosns_init();
    // netbiosns_set_name(CONFIG_EXAMPLE_MDNS_HOST_NAME);
    // auto &store = NvsApi::getInstance();
    // auto logger = store.getValue("syslogDst");
    // if (logger != "") {
    //     Syslog::setupTask(IpAdr{logger, 0});
    // }
}

/** Event handler for IP_EVENT_ETH_GOT_IP */
static void got_ip_event_handler(void *arg, esp_event_base_t event_base,
                                 int32_t event_id, void *event_data)
                                 {
    ip_event_got_ip_t *event = (ip_event_got_ip_t *) event_data;
    const esp_netif_ip_info_t *ip_info = &event->ip_info;

    ESP_LOGI(TAG, "Ethernet Got IP Address");
    ESP_LOGI(TAG, "~~~~~~~~~~~");
    ESP_LOGI(TAG, "ETHIP:" IPSTR, IP2STR(&ip_info->ip));
    ESP_LOGI(TAG, "ETHMASK:" IPSTR, IP2STR(&ip_info->netmask));
    ESP_LOGI(TAG, "ETHGW:" IPSTR, IP2STR(&ip_info->gw));
    ESP_LOGI(TAG, "~~~~~~~~~~~");
    startServerTasks();
}

// Event handler for Ethernet
static void eth_event_handler(void *arg, esp_event_base_t event_base,
                              int32_t event_id, void *event_data)
                              {
    uint8_t mac_addr[6] = {0};
    switch (event_id) {
    case ETHERNET_EVENT_CONNECTED:
        ESP_LOGI(TAG, "Ethernet Link Up");
        s_ethernet_is_connected = true;
        ESP_LOGI(TAG, "Ethernet Link Up");
        ESP_LOGI(TAG, "Ethernet HW Addr %02x:%02x:%02x:%02x:%02x:%02x",
                 mac_addr[0], mac_addr[1], mac_addr[2], mac_addr[3], mac_addr[4], mac_addr[5]);
        break;
    case ETHERNET_EVENT_DISCONNECTED:
        ESP_LOGI(TAG, "Ethernet Link Down");
        s_ethernet_is_connected = false;
        break;
    case ETHERNET_EVENT_START:
        ESP_LOGI(TAG, "Ethernet Started");
        break;
    case ETHERNET_EVENT_STOP:
        ESP_LOGI(TAG, "Ethernet Stopped");
        break;
    default:
        break;
    }
}

static void initialize_ethernet(void) 
{
    uint8_t eth_port_cnt = 0;
    esp_eth_handle_t *eth_handles;
    ESP_ERROR_CHECK(example_eth_init(&eth_handles, &eth_port_cnt));
    if (eth_port_cnt > 1) {
        ESP_LOGW(TAG, "multiple Ethernet devices detected, the first initialized is to be used!");
    }

    s_eth_handle = eth_handles[0];
    free(eth_handles);

    // Initialize TCP/IP network interface aka the esp-netif (should be called only once in application)
    ESP_ERROR_CHECK(esp_netif_init());
    // Create default event loop that running in background
    ESP_ERROR_CHECK(esp_event_loop_create_default());

    // Use ESP_NETIF_DEFAULT_ETH when just one Ethernet interface is used and you don't need to modify
    // default esp-netif configuration parameters.
    esp_netif_config_t cfg = ESP_NETIF_DEFAULT_ETH();
    esp_netif_t *eth_netif = esp_netif_new(&cfg);
    // Attach Ethernet driver to TCP/IP stack
    ESP_ERROR_CHECK(esp_netif_attach(eth_netif, esp_eth_new_netif_glue(s_eth_handle)));

    ESP_ERROR_CHECK(esp_event_handler_register(ETH_EVENT, ESP_EVENT_ANY_ID, eth_event_handler, NULL));
    ESP_ERROR_CHECK(esp_event_handler_register(IP_EVENT, IP_EVENT_ETH_GOT_IP, &got_ip_event_handler, NULL));
    ESP_ERROR_CHECK(esp_eth_start(s_eth_handle));
}

namespace Effect {
    extern const WaveForm_t OffsetPulseWave{gau8_offsetPulse, cu16_TemplateLength};
    extern const WaveForm_t InitSlopeWave{gau8_initSlope, cu16_TemplateLength};
    extern const WaveForm_t OffsetSlopeWave{gau8_offsetSlope, cu16_TemplateLength};
    extern const WaveForm_t FullSlopeWave{gau8_fullSlope, cu16_TemplateLength};
    extern const WaveForm_t FullPulseWave{gau8_fullPulse, cu16_TemplateLength};

    const Macro macDemoStart = {
        {64, 1, &InitSlopeWave, StateMachine::UpdateWave, 0xFF},
        {32, 1, StateMachine::UpdateIdle, 0xFF},
    };

    const Macro macDemoPulse = {
        {64, 1, &InitSlopeWave, StateMachine::UpdateWave, 0xFF},
        {32, 2, StateMachine::UpdateIdle, 0xFF},
        {64, 1, &OffsetPulseWave, StateMachine::UpdateWave, 0xFF},
    };

    const Macro macDemoFlicker = {
        {64, 1, &InitSlopeWave, StateMachine::UpdateWave, 0xFF},
        {32, 1, StateMachine::UpdateFlicker, 0xFF},
    };
}

void LightingTask() {
    ESP_LOGI(TAG, "Start Lighting");

    static I2cMaster master(I2C_NUM_0,
        /*sda=*/ GPIO_NUM_10,
        /*scl=*/ GPIO_NUM_11);
    // static PmwExpanderPca9685 pca9685(master);
    static PmwExpanderPca9685 ledDriver1(master, /*iverted=*/ false);
    static PmwExpanderPca9685 ledDriver2(master, /*iverted=*/ false, 0x41);
    PmwExpanderPca9685::GrayValues values[2];

    OutputPort pwmEnable(GPIO_NUM_5, GpioPort::Logic::Inverted);
    pwmEnable.write(true);

    int idx = 0;
    while (true) {
        values[0].fill(0);
        values[1].fill(0);

        // values[x][4] = 0x200; // Actually first
        values[0][idx] = 0x200;
        ledDriver1.sendImage(values[0]);

        // values[1][4] = 0x100; // Actually first
        values[1][idx] = 0x400;
        ledDriver2.sendImage(values[1]);

        if (++idx >= 16 ) {
            idx = 0;
        }
        fos::Task::delayMs(100);
    }


    // ESP_LOGI(TAG, "Start Lighting");

    // static I2cMaster master(I2C_NUM_0, GPIO_NUM_10, GPIO_NUM_11);
    // static PmwExpanderPca9685 pca9685(master);
    // PmwExpanderPca9685::GrayValues values;

    // AddressableLeRtz ledBar(GPIO_NUM_15, AddressableLeRtz::Type::Rgb, 6);
    // std::vector<Color> pictoSpots(6);
    // for (auto &&led : pictoSpots) {
    //     led.set(0, 0, 0);
    // }

    // auto const &colorSource = activeState.color;
    // std::array<Effect::EffectProcessor, 6> processors{
    //     Effect::EffectProcessor(8),
    //     Effect::EffectProcessor(8),
    //     Effect::EffectProcessor(8),
    //     Effect::EffectProcessor(8),
    //     Effect::EffectProcessor(8),
    //     Effect::EffectProcessor(8)};

    // processors[0].setEffect(Effect::macDemoStart, &colorSource, 16);
    // processors[1].setEffect(Effect::macDemoStart, &colorSource, 32);
    // processors[2].setEffect(Effect::macDemoPulse, &colorSource, 48);
    // processors[3].setEffect(Effect::macDemoPulse, &colorSource, 64);
    // processors[4].setEffect(Effect::macStartFlicker, &colorSource, 80);
    // processors[5].setEffect(Effect::macStartIdle, &colorSource, 96);

    // int idx = 0;
    // values.fill(0);
    // while (true) {
    //     values[0] = activeState.color.getRed() * 257;
    //     values[1] = activeState.color.getGreen() * 257;
    //     values[2] = activeState.color.getBlue() * 257;
    //     pca9685.sendImage(values);

    //     for (size_t i = 0; i < 600; i++) {
    //         Task::delayMs(20);
    //         for (size_t i = 0; i < pictoSpots.size(); i++) {
    //             pictoSpots[i] = processors[i].tick();
    //         }
    //         ledBar.send(pictoSpots);
    //     }

    //     processors[1].setEffect(Effect::macDark, &colorSource, 32);
    //     processors[2].setEffect(Effect::macDark, &colorSource, 48);
    //     processors[3].setEffect(Effect::macDark, &colorSource, 64);
    //     processors[4].setEffect(Effect::macDark, &colorSource, 80);

    //     for (size_t i = 0; i < 50; i++) {
    //         Task::delayMs(20);
    //         for (size_t i = 0; i < pictoSpots.size(); i++) {
    //             pictoSpots[i] = processors[i].tick();
    //         }
    //         ledBar.send(pictoSpots);
    //     }

    //     processors[1].setEffect(Effect::macDemoStart, &colorSource, 32);
    //     processors[2].setEffect(Effect::macDemoPulse, &colorSource, 48);
    //     processors[3].setEffect(Effect::macDemoPulse, &colorSource, 64);
    //     processors[4].setEffect(Effect::macStartFlicker, &colorSource, 80);
    // }
}

void MovementTask() {
    ESP_LOGI(TAG, "Start Stepper");
    constexpr int MicroSteps = 32;
    StepperDriverDrv8833::Pins stepperPins{
        LEDC_TIMER_0,
        {LEDC_CHANNEL_0, LEDC_CHANNEL_1, LEDC_CHANNEL_2, LEDC_CHANNEL_3},
        {GPIO_NUM_23, GPIO_NUM_22, GPIO_NUM_21, GPIO_NUM_20},
    };
    StepperDriverDrv8833 stepperDriver(stepperPins, MicroSteps);
    stepperDriver.setPos(0);

    StepperMotor::Config stepperCfg{1.8F, MicroSteps};
    InputPort initSens(GPIO_NUM_18);
    
    InputPort motorFault(GPIO_NUM_8);
    motorFault.setLogic(GpioPort::Logic::Inverted);
    
    OutputPort motorSleep(GPIO_NUM_15, GpioPort::Logic::Inverted);
    motorSleep.write(false);
    
    StepperMotor stepper(stepperCfg, stepperDriver, initSens);

    int i = 0;
    while (i++ < 80) {
        stepperDriver.step(1);
        stepperDriver.getSubPos();
        fos::Task::delayMs(100);
    }

    ESP_LOGI("Stepper", "Find Home");
    stepper.findRef();
    ESP_LOGI(TAG, "Fist Position is %f.2", stepper.getScaledPos() * 6);
    fos::Task::delayMs(1000);

    while (true) {
        fos::Task::delayMs(100);
    }



    // ESP_LOGI(TAG, "Start Stepper");
    // StepperDriverTb6612::Pins stepperPins{
    //     LEDC_TIMER_0,
    //     LEDC_CHANNEL_0,
    //     GPIO_NUM_23,
    //     {GPIO_NUM_22, GPIO_NUM_21},
    //     LEDC_CHANNEL_1,
    //     GPIO_NUM_20,
    //     {GPIO_NUM_19, GPIO_NUM_18},
    // };
    // constexpr int MicroSteps = 32;
    // StepperDriverTb6612 stepperDriver(stepperPins, MicroSteps);
    // stepperDriver.setPos(0);

    // StepperMotor::Config stepperCfg{1.8F, MicroSteps};
    // InputPort initSens(GPIO_NUM_3);
    // initSens.setLogic(GpioPort::Logic::Inverted);
    // StepperMotor stepper(stepperCfg, stepperDriver, initSens);

    // ESP_LOGI(TAG, "Find Home");

    // stepper.findRef();
    // ESP_LOGI(TAG, "Fist Position is %f.2", stepper.getScaledPos() * 6);
    // Task::delayMs(1000);

    // AddressableLeRtz ledRing(GPIO_NUM_9, AddressableLeRtz::Type::Rgb, 12);
    // std::vector<Color> ledStrip(12);
    // for (auto &&led : ledStrip) {
    //     led.set(0, 0, 0);
    // }

    // uint32_t ledPos = 0;
    // ledStrip[0].set(
    //     activeState.color.getRed(),
    //     activeState.color.getGreen(),
    //     activeState.color.getBlue(),
    //     0);
    // ledRing.send(ledStrip);

    // while (true) {
    //     ESP_LOGD(TAG, "Waiting for command");
    //     auto cmd = commandQueue.dequeue();
    //     if (cmd->type == MoveType::Step) {
    //         if (stepper.isIdle()) {
    //             auto delta = cmd->deltaPic;
    //             auto profile = cmd->profile;

    //             for (auto &&led : ledStrip) {
    //                 led.set(0, 0, 0);
    //             }
    //             ledPos = (ledPos + delta) % 6;
    //             activeState.pictogram = ledPos;

    //             ledStrip[ledPos * 2].set(
    //                 cmd->color.getRed(),
    //                 cmd->color.getGreen(),
    //                 cmd->color.getBlue(),
    //                 0);
    //             ledRing.send(ledStrip);
    //             MqttClient::publishPosition(ledPos);

    //             delta = std::min<uint8_t>(delta, 7);
    //             Syslog::logv(Syslog::Severity::Notice, "Running Stepper by %i steps with prof=%i a=%.2f w=%.2f", delta, profile, activeState.accel, activeState.speed);
    //             stepper.moveBy(delta * std::numbers::pi / 3,
    //                            profile, activeState.accel, activeState.speed);
    //         }
    //     } else if (cmd->type == MoveType::AllPictograms) {
    //         activeState.pictogram = -1;
    //         stepper.findRef();
    //         Task::delayMs(1000);
    //         for (size_t i = 0; i < 6; i++) {
    //             for (auto &&led : ledStrip) {
    //                 led.set(0, 0, 0);
    //             }
    //             ledStrip[i * 2].set(0, 0, 16);
    //             ledRing.send(ledStrip);
    //             stepper.moveBy(std::numbers::pi / 3);
    //             while (!stepper.isIdle()) {
    //                 Task::delayMs(100);
    //             }
    //             ESP_LOGI(TAG, "Position is %f.2", stepper.getScaledPos() * 6);
    //         }
    //     } else if (cmd->type == MoveType::Arbitrary) {
    //         activeState.pictogram = -1;
    //         ESP_LOGI(TAG, "Full Turn");
    //         stepper.demoFullTurn();
    //         Task::delayMs(4000);

    //         while (true) {
    //             if (stepper.isIdle()) {
    //                 ESP_LOGI(TAG, "Position is %f.2", stepper.getScaledPos() * 6);
    //                 Task::delayMs(500);
    //                 stepper.createDemoProfile();
    //             }
    //             Task::delayMs(20);
    //         }
    //     }
    // }

}

void IoTest() 
{
    ESP_LOGI(TAG, "Start IO");
    InputPort in2(GPIO_NUM_19);
    OutputPort nReset(GPIO_NUM_9);

    while (true) {
        nReset.write(in2.read());
        fos::Task::delayMs(100);
   }
}

void app_main(void) 
{
    esp_err_t ret = nvs_flash_init();
    if (ret == ESP_ERR_NVS_NO_FREE_PAGES || ret == ESP_ERR_NVS_NEW_VERSION_FOUND) {
        ESP_ERROR_CHECK(nvs_flash_erase());
        ret = nvs_flash_init();
    }
    ESP_ERROR_CHECK(ret);
    initialize_ethernet();
    static fos::Task lightingTask(LightingTask, "Lights", 0x4000uL);
    static fos::Task movementTask(MovementTask, "Motor", 0x4000uL);
    static fos::Task ioTask(IoTest, "IO", 0x4000uL);
}