#include <ti_msp_dl_config.h>
#include <FreeRTOS.h>
#include <task.h>

#include <Print.h>
#include <Encoder.h>
#include <Imu.h>
#include <Motor.h>
#include <TrackerMpu.h>
#include <TrackerGpio.h>
#include <2025_include_user/Beep.h>
#include <Host.h>
#include <Key.h>
#include <Led.h>
#include <Servo.h>
#include <Stepmotor.h>
#include <Oled.hpp>
#include <OledMenuDriver.h>
#include <OledMenuPaint.h>
#include <TrackerGpio.h>
#include <TrackerGpio.h>


/************ 外部中断测试部分 ************/

extern "C" void GROUP1_IRQHandler() {
    //读取Group1的中断寄存器并清除中断标志位
    switch (DL_Interrupt_getPendingGroup(DL_INTERRUPT_GROUP_1)) {
        //检查是否是KEY的GPIOB端口中断，注意是INT_IIDX，不是PIN_22_IIDX
        case Key_GPIOA_INT_IIDX:
        case GPIO_MULTIPLE_GPIOB_INT_IIDX: {
            const uint32_t statusA = DL_GPIO_getPendingInterrupt(GPIOA);
            const uint32_t statusB = DL_GPIO_getPendingInterrupt(GPIOB);
            // print("%d\n", statusA);
            // print("%d\n", statusB);
            if (statusA == KEY1_PIN_ID + 1) {
                Key::keys[KEY1].state = 1;
            } else if (statusB == KEY2_PIN_ID + 1) {
                Key::keys[KEY2].state = 1;
            } else if (statusB == KEY3_PIN_ID + 1) {
                Key::keys[KEY3].state = 1;
            } else if (statusB == KEY4_PIN_ID + 1) {
                Key::keys[KEY4].state = 1;
            }
            // TrackerGpio::inject(Tracker_Track1_PIN, DL_GPIO_readPins(Tracker_Track1_PORT, Tracker_Track1_PIN) > 0);
            // TrackerGpio::inject(Tracker_Track2_PIN, DL_GPIO_readPins(Tracker_Track2_PORT, Tracker_Track2_PIN) > 0);
            // TrackerGpio::inject(Tracker_Track3_PIN, DL_GPIO_readPins(Tracker_Track3_PORT, Tracker_Track3_PIN) > 0);
            // TrackerGpio::inject(Tracker_Track4_PIN, DL_GPIO_readPins(Tracker_Track4_PORT, Tracker_Track4_PIN) > 0);
            // TrackerGpio::inject(Tracker_Track5_PIN, DL_GPIO_readPins(Tracker_Track5_PORT, Tracker_Track5_PIN) > 0);
            // TrackerGpio::inject(Tracker_Track6_PIN, DL_GPIO_readPins(Tracker_Track6_PORT, Tracker_Track6_PIN) > 0);
            // TrackerGpio::inject(Tracker_Track7_PIN, DL_GPIO_readPins(Tracker_Track7_PORT, Tracker_Track7_PIN) > 0);
            DL_GPIO_clearInterruptStatus(GPIOA, statusA);
            DL_GPIO_clearInterruptStatus(GPIOB, statusB);
            break;
        }
        default:
            break;
    }
}

/************ FreeRtos 任务定义区 ************/

[[noreturn]] void vLedTask(void *pvParameters) {
    while (true) {
        Led::on(LED0);
        // DL_GPIO_setPins(Lights_PORT, Lights_Light_PIN);
        vTaskDelay(pdMS_TO_TICKS(500)); // 每500毫秒切换一次LED状态
        Led::off(LED0);
        // DL_GPIO_clearPins(Lights_PORT, Lights_Light_PIN);
        vTaskDelay(pdMS_TO_TICKS(500)); // 每500毫秒切换一次LE+D状态
    }
}

[[noreturn]] void vOledTask(void *pvParameters) {
    MenuDriver::init();
    MenuPaint::Init();
    while (true) {
        const int direction = (int) Host::get("Direction", 0);
        if (Key::getState(KEY1 || direction == 2)) {
            MenuPaint::ScrollUp();
        }
        if (Key::getState(KEY2) || direction == 8) {
            MenuPaint::ScrollDown();
        }
        if (Key::getState(KEY3) || direction == 4) {
            MenuPaint::Enter();
        }
        if (Key::getState(KEY4) || direction == 6) {
            MenuPaint::Exit();
        }

        MenuPaint::Paint();
        vTaskDelay(pdMS_TO_TICKS(10));
    }
}

[[noreturn]] void vTestTask(void *pvParameters) {
    while (true) {
        // print("Ctrl: %f\n", Host::get("Ctrl", 0.0));
        // print("x: %f\n", Host::get("x", 0.0));
        // print("y: %f\n", Host::get("y", 0.0));
        // Motor::setSpeed(3000, 3000);
        // TrackerGpio::Data data = TrackerGpio::getData(); // 每位数据：0 表示压线，1 表示空白
        // const char *d = data.data;
        //
        // print("Data:%d,%d,%d,%d,%d,%d,%d\n", d[0], d[1], d[2], d[3], d[4], d[5], d[6]);
        // vTaskDelay(pdMS_TO_TICKS(100));
    }
}

static uint8_t turn_limit = 0;
[[noreturn]] void vTrackerTask(void *pvParameters) {
    // 计算偏移量（简单加权）
    // 左侧为正，右侧为负
    constexpr float weights[7] = {-2.1, -1.5, -0.8, 0, 0.8, 1.5, 2.1};
    constexpr int base_speed = 4000; // 基础速度
    constexpr int adjust_speed = 2000; // 差速调整速度

    while (true) {
        if (turn_limit <= 0) {
            for (uint8_t i = 0; i < 5; i++) {
                if (MenuPaint::GetSwitchState(i) == 1) {
                    turn_limit = 4 * (i + 1) + 1;
                }
            }

            Motor::stop();
            vTaskDelay(pdMS_TO_TICKS(50));
            continue;
        }

        // 读取循迹传感器数据
        TrackerGpio::Data data = TrackerGpio::getData(); // 每位数据：0 表示压线，1 表示空白
        const char *d = data.data;

        float error = 0;
        int sum = 0;

        for (int i = 0; i < 7; ++i) {
            if (d[i] == 0) {
                // 压到线
                error += weights[i];
                sum++;
            }
        }

        if (sum == 0) {
            turn_limit--;
            if (turn_limit <= 0) {
                Motor::stop();
                vTaskDelay(pdMS_TO_TICKS(50));
                continue;
            }

            uint8_t is_speed = (MenuPaint::GetSwitchState(5) == 1) ? 1 : 0;
            uint8_t speed = is_speed ? 210 : 160;
            Motor::setLeftSpeed(0);
            Motor::setRightSpeed(speed * 10);
            while (true) {
                data = TrackerGpio::getData(); // 每位数据：0 表示压线，1 表示空白
                const char *inner_d = data.data;
                if (inner_d[3] == 0 || inner_d[2] == 0 || inner_d[1] == 0 || inner_d[0] == 0)
                    break;
                Motor::setLeftSpeed(0);
                Motor::setRightSpeed(speed * 10);
                vTaskDelay(pdMS_TO_TICKS(50));
            }
        } else {
            int left_speed = base_speed;
            int right_speed = base_speed;

            error = error / sum;
            // 差速控制，误差越大，左右轮速度差越大
            left_speed = (int) (base_speed - error * adjust_speed / 4);
            right_speed = (int) (base_speed + error * adjust_speed / 4);

            // 限制最大最小速度
            if (left_speed > 10000) left_speed = 10000;
            if (left_speed < -10000) left_speed = -10000;
            if (right_speed > 10000) right_speed = 10000;
            if (right_speed < -10000) right_speed = -10000;

            // 设置电机速度
            Motor::setLeftSpeed(left_speed);
            Motor::setRightSpeed(right_speed);

            vTaskDelay(pdMS_TO_TICKS(50));
        }
    }
}

[[noreturn]] void vStepMotorTask(void *pvParameters) {
    // // PID参数（根据实际调试再微调）
    //
    // const float integral_limit = 500.0f; // 积分限幅，避免积分快速累积
    // const float DEAD_ZONE = 10.0f; // 误差死区：误差在±5像素内不动作
    //
    // float integral = 0.0f;
    // float last_error = 0.0f;
    vTaskDelay(pdMS_TO_TICKS(500));

    while (true) {
        // DL_TimerG_setCaptureCompareValue(Servo_INST, (uint32_t)800, GPIO_Servo_C0_IDX);
        Servo::SetUpAngle(Host::getWithoutClean("angle", 87.0));
        // Servo::SetDownAngle(120);
        // // if (MenuPaint::GetSwitchState(5) == 0 || MenuPaint::GetSwitchState(6) == 0) {
        // //     vTaskDelay(pdMS_TO_TICKS(100));
        // //     continue;
        // // }
        //
        // const double Kp = Host::getWithoutClean("kp", 0.2);
        // const double Ki = Host::getWithoutClean("ki", 0.0005);
        // const double Kd = Host::getWithoutClean("kd", 0.006);
        // const double t_ = Host::posX; // 获取像素误差，范围0~240
        // const uint8_t direction = (t_ > 0) ? 1 : 0;
        // const double error = t_ > 0 ? t_ : -t_;
        //
        //
        // print("Kp: %f, Ki: %f, Kd: %f\n", Kp, Ki, Kd);
        //
        // // 死区处理
        // if (fabs(error) < DEAD_ZONE) {
        //     StepMotor::setPosition(0, 32, 0.0f, 0); // 停止动作
        //     integral = 0; // 死区内积分归零，避免积累
        //     // while (true) {
        //     //     // TODO 激光ON
        //     //     vTaskDelay(pdMS_TO_TICKS(1000));
        //     //     // TODO 激光OFF
        //     // }
        // } else {
        //     // 积分项累加 & 限幅
        //     integral += error;
        //     if (integral > integral_limit) integral = integral_limit;
        //     if (integral < -integral_limit) integral = -integral_limit;
        //
        //     // 微分项
        //     float derivative = error - last_error;
        //
        //     // PID输出
        //     float output = Kp * error + Ki * integral + Kd * derivative;
        //
        //     // 限制输出范围（0.3° ~ 2.0°）
        //     if (output < 0.3f) output = 0.3f;
        //     if (output > 4.0f) output = 4.0f;
        //
        //     // 控制步进电机
        //     StepMotor::setPosition(direction, 32, output, -10);
        //     // print("Motor: speed=%f, direction=%d\n", output, direction);
        //
        //     last_error = error; // 保存上次误差
        // }

        vTaskDelay(pdMS_TO_TICKS(500));
    }
}

/***************** 主函数 ******************/

int main() {
    /*** SysConfig 初始化 ***/
    SYSCFG_DL_init();

    /*** 模块初始化 ***/
    Print_init();
    Encoder::init();
    // Imu::init();
    Motor::init();
    // TrackerMpu::init();
    // Beep::init();
    // Led::init();
    Host::init();
    Key::init();
    // StepMotor::init();

    /*** FreeRtos 任务初始化 ***/
    xTaskCreate(vLedTask, "TestLedTask", 32, nullptr, 1, nullptr);
    xTaskCreate(vOledTask, "vOledTask", 128, nullptr, 1, nullptr);
    // xTaskCreate(vTestTask, "vTestTask", 512, nullptr, 1, nullptr);
    xTaskCreate(vTrackerTask, "vTrackerTask", 256, nullptr, 1, nullptr);
    xTaskCreate(vStepMotorTask, "vStepMotorTask", 32, nullptr, 1, nullptr);


    /*** 启动 FreeRtos ***/
    vTaskStartScheduler();

    return 0;
}

/************ FreeRtos 错误处理部分 ************/

#ifdef __cplusplus
extern "C" {
#endif

void vApplicationStackOverflowHook(TaskHandle_t xTask, char *pcTaskName) {
    /* 栈溢出时的处理代码 */
    /* 例如，打印错误信息并重置系统 */

    /* 参数:
     * xTask - 发生栈溢出的任务句柄
     * pcTaskName - 发生栈溢出的任务名称
     */

    /* 通常在这里添加一些错误处理和系统恢复的代码 */
    while (1) {
        /* 可以在此处添加错误处理代码，如闪烁LED或输出调试信息 */
    }
}

/* 在全局范围内定义存储空间 */
static StaticTask_t xIdleTaskTCB;
static StackType_t uxIdleTaskStack[configMINIMAL_STACK_SIZE];

/* 实现所需的函数 */
void vApplicationGetIdleTaskMemory(StaticTask_t **ppxIdleTaskTCBBuffer,
                                   StackType_t **ppxIdleTaskStackBuffer,
                                   uint32_t *pulIdleTaskStackSize) {
    /* 传递静态分配的空闲任务数据结构的地址 */
    *ppxIdleTaskTCBBuffer = &xIdleTaskTCB;

    /* 传递静态分配的空闲任务堆栈的地址 */
    *ppxIdleTaskStackBuffer = uxIdleTaskStack;

    /* 传递堆栈大小 */
    *pulIdleTaskStackSize = configMINIMAL_STACK_SIZE;
}


/* 为定时器任务定义静态内存 */
static StaticTask_t xTimerTaskTCB;
static StackType_t uxTimerTaskStack[configTIMER_TASK_STACK_DEPTH];

void vApplicationGetTimerTaskMemory(StaticTask_t **ppxTimerTaskTCBBuffer,
                                    StackType_t **ppxTimerTaskStackBuffer,
                                    uint32_t *pulTimerTaskStackSize) {
    /* 传递静态分配的定时器任务数据结构的地址 */
    *ppxTimerTaskTCBBuffer = &xTimerTaskTCB;

    /* 传递静态分配的定时器任务堆栈的地址 */
    *ppxTimerTaskStackBuffer = uxTimerTaskStack;

    /* 传递堆栈大小 */
    *pulTimerTaskStackSize = configTIMER_TASK_STACK_DEPTH;
}

#ifdef __cplusplus
}
#endif


// [[noreturn]] void vStepMotorTask(void *pvParameters) {
//     while (true) {
//         double speed = Host::posX;
//         uint8_t direction = 1;
//
//         if (Host::x_count <= 0) {
//             speed = 0.0;
//         } else {
//             Host::x_count--;
//             if (Host::x_count == 0) {
//                 Host::posX = 0.0;  // 用完后置零
//             }
//         }
//
//         direction = speed >= 0 ? 1 : 0;
//         speed = speed >= 0 ? speed : -speed;
//         StepMotor::setSpeed(direction, 32, float(speed));
//         print("Motor: speed=%f, direction=%d\n", speed, direction);
//
//         vTaskDelay(pdMS_TO_TICKS(70));
//     }
// }

// [[noreturn]] void vStepMotorTask(void *pvParameters) {
//     while (true) {
//         int error = (int) Host::posX;
//         uint8_t direction = error > 0;
//         if (error > 5 || error < -5) {
//             StepMotor::setPosition(direction, 32, 0.5, 10);
//         }
//
//         vTaskDelay(pdMS_TO_TICKS(93));
//     }
// }
// print("FunA: %d\n", MenuPaint::GetSwitchState(0));
// print("FunB: %d\n", MenuPaint::GetSwitchState(1));
// if (MenuPaint::GetSwitchState(0) == 1) {
//     Beep::start();
// } else {
//     Beep::stop();
// }

// [[noreturn]] void vTrackerTask(void *pvParameters) {
//     while (true) {
//         print("Tracker!\n");
//         const TrackerGpio::Data data = TrackerGpio::getData();
//         print("Tracker: %d, %d, %d, %d, %d, %d, %d\n", data.data[0], data.data[1], data.data[2], data.data[3], data.data[4], data.data[5], data.data[6]);
//         vTaskDelay(pdMS_TO_TICKS(1000));
//     }
// }