#include "apphardware.hpp"

#include <can/can_dbg.h>

#include "apphal.hpp"
#include "LED/led.h"
#include "status/motor_manager.h"

using namespace iflytop;

#define TAG "AppHardware"

extern SPI_HandleTypeDef hspi1;

AppHardware::AppHardware()
{
    mutex_ = osMutexNew(NULL);
    if (mutex_ == NULL) {
        // 处理互斥锁创建失败
        ZLOGI(TAG, "mutex_ create failed");
    }
}

void AppHardware::initialize() {
    AppHal::tmc_spi_init();
    // 配置MOTO电源引脚
    MOTO_POWER_EN.initAsOutput(MOTO_POWER_EN_IO, kxs_gpio_nopull, false, true);

    // 初始化 MOTO1 相关引脚
    MOTO1_CSN.initAsOutput(MOTO1_CSN_IO, kxs_gpio_nopull, false, true);
    MOTO1_DRV_ENN.initAsOutput(MOTO1_DRV_ENN_IO, kxs_gpio_nopull, false, true);
    MOTOR1_REF_L.initAsInput(MOTOR1_REF_L_IO, kxs_gpio_nopull, kxs_gpio_no_irq, false);
    MOTOR1_REF_R.initAsInput(MOTOR1_REF_R_IO, kxs_gpio_nopull, kxs_gpio_no_irq, false);

    // 初始化 MOTO2 相关引脚
    MOTO2_CSN.initAsOutput(MOTO2_CSN_IO, kxs_gpio_nopull, false, true);
    MOTO2_DRV_ENN.initAsOutput(MOTO2_DRV_ENN_IO, kxs_gpio_nopull, false, true);
    MOTOR2_REF_L.initAsInput(MOTOR2_REF_L_IO, kxs_gpio_nopull, kxs_gpio_no_irq, false);
    MOTOR2_REF_R.initAsInput(MOTOR2_REF_R_IO, kxs_gpio_nopull, kxs_gpio_no_irq, false);

    // 初始化 MOTO3 相关引脚
    MOTO3_CSN.initAsOutput(MOTO3_CSN_IO, kxs_gpio_nopull, false, true);
    MOTO3_DRV_ENN.initAsOutput(MOTO3_DRV_ENN_IO, kxs_gpio_nopull, false, true);
    MOTOR3_REF_L.initAsInput(MOTOR3_REF_L_IO, kxs_gpio_nopull, kxs_gpio_no_irq, false);
    MOTOR3_REF_R.initAsInput(MOTOR3_REF_R_IO, kxs_gpio_nopull, kxs_gpio_no_irq, false);


    osDelay(1500);
    MOTO_POWER_EN.setState(false);

    TMC51X0Cfg tmc5130cfg1 = {&MOTOR_SPI_INS, MOTO1_CSN_IO, MOTO1_DRV_ENN_IO};
    MOTO1.initialize(0, tmc5130cfg1);

    MOTO1.enable(false);
#if 0
    MOTO1.setIHOLD_IRUN(0, 7, 100);
#else
    MOTO1.setIHOLD_IRUN(5, 10, 100);
#endif
    MOTO1.setScale(1000);
    MOTO1.setEncResolution(-1000);

    // 配置 MOTO2
    TMC51X0Cfg tmc5130cfg2 = {&MOTOR_SPI_INS, MOTO2_CSN_IO, MOTO2_DRV_ENN_IO};
    MOTO2.initialize(1, tmc5130cfg2);

    MOTO2.enable(false);
#if 0
    MOTO2.setIHOLD_IRUN(0, 10, 100);
#else
    MOTO2.setIHOLD_IRUN(5, 10, 100);
#endif
    MOTO2.setScale(1000);
    MOTO2.setEncResolution(-1000);

    // 配置 MOTO3
    TMC51X0Cfg tmc5130cfg3 = {&MOTOR_SPI_INS, MOTO3_CSN_IO, MOTO3_DRV_ENN_IO};
    MOTO3.initialize(2, tmc5130cfg3);

    MOTO3.enable(false);
    MOTO3.setIHOLD_IRUN(5, 10, 100);
    MOTO3.setScale(1000);
    MOTO3.setEncResolution(-1000);

    MOTO1.enable(true);
    osDelay(10);
    MOTO2.enable(true);
    osDelay(10);
    MOTO3.enable(true);
    osDelay(10);

    MOTO1.setGlobalScale(32);
    MOTO2.setGlobalScale(32);
    MOTO3.setGlobalScale(32);

    ZLOGI(TAG, "motor1 initialize TMC51X0: %02X", MOTO1.readICVersion());
    ZLOGI(TAG, "motor2 initialize TMC51X0: %02X", MOTO2.readICVersion());
    ZLOGI(TAG, "motor3 initialize TMC51X0: %02X", MOTO3.readICVersion());

    // GPIO 初始化
    AirValve::initGPIO();
    three_way_valve.initGPIO();
    pump_controller.init();
    high_voltage_pack.init(true);
    laser_control.init(true);
    temp_control_nozzle.init();
    temp_control_slide.init();
    // 初始化电机索引
     MotorManager::ins()->motors[0].setMotorIndex(0);
     MotorManager::ins()->motors[1].setMotorIndex(1);
     MotorManager::ins()->motors[2].setMotorIndex(2);

#if 1
    RK3588_POWER_OFF;       // 3588 下电
#else
    RK3588_POWER_ON;
#endif
    LED_KEY_OFF;         // 按键灯关闭

#if CAN_MODULE_ENABLE
    ZLOGI(TAG, "===== CAN Controller  Start...");
    AppHardware::ins()->can0Controller.start(&hcan1);   // 启动CAN
    ZLOGI(TAG, "===== CAN Controller  Start Suc");
#endif
    ZLOGI(TAG, "===== UART Controller  Start...");
    AppHardware::ins()->uart_control.start();   // 启动CAN
    ZLOGI(TAG, "===== UART Controller  Start Suc");

    isLaunched_ = false;
    isStarted_ = false;
}

bool AppHardware::isHardInitOk() { return hardwareInitedOK; }

void AppHardware::SystemPowerOn() {
    if(isStarted_) {
        return;
    }
    tri_color_light(COLOR_GREEN);  // 开启绿灯
    LIGHT_FLOOD_ON;     // 照明灯开

    osDelay(10);
    ZLOGI(TAG, "======================= System Start =======================");
    isStarted_ = true;
}

void AppHardware::SystemPowerOff(bool is_force) {
    if(!is_force) {
        if(!isStarted_) {
            return;
        }
    }

    tri_color_light(COLOR_OFF);  // 关灯
    LIGHT_FLOOD_OFF;     // 照明灯关闭

    high_voltage_pack.turnOffPower();
    pump_controller.powerOff();
    three_way_valve.setMode(ThreeWayValve::ON_C);
    laser_control.powerOff();

    AirValve::closeCleaning();
    AirValve::closeDehumidification();
    AirValve::closeNozzle();

    MotorManager::ins()->motors[0].runE_Stop();
    osDelay(10);
    MotorManager::ins()->motors[1].runE_Stop();
    osDelay(10);
    MotorManager::ins()->motors[2].runE_Stop();
    osDelay(10);

    ZLOGI(TAG, "======================= System Shut Down =======================");
    isStarted_ = false;
}

void AppHardware::setE_Stop(bool isE_Stop) {
    this->isE_Stop_ = isE_Stop;
}

bool AppHardware::isE_Stop() {
    // return isE_Stop_;
    return false;
}

void AppHardware::toggleLaunched() {
    this->isLaunched_ = !isLaunched_;
}

bool AppHardware::isLaunched() {
    return isLaunched_;
}

bool AppHardware::isStarted() {
    return isStarted_;
}

void AppHardware::setFlowSpeed(uint16_t flowSpeed) {
    osMutexAcquire(mutex_, osWaitForever);
    osMutexRelease(mutex_);
}

void AppHardware::setHumidity(int16_t humidity) {
    osMutexAcquire(mutex_, osWaitForever);
    osMutexRelease(mutex_);
}

void AppHardware::setTemp(int16_t temp) {
    osMutexAcquire(mutex_, osWaitForever);
    osMutexRelease(mutex_);
}

uint16_t AppHardware::getFlowSpeed() {
    osMutexAcquire(mutex_, osWaitForever);
    osMutexRelease(mutex_);
}

int16_t AppHardware::getHumidity() {
    osMutexAcquire(mutex_, osWaitForever);
    osMutexRelease(mutex_);
}

int16_t AppHardware::getTemp() {
    osMutexAcquire(mutex_, osWaitForever);

    osMutexRelease(mutex_);
}