#include "hardware.hpp"

#include <stddef.h>
#include <stdio.h>

#include "main.h"
#include "project.hpp"
//
// #include "sdk/components/single_axis_motor_control_v2/single_axis_motor_control_v2.hpp"
#include "sdk/components/iflytop_can_slave_modules/idcard_reader_service.hpp"
#include "sdk/components/single_axis_motor_control/single_axis_motor_control.hpp"
#include "sdk/hal/zhal.hpp"
#include "sdk\components\iflytop_can_slave_modules\io_control_service.hpp"
#include "sdk\components\iflytop_can_slave_v1\iflytop_can_slave.hpp"
#include "sdk\components\m3078\m3078_code_scaner.hpp"
#include "sdk\components\tmc\ic\ztmc4361A.hpp"
#include "sdk\components\tmc\ic\ztmc5130.hpp"
//
#include "sdk\components\huacheng_sensor\dp600_pressure_sensor.hpp"
#include "sdk\components\zcan_module\huacheng_pressure_sensor.hpp"
#include "sdk\components\zcan_module\zcan_basic_order_module.hpp"
#include "sdk\components\zcan_module\zcan_pump_ctrl_module.hpp"
#include "sdk\components\zcan_module\zcan_trigle_warning_light_ctl_module.hpp"

using namespace iflytop;
#define TAG "main"

TMC5130 m_motor1;
TMC5130 m_motor2;

ZGPIO triLight_R;
ZGPIO triLight_G;
ZGPIO triLight_Y;
ZGPIO triLight_BEEP;

ZCanBasicOrderModule            m_basicOrderModule;
ZCanPumpCtrlModule              m_pumpCtrlModule;
ZCanTrigleWarningLightCtlModule m_warningLightCtlModule;
HuachengPressureSensor          m_huachengPressureSensor;

DP600PressureSensor m_dp600PressureSensor2;
DP600PressureSensor m_dp600PressureSensor3;
DP600PressureSensor m_dp600PressureSensor4;

ZGPIO IO_PD13_IN;
ZGPIO IO_PC7_IN;

#define PROCESS_CMD(cmd, id)                                                                           \
  if ((cmdheader->cmdid == (uint16_t)cmd) && (cmdheader->subcmdid == 0) && cmdheader->data[0] == id) { \
    matching = true;                                                                                   \
  }                                                                                                    \
  if ((cmdheader->cmdid == (uint16_t)cmd) && (cmdheader->subcmdid == 0) && cmdheader->data[0] == id)

void setmotor(TMC5130 *motor, int16_t acc_rpm2, int16_t rpm, int16_t idlepower, int16_t power) {
  int32_t ppm = rpm / 60.0 * 51200;
  int32_t acc = acc_rpm2 / 60.0 * 51200;

  int16_t _idlepower = 1;
  int16_t _power     = 31;

  if (idlepower > 0 && idlepower < 31) {
    _idlepower = idlepower;
  }
  if (power > 0 && power < 31) {
    _power = power;
  }

  motor->setIHOLD_IRUN(_idlepower, _power, 10);  // 5W
  motor->setAcceleration(acc);
  motor->setDeceleration(acc);
  motor->rotate(ppm);
}

