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

/// NOTE: Create IDF shell navigate to directory ("test/integration")
//          and run "idf.py -p COM9 flash monitor"

#include "Processor.hpp"
#include "PwmExpanderPca9685.hpp"
#include "Task.hpp"
#include "esp_event.h"
#include "esp_log.h"
#include "sdkconfig.h"
#include "stepperDrv.hpp"
#include "stepperMotor.hpp"
#include "watchFace.hpp"
#include <array>
#include <cmath>
#include <cstdlib>
#include <cstring>

/* Watchface        Indicies
        6/                  5/
    5   /   1           4   /   0
    4       2           3       1
        3                   2
*/

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

static const char *TAG = "IT";

struct DeviceState {
    uint8_t minJumps{6};
    uint8_t maxJumps{32};
    uint8_t profile{};
    uint8_t restTime{};
    float speed = 1.0; // Rad/sec
    float accel = 1.0; // Rad/sec^2
    Color color{0x80, 0x80, 0x80};
    uint8_t pictogram{}; // 0..5 active Pictogramm
};

class IndexIllumination {
  public:
    IndexIllumination()
        : master(I2C_NUM_0, /*sda=*/GPIO_NUM_10, /*scl=*/GPIO_NUM_11),
          ledDriver1(master, /*iverted=*/false),
          ledDriver2(master, /*iverted=*/false, 0x41),
          pwmEnable(GPIO_NUM_1, GpioPort::Logic::Inverted),
          RED{CRed.getColor<Color::Rgbw>()},
          GREEN{CGreen.getColor<Color::Rgbw>()},
          BLUE{CBlue.getColor<Color::Rgbw>()} {
        pwmEnable.write(true);
        clearColorData();
        syncToDevice();
    }

    void clearColorData() {
        for (size_t i = 0; i < 2; i++) {
            for (size_t j = 0; j < PmwExpanderPca9685::PwmChannels; j++) {
                Values[i][j] = 0;
            }
        }
    }

    void setColorOfIndexTo(uint8_t idx, Color::Rgbw color) {
        if (idx >= 6) {
            return;
        }
        auto sp = LedStartPoints[idx];
        sp[0] = (uint16_t)(color.white) << 5;
        sp[1] = (uint16_t)(color.blue) << 5;
        sp[2] = (uint16_t)(color.green) << 5;
        sp[3] = (uint16_t)(color.red) << 5;
    }

    void syncToDevice() {
        ledDriver1.sendImage(Values[0]);
        ledDriver2.sendImage(Values[1]);
    }

    void flashAll(Color::Rgbw color, int duration = 200) {
        clearColorData();
        for (size_t i = 0; i < 6; i++) {
            setColorOfIndexTo(i, color);
        }
        syncToDevice();
        fos::Task::delayMs(duration);
        clearColorData();
        syncToDevice();
    }

  private:
    PmwExpanderPca9685::GrayValues Values[2]{};
    std::array<uint16_t *, 6> LedStartPoints{
        &(Values[0][4]), // 1
        &(Values[1][12]),
        &(Values[1][8]),
        &(Values[1][4]),
        &(Values[0][12]),
        &(Values[0][8]), // 6
    };

    I2cMaster master;
    PmwExpanderPca9685 ledDriver1;
    PmwExpanderPca9685 ledDriver2;
    OutputPort pwmEnable;
    const Color::Rgbw RED;
    const Color::Rgbw GREEN;
    const Color::Rgbw BLUE;
};

DeviceState activeState;

void IndexColorSweep(IndexIllumination &illum) {
    const auto RED = CRed.getColor<Color::Rgbw>();
    const auto GREEN = CGreen.getColor<Color::Rgbw>();
    const auto BLUE = CBlue.getColor<Color::Rgbw>();

    illum.flashAll(RED);
    fos::Task::delayMs(200);
    illum.flashAll(GREEN);
    fos::Task::delayMs(200);
    illum.flashAll(BLUE);
    fos::Task::delayMs(200);

    illum.clearColorData();
    illum.setColorOfIndexTo(0, GREEN);
    illum.syncToDevice();
    fos::Task::delayMs(500);

    illum.clearColorData();
    illum.setColorOfIndexTo(0, RED);
    illum.setColorOfIndexTo(1, GREEN);
    illum.syncToDevice();
    fos::Task::delayMs(500);

    illum.clearColorData();
    illum.setColorOfIndexTo(1, RED);
    illum.setColorOfIndexTo(2, GREEN);
    illum.syncToDevice();
    fos::Task::delayMs(500);

    illum.clearColorData();
    illum.setColorOfIndexTo(2, RED);
    illum.setColorOfIndexTo(3, GREEN);
    illum.syncToDevice();
    fos::Task::delayMs(500);

    illum.clearColorData();
    illum.setColorOfIndexTo(3, RED);
    illum.setColorOfIndexTo(4, GREEN);
    illum.syncToDevice();
    fos::Task::delayMs(500);

    illum.clearColorData();
    illum.setColorOfIndexTo(4, RED);
    illum.setColorOfIndexTo(5, GREEN);
    illum.syncToDevice();
    fos::Task::delayMs(500);

    illum.clearColorData();
    illum.setColorOfIndexTo(5, RED);
    illum.syncToDevice();
    fos::Task::delayMs(500);

    illum.clearColorData();
    illum.setColorOfIndexTo(0, GREEN);
    illum.syncToDevice();
}

constexpr bool DoFlushAtBeginning = false;
constexpr bool DoFullTurns = false;
constexpr bool DoTurnSteps = false;
constexpr bool DoTurnProfiles = true;

void AllmostDone(uint8_t newIdx) {
    ESP_LOGW(TAG, "Done callback");
}

void testMovementTask() {
    esp_log_level_set(TAG, ESP_LOG_DEBUG);
    esp_log_level_set("STP", ESP_LOG_DEBUG);
    ESP_LOGD(TAG, "Init Lights for signaling");

    IndexIllumination illum;

    if constexpr (DoFlushAtBeginning) {
        IndexColorSweep(illum);
    }
    const auto BLUE = CBlue.getColor<Color::Rgbw>();

    ESP_LOGD(TAG, "Init Stepper");

    constexpr int MicroSteps = 32;
    constexpr float Intensity = 0.25;
    constexpr float StepWidth = 1.8;
    StepperDriverDrv8833::Pins stepperPins({LEDC_TIMER_0,
                                            {LEDC_CHANNEL_0, LEDC_CHANNEL_1, LEDC_CHANNEL_2, LEDC_CHANNEL_3},
                                            {GPIO_NUM_21, GPIO_NUM_20, GPIO_NUM_23, GPIO_NUM_22}});
    StepperDriverDrv8833 stepperDriver(stepperPins, MicroSteps, Intensity);
    StepperConfig stepperCfg(StepWidth, MicroSteps);
    InputPort initSens(GPIO_NUM_18);
    OutputPort motorSleep(GPIO_NUM_15, GpioPort::Logic::Inverted);
    StepperMotor stepper(stepperCfg, stepperDriver, initSens);

    WatchMotor watch(stepper, motorSleep, AllmostDone);

    ///
    /// Full Turns with full steps
    ///
    if constexpr (DoFullTurns) {
        ESP_LOGD(TAG, "Find Home");
        stepper.findRef();
        ESP_LOGD(TAG, "Fist Position is %3d / %.2f / Micro %d", stepper.getQuanted(), stepper.getScaledPos() * 6, stepper.getSubPos());
        fos::Task::delayMs(2000);

        ESP_LOGD(TAG, "Make one turns with full steps");
        // stepper.setPos(0);
        illum.flashAll(BLUE);
        int i = 0;

        ESP_LOGD(TAG, "Position at begin is %3d / %.2f", stepper.getQuanted(), stepper.getScaledPos() * 6);
        while (i++ < 200) {
            stepper.fullStep(1);
            fos::Task::delayMs(100);
            ESP_LOGD(TAG, "Position is %3d / %.2f", stepper.getQuanted(), stepper.getScaledPos() * 6);
        }
        fos::Task::delayMs(2000);

        int spB = stepper.getSubPos();
        stepper.demoFullTurn();
        int spE = stepper.getSubPos();
        ESP_LOGD(TAG, "Fullsteps SubPos begin %d / end %d", spB, spE);
        fos::Task::delayMs(2000);

        ESP_LOGD(TAG, "Position at begin is %3d / %.2f", stepper.getQuanted(), stepper.getScaledPos() * 6);
        i = 0;
        while (i++ < 64) {
            stepper.step(1);
            fos::Task::delayMs(100);
            ESP_LOGD(TAG, "Position is %3d / %.2f", stepper.getQuanted(), stepper.getScaledPos() * 6);
        }
    }

    ///
    /// Homing
    ///
    ESP_LOGD(TAG, "Find Home");
    stepper.findRef();
    ESP_LOGD(TAG, "Fist Position is %3d / %.2f / Micro %d", stepper.getQuanted(), stepper.getScaledPos() * 6, stepper.getSubPos());
    for (size_t i = 0; i < 7; i++) {
        stepper.fullStep(1);
        fos::Task::delayMs(40);
    }
    illum.flashAll(BLUE);
    fos::Task::delayMs(1000);

    auto ref = stepper.zeroPosition();
    ESP_LOGD(TAG, "Reference saved at %.2f ", ref);
    ESP_LOGD(TAG, "Correct Home");

    ///
    /// Multiple Turns continuous
    ///
    if constexpr (DoTurnSteps) {

        ESP_LOGD(TAG, "Fist Position is %3d / %.2f / Micro %d", stepper.getQuanted(), stepper.getScaledPos() * 6, stepper.getSubPos());
        fos::Task::delayMs(1000);
        ESP_LOGI(TAG, "Move to each index multiple times");
        illum.flashAll(BLUE);
        for (size_t i = 1; i <= 2 * 3; i++) {
            watch.moveTo(3 * i, 0, 0, 1, 1);
            ESP_LOGD(TAG, "Position is %3d / %.2f", stepper.getQuanted(), stepper.getScaledPos() * 6);
        }

        ESP_LOGI(TAG, "Move to each index multiple times");
        illum.flashAll(BLUE);
        for (size_t i = 1; i <= 6 * 3; i++) {
            watch.moveTo(i, 0, 0, 1, 1);
            ESP_LOGD(TAG, "Position is %3d / %.2f", stepper.getQuanted(), stepper.getScaledPos() * 6);
        }
    }

    ///
    /// All other Profiles
    ///
    if constexpr (DoTurnProfiles) {
        for (size_t i = ProfileGenerator::Smooth; i < ProfileGenerator::NumProfiles; i++) {
            // for (size_t i = ProfileGenerator::JumpIn; i < ProfileGenerator::NumProfiles; i++) { // If only part is needed
            ESP_LOGI(TAG, "Running Profile %d ", i);
            ESP_LOGD(TAG, "Fist Position is %3d / %.2f / Micro %d", stepper.getQuanted(), stepper.getScaledPos() * 6, stepper.getSubPos());
            fos::Task::delayMs(1000);
            illum.flashAll(BLUE);
            int steps[3]{4, 2, 0};
            for (size_t j = 0; j < 3; j++) {
                watch.moveTo(steps[j], i, 0, 1, 1);
                ESP_LOGI(TAG, "Last position is %3d / %.2f", stepper.getQuanted(), stepper.getScaledPos() * 6);
                fos::Task::delayMs(1000);
            }
        }

        for (size_t i = ProfileGenerator::Smooth; i < ProfileGenerator::NumProfiles; i++) {
            // for (size_t i = ProfileGenerator::JumpIn; i < ProfileGenerator::NumProfiles; i++) { // If only part is needed
            ESP_LOGI(TAG, "Running Profile %d ", i);
            ESP_LOGD(TAG, "Fist Position is %3d / %.2f / Micro %d", stepper.getQuanted(), stepper.getScaledPos() * 6, stepper.getSubPos());
            fos::Task::delayMs(1000);
            illum.flashAll(BLUE);
            int steps[3]{4, 2, 0};
            for (size_t j = 0; j < 3; j++) {
                watch.moveTo(steps[j], i, 0, 3, 3);
                ESP_LOGI(TAG, "Last position is %3d / %.2f", stepper.getQuanted(), stepper.getScaledPos() * 6);
                fos::Task::delayMs(1000);
            }
        }
    }

    while (true) {
        ESP_LOGI(TAG, "Finish");
        fos::Task::delayMs(60000);
    }
}

void app_main(void) {
    ESP_LOGI(TAG, "Start Movement Integration Tests");

    static fos::Task movementTest(testMovementTask, "Move", fos::Task::DefaultStackSize * 8);

    while (true) {
        fos::Task::delayMs(5000);
        // ESP_LOGI(TAG, "Finish");
    }
}