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

// Conig-Key:
//
//
//

struct {
    const char *syslogDest = "syslogDst";  // example: "172.16.16.10"
    const char *mqttBroker = "mqttBroker"; // example: "172.16.17.62"
    const char *mqttPrefix = "mqttPrefix"; // example: "SomeTag"
} ConfigKeys;

// MQTT Topics
// Subscriptions:
//  <mqttPrefix>/watch/MoveTo
//  <mqttPrefix>/watch/Shuffle
// Publishes:
//  <mqttPrefix>/watch/inPosition
//  <mqttPrefix>/watch/changingPosition

#include "watchMain.hpp"

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

struct DeviceState {
    uint8_t minJumps;
    uint8_t maxJumps;
    uint8_t profile;
    uint8_t restTime;
    float speed = 1.0; // Rad/sec
    float accel = 1.0; // Rad/sec^2
    Color color{16, 16, 16};
    uint8_t pictogram;
};

enum class MoveType {
    StepTo,
    Shuffle,
    Demo
};
struct MoveCommand {
    MoveType type;
    uint8_t pos;
};
static fos::Queue<MoveCommand> commandQueue(8);

DeviceState activeState;
static const char *TAG = "Watch";
static bool EthernetIsConnected = false;

std::unique_ptr<Cli> pShell;

void onCommand(std::string name, Cli::CArgs args) {
    ESP_LOGI(TAG, "Invoking: %s", name.c_str());
}

void startUpdate(Cli::CArgs args) {
    ESP_LOGI(TAG, "Invoking Update");
    if (args.size() != 1) {
        pShell->write("Need URL to application'\n");
        return;
    }
    // @ToDo: Start Update here
}

void getConfig(Cli::CArgs args) {
    if (args.size() != 1) {
        pShell->write("Needs string key for query'\n");
        return;
    }
    auto key = args[0];
    auto &store = NvsApi::getInstance();
    auto val = store.getValue(key);
    pShell->write(key + ": " + val + "\n");
}

void setConfig(Cli::CArgs args) {
    if (args.size() != 2) {
        pShell->write("Needs string key and value'\n");
        return;
    }
    auto key = args[0];
    auto val = args[1];

    auto &store = NvsApi::getInstance();
    pShell->write(std::string{"Storing value = '"} + val + "'\n");
    store.setValue(key, val);
}

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

void setDynamics(Cli::CArgs args) {
    ESP_LOGI(TAG, "on setDynamics:");
    if (args.size() != 2) {
        pShell->write("need float values for velocity and acceleration'\n");
        return;
    }

    try {
        float v = std::stof(args[0]);
        float a = std::stof(args[1]);
        activeState.accel = a;
        activeState.speed = v;
    } catch (const std::exception &e) {
        ESP_LOGI(TAG, "Failed to parse %s and %s", args[0].c_str(), args[1].c_str());
    }
}

void triggerMovement(Cli::CArgs args) {
    ESP_LOGI(TAG, "on triggerMovement:");
    if (args.size() != 1) {
        pShell->write("need target position'\n");
        return;
    }

    try {
        int p = std::stoi(args[0]);
        activeState.profile = p;
        commandQueue.enqueue(std::make_unique<MoveCommand>(MoveType::StepTo, p));
    } catch (const std::exception &e) {
        ESP_LOGI(TAG, "Failed to parse %s and %s", args[0].c_str(), args[1].c_str());
    }
}

void startDemoMovement(Cli::CArgs args) {
    ESP_LOGI(TAG, "on startDemoMovement:");
    commandQueue.enqueue(std::make_unique<MoveCommand>(MoveType::Demo));
}

/**
 * @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) {
                pShell = std::make_unique<Cli>(session, [](std::string n, Cli::CArgs a) { onCommand(n, a); });
                pShell->addBinding(
                    {"update", "Start OTA-Update-Process",
                     [](Cli::CArgs a) { startUpdate(a); }});
                pShell->addBinding(
                    {"logserver_set", "Set log server config ip",
                     [](Cli::CArgs a) { setLogServerConfig(a); }});

                pShell->addBinding(
                    {"getcfg", "Get configuration value for key",
                     [](Cli::CArgs a) { getConfig(a); }});
                pShell->addBinding(
                    {"setcfg", "Set configuration value by key and value",
                     [](Cli::CArgs a) { setConfig(a); }});

                pShell->addBinding(
                    {"dyn_set", "set dynamics of movement (velocity,accelerations) as floats",
                     [](Cli::CArgs a) { setDynamics(a); }});
                pShell->addBinding(
                    {"move", "start movement (profile-idx,steps) as integers ",
                     [](Cli::CArgs a) { triggerMovement(a); }});
            },
            [](TcpSession &session) {
                pShell->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  m%i M%i p%i a%i w%i r%i",
                 /* RGB*/ data[0], data[1], data[2], /*Intens*/ data[3],
                 /* MinMaxJumps */ data[4], data[5], /*profile*/ data[6], /*accel/speed*/ data[7], data[8],
                 /*RestTime*/ data[9]);

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

    activeState.minJumps = data[4];
    activeState.maxJumps = data[5];
    activeState.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.restTime = data[9];
}