void Hardware::initialize() {
  IO_PD13_IN.initAsInput(PD13, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, false /*mirror*/);
  IO_PC7_IN.initAsInput(PC7, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, false /*mirror*/);

  {
    TMC5130::cfg_t cfg = {.hspi = &MOTOR_SPI, .enn_pin = MOTOR1_ENN, .csn_pin = MOTOR1_CSN};

    m_motor1.initialize(&cfg);
    int32_t chipv = m_motor1.readChipVERSION();
    ZLOGI(TAG, "m_motor1:%lx", chipv);
    m_motor1.setIHOLD_IRUN(1, 20, 0);
    m_motor1.setMotorShaft(true);

    m_motor1.setAcceleration(300000);
    m_motor1.setDeceleration(300000);
    // m_motor1.rotate(1000000);
  }

  {
    TMC5130::cfg_t cfg = {.hspi = &MOTOR_SPI, .enn_pin = MOTOR2_ENN, .csn_pin = MOTOR2_CSN};

    m_motor2.initialize(&cfg);
    int32_t chipv = m_motor2.readChipVERSION();
    ZLOGI(TAG, "m_motor2:%lx", chipv);
    m_motor2.setIHOLD_IRUN(1, 20, 0);  // 5W
    m_motor2.setMotorShaft(true);

    m_motor2.setAcceleration(300000);
    m_motor2.setDeceleration(300000);
    // m_motor1.rotate(1000000);
  }

  triLight_R.initAsOutput(PD8, ZGPIO::kMode_nopull, false, false);
  triLight_G.initAsOutput(PD7, ZGPIO::kMode_nopull, false, false);
  triLight_Y.initAsOutput(PD9, ZGPIO::kMode_nopull, false, false);
  triLight_BEEP.initAsOutput(PD10, ZGPIO::kMode_nopull, false, false);

  m_dp600PressureSensor2.initialize(&huart3, 2);
  m_dp600PressureSensor3.initialize(&huart3, 3);
  m_dp600PressureSensor4.initialize(&huart3, 4);
}

void dumpdp600data(DP600PressureSensor::sensor_data_t *data) {
  ZLOGI(TAG, "value:%d", data->value);
  ZLOGI(TAG, "zero_point:%d", data->zero_point);
  ZLOGI(TAG, "range_full_point:%d", data->range_full_point);
  ZLOGI(TAG, "precision:%d", data->precision);
  ZLOGI(TAG, "pressure_unit:%d", data->pressure_unit);
}

void packet_kcmd_read_huacheng_pressure_sensor_data(int id, DP600PressureSensor::sensor_data_t *dp600data, uint8_t *receipt, int32_t &receiptsize) {
  receipt[0] = id;
  receipt[1] = 0;
  receipt[2] = dp600data->precision;
  receipt[3] = dp600data->pressure_unit;
  memcpy(receipt + 4, &dp600data->value, 2);
  memcpy(receipt + 6, &dp600data->zero_point, 2);
  memcpy(receipt + 8, &dp600data->range_full_point, 2);
  receiptsize = 10;
}

int32_t Hardware::process_rx_packet(from_where_t fromwhere, uint8_t *packet, int32_t len, uint8_t *receipt, int32_t &receiptsize, bool &matching) {
  Cmdheader_t *cmdheader = (Cmdheader_t *)packet;

  /**
   * @brief 获取蒸发仓水浸状态
   */
  PROCESS_CMD(kcmd_proportional_read_water_immersion_sensor, 0) {
    ((int32_t *)receipt)[0] = !IO_PC7_IN.getState();
    receiptsize             = 4;
    return 0;
  }

  /**
   * @brief 获取设备底盘水浸传感器
   */
  PROCESS_CMD(kcmd_proportional_read_water_immersion_sensor, 1) {
    ((int32_t *)receipt)[0] = !IO_PD13_IN.getState();
    receiptsize             = 4;
    return 0;
  }

  /**
   * @brief 控制加液泵
   */
  PROCESS_CMD(kcmd_peristaltic_pump_ctl, 1) {
    int16_t acc       = *(int16_t *)(&cmdheader->data[2]);
    int16_t rpm       = *(int16_t *)(&cmdheader->data[4]);
    int16_t idlepower = cmdheader->data[6];
    int16_t power     = cmdheader->data[7];

    setmotor(&m_motor1, acc, rpm, idlepower, power);
    receipt[0]  = cmdheader->data[0];
    receiptsize = 1;
    return 0;
  }

  /**
   * @brief 控制-喷液泵
   */
  PROCESS_CMD(kcmd_peristaltic_pump_ctl, 2) {
    int16_t acc       = *(int16_t *)(&cmdheader->data[2]);
    int16_t rpm       = *(int16_t *)(&cmdheader->data[4]);
    int16_t idlepower = cmdheader->data[6];
    int16_t power     = cmdheader->data[7];

    setmotor(&m_motor2, acc, rpm, idlepower, power);
    receipt[0]  = cmdheader->data[0];
    receiptsize = 1;
    return 0;
  }

  /**
   * @brief 三色指示灯控制
   */
  PROCESS_CMD(kcmd_triple_warning_light_ctl, 1) {
    /**
     * @brief 0:设置状态
     *  cmd:
     *     [0]:SENSORID
     *     [2]:红色灯状态
     *     [3]:黄色灯状态
     *     [4]:绿色灯状态
     *     [5]:蜂鸣器状态
     *  ack         : b0:id
     *  ack_datalen : 1
     */
    uint8_t id   = cmdheader->data[0];
    uint8_t r    = cmdheader->data[2];
    uint8_t g    = cmdheader->data[3];
    uint8_t b    = cmdheader->data[4];
    uint8_t beep = cmdheader->data[5];

    triLight_R.setState(r != 0);
    triLight_G.setState(g != 0);
    triLight_Y.setState(b != 0);
    triLight_BEEP.setState(beep != 0);

    receipt[0]  = cmdheader->data[0];
    receiptsize = 1;
  }

  static DP600PressureSensor::sensor_data_t dp600data;

  /**
   * @brief 液位测量压力传感器
   */
  PROCESS_CMD(kcmd_read_huacheng_pressure_sensor, 1) {
    static ModbusBlockHost modbusBlockHost;
    modbusBlockHost.initialize(&huart3);
    int16_t val[1] = {0};
    bool    suc    = modbusBlockHost.readReg03Muti(1, 0x00, (uint16_t *)val, 1, 50);
    if (!suc) return 1002;
    dp600data.precision        = 3;
    dp600data.pressure_unit    = 1;
    dp600data.value            = val[0];
    dp600data.zero_point       = 0;
    dp600data.range_full_point = 0;
    packet_kcmd_read_huacheng_pressure_sensor_data(cmdheader->data[0], &dp600data, receipt, receiptsize);
    if (fromwhere == kuart) {
      dumpdp600data(&dp600data);
    }
    return 0;
  }
  /**
   * @brief 空压机压力传感器
   */
  PROCESS_CMD(kcmd_read_huacheng_pressure_sensor, 2) {
    bool suc = m_dp600PressureSensor2.readVal(&dp600data);
    if (!suc) return 1002;
    packet_kcmd_read_huacheng_pressure_sensor_data(cmdheader->data[0], &dp600data, receipt, receiptsize);
    if (fromwhere == kuart) {
      dumpdp600data(&dp600data);
    }
    return 0;
  }
  /**
   * @brief 加液蠕动泵压力传感器
   */
  PROCESS_CMD(kcmd_read_huacheng_pressure_sensor, 3) {
    bool suc = m_dp600PressureSensor3.readVal(&dp600data);
    if (!suc) return 1002;
    packet_kcmd_read_huacheng_pressure_sensor_data(cmdheader->data[0], &dp600data, receipt, receiptsize);
    if (fromwhere == kuart) {
      dumpdp600data(&dp600data);
    }
    return 0;
  }
  /**
   * @brief 喷射蠕动泵压力传感器
   */
  PROCESS_CMD(kcmd_read_huacheng_pressure_sensor, 4) {
    bool suc = m_dp600PressureSensor4.readVal(&dp600data);
    if (!suc) return 1002;
    packet_kcmd_read_huacheng_pressure_sensor_data(cmdheader->data[0], &dp600data, receipt, receiptsize);
    if (fromwhere == kuart) {
      dumpdp600data(&dp600data);
    }
    return 0;
  }
}
void Hardware::loop() {}