#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()
{
}

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);
    ZLOGI(TAG, "motor1 initialize TMC51X0: %02X", MOTO1.readICVersion());
#if 0
    MOTO1.setIHOLD_IRUN(0, 7, 100);
#else
    MOTO1.setIHOLD_IRUN(0, 5, 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);
    ZLOGI(TAG, "motor2 initialize TMC51X0: %02X", MOTO2.readICVersion());

#if 0
    MOTO2.setIHOLD_IRUN(0, 10, 100);
#else
    MOTO2.setIHOLD_IRUN(0, 5, 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);
    ZLOGI(TAG, "motor3 initialize TMC51X0: %02X", MOTO3.readICVersion());
    MOTO3.setIHOLD_IRUN(0, 5, 100);
    MOTO3.setScale(1000);
    MOTO3.setEncResolution(-1000);

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

    RK3588_POWER_OFF;       // 3588 下电
    LED_KEY_OFF;         // 按键灯关闭
    ZLOGI(TAG, "===== CAN Controller  Start...");
    AppHardware::ins()->can0Controller.start(&hcan1);   // 启动CAN
    ZLOGI(TAG, "===== CAN Controller  Start Suc");
    isLaunched_ = false;
    isStarted_ = false;
}

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

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

    AppHardware::ins()->MOTO1.enable(true);
    osDelay(10);
    AppHardware::ins()->MOTO2.enable(true);
    osDelay(10);
    AppHardware::ins()->MOTO3.enable(true);
    osDelay(10);

    osDelay(200);

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

void AppHardware::SystemPowerOff() {
    if(!isStarted_) {
        return;
    }
    LED_GREEN_ON;           // 关灯
    LIGHT_FLOOD_OFF;     // 照明灯关闭

    AppHardware::ins()->high_voltage_pack.turnOffPower();
    AppHardware::ins()->pump_controller.powerOff();
    AppHardware::ins()->three_way_valve.setMode(ThreeWayValve::ON_C);
    AppHardware::ins()->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);

    AppHardware::ins()->MOTO1.enable(false);
    AppHardware::ins()->MOTO2.enable(false);
    AppHardware::ins()->MOTO3.enable(false);
    osDelay(200);
    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_;
}

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

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

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