void moveToSubs(std::string data) {
    //             Syslog::logv(Syslog::Severity::Notice,
    //                          "Running Stepper by %i steps with prof=%i a=%.2f w=%.2f",
    //                          delta, profile, activeState.accel, activeState.speed);

    ESP_LOGI(TAG, "MoveTo command '%s'", data.c_str());
    try {
        int p = std::stoi(data);
        commandQueue.enqueue(std::make_unique<MoveCommand>(MoveType::StepTo, p));
        auto &client = MqttClient::getInstance();
        client.publish("watch/inPosition", data + "Picto");
    } catch (const std::exception &e) {
        ESP_LOGE(TAG, "Error to process data %s, with ex: %s", data.c_str(), e.what());
    }
}

void startShuffle(std::string data) {
    ESP_LOGI(TAG, "StartShuffle command %s", data.c_str());

    try {
        commandQueue.enqueue(std::make_unique<MoveCommand>(MoveType::Shuffle, 0));
        auto &client = MqttClient::getInstance();
        client.publish("watch/changingPosition", "....");
    } catch (const std::exception &e) {
        ESP_LOGE(TAG, "Error to process data %s, with ex: %s", data.c_str(), 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 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();
            // parse artnet as long socket is active, then restart socket
            while (socket.isActive()) {
                ESP_LOGD(TAG, "Waiting for data");
                artnet.parse();
            }
        }

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

fos::Semaphore etherActive;

/** 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, "~~~~~~~~~~~");
    etherActive.give();
}

// 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");
        EthernetIsConnected = 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");
        EthernetIsConnected = 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!");
    }

    static esp_eth_handle_t s_eth_handle = NULL;
    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_1, GpioPort::Logic::Inverted);
    pwmEnable.write(true);

    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);

    //     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);
    //     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);

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

        // values[x][4] = 0x200; // Actually first
        values[0][4] = activeState.color.getRed() * 257;
        values[0][5] = activeState.color.getGreen() * 257;
        values[0][6] = activeState.color.getBlue() * 257;
        
        // values[1][4] = 0x100; // Actually first
        values[1][4] = activeState.color.getRed() * 257;
        values[1][5] = activeState.color.getGreen() * 257;
        values[1][6] = activeState.color.getBlue() * 257;
        
        ledDriver1.sendImage(values[0]);
        ledDriver2.sendImage(values[1]);
        fos::Task::delayMs(40);
    }
}

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, 0.25F);
    stepperDriver.setPos(0);

    StepperConfig 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);

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

    while (true) {
        ESP_LOGD(TAG, "Waiting for command");
        auto cmd = commandQueue.dequeue();
        if (cmd->type == MoveType::StepTo) {
            if (stepper.isIdle()) {
                // activeState.pictogram = ledPos;
                // ledRing.send(ledStrip);
                // activeState.pictogram
                auto delta = std::min<uint8_t>(cmd->pos, 128);
                Syslog::logv(Syslog::Severity::Notice,
                             "Running Stepper by %i steps with prof=%i a=%.2f w=%.2f",
                             delta, activeState.profile, activeState.accel, activeState.speed);
                stepper.moveBy(delta * std::numbers::pi / 3,
                               activeState.profile, activeState.accel, activeState.speed);
            } else {
                fos::Task::delayMs(100);
            }
        } else if (cmd->type == MoveType::Demo) {
            int i = 0;
            while (i++ < 80) {
                stepperDriver.step(1);
                stepperDriver.getSubPos();
                fos::Task::delayMs(100);
            }
            ESP_LOGI("Stepper", "Find Home");
            stepper.findRef();
            activeState.pictogram = -1;
            stepper.findRef();
            fos::Task::delayMs(1000);
            for (size_t i = 0; i < 6; i++) {
                stepper.moveBy(std::numbers::pi / 3);
                while (!stepper.isIdle()) {
                    fos::Task::delayMs(100);
                }
                ESP_LOGI(TAG, "Position is %f.2", stepper.getScaledPos() * 6);
            }
        } else {
            fos::Task::delayMs(100);
        }
    }
}

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();
    NvsApi::init();
    // static fos::Task lightingTask(LightingTask, "Lights", 0x4000uL);
    // static fos::Task movementTask(MovementTask, "Motor", 0x4000uL);
    // static fos::Task ioTask(IoTest, "IO", 0x4000uL);

    etherActive.take();
    ESP_LOGI(TAG, "Start net services");

    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);

    auto &store = NvsApi::getInstance();
    auto logger = store.getValue(ConfigKeys.syslogDest);
    if (logger != "") {
        Syslog::setupTask(IpAdr{logger, 0});
    }

    auto broker = store.getValue(ConfigKeys.mqttBroker);
    if (broker != "") {
        ESP_LOGI(TAG, "Initializing MQTT");
        auto prefix = store.getValue(ConfigKeys.mqttPrefix);
        auto &client = MqttClient::init(broker, prefix);
        if (!client.isRunning()) {
            client.addSubscription("watch/moveTo", [](std::string d) { moveToSubs(d); });
            client.addSubscription("watch/shuffle", [](std::string d) { startShuffle(d); });
            client.start();
        }
    }

    while (true) {
        fos::Task::delayMs(5000);
        // ToDo send
        Syslog::logv(Syslog::Severity::Notice, "Health state");
    }
}