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

#include "sdk/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp"
#include "sdk/components/zcancmder_module/zcan_step_motor_ctrl_module.hpp"
#include "sdk/os/zos.hpp"
#include "sdk\components\flash\zsimple_flash.hpp"
#include "sdk\components\mini_servo_motor\feite_servo_motor.hpp"
#include "sdk\components\tmc\ic\ztmc4361A.hpp"
#include "sdk\components\xy_robot_ctrl_module\xy_robot_ctrl_module.hpp"
#include "sdk\components\zcancmder\zcanreceiver.hpp"
#include "sdk\components\zcancmder_module\zcan_basic_order_module.hpp"
#include "sdk\components\zcancmder_module\zcan_xy_robot_module.hpp"
//
#include "sdk\components\flash\znvs.hpp"
//

#define TAG "main"
using namespace iflytop;
using namespace std;

extern void umain();
extern "C" {
void StartDefaultTask(void const* argument) { umain(); }
}
#define TMC_MOTOR_SPI hspi1

static TMC4361A                motora;
static TMC4361A                motorb;
static ZCanCmder               zcanCmder;
static ZCanBasicOrderModule    zcanBasicOrderModule;
static ZCANXYRobotCtrlModule   zcanXYRobotCtrlModule;
static XYRobotCtrlModule       xyRobotCtrlModule;
static StepMotorCtrlModule     stepMotorCtrlModule;
static ZCanStepMotorCtrlModule zcanStepMotorCtrlModule;

void umain() {
  chip_cfg_t chipcfg;
  chipcfg.us_dleay_tim          = &DELAY_US_TIMER;
  chipcfg.tim_irq_scheduler_tim = &TIM_IRQ_SCHEDULER_TIMER;
  chipcfg.huart                 = &DEBUG_UART;
  chipcfg.debuglight            = DEBUG_LIGHT_GPIO;

  chip_init(&chipcfg);

  zos_cfg_t zoscfg;
  zos_init(&zoscfg);

  ZNVS::ins().initialize();
  // ZNVS::ins().set_config_int32("test", 1234);
  ZNVS::ins().dumpcfg();

  osDelay(1000);

  {
    TMC4361A::cfg_t cfg = {
        .spi               = &TMC_MOTOR_SPI,             //
        .csgpio            = TMC_MOTOR1_SPI_SELECT1_IO,  //
        .resetPin          = TMC_MOTOR1_nRESET_IO,       //
        .fREEZEPin         = TMC_MOTOR1_nFREEZE_IO,      //
        .ennPin            = PinNull,                    //
        .driverIC_ennPin   = TMC_MOTOR1_SUB_IC_ENN_IO,   //
        .driverIC_resetPin = PinNull,                    //
    };
    motora.initialize(&cfg);
    motora.setMotorShaft(false);
    ZLOGI(TAG, "motora initialize TMC4361A:%x DriverIC:%x", motora.readICVersion(), motora.readSubICVersion());
  }

  {
    TMC4361A::cfg_t cfg = {
        .spi               = &TMC_MOTOR_SPI,             //
        .csgpio            = TMC_MOTOR2_SPI_SELECT1_IO,  //
        .resetPin          = TMC_MOTOR2_nRESET_IO,       //
        .fREEZEPin         = TMC_MOTOR2_nFREEZE_IO,      //
        .ennPin            = PinNull,                    //
        .driverIC_ennPin   = TMC_MOTOR2_SUB_IC_ENN_IO,   //
        .driverIC_resetPin = PinNull,                    //
    };
    motorb.initialize(&cfg);
    motorb.setMotorShaft(false);
    ZLOGI(TAG, "motorb initialize TMC4361A:%x DriverIC:%x", motorb.readICVersion(), motorb.readSubICVersion());
  }

  motora.setAcceleration(300000);
  motora.setDeceleration(300000);
  motora.setIHOLD_IRUN(0, 2, 10);

  motorb.setAcceleration(300000);
  motorb.setDeceleration(300000);
  motorb.setIHOLD_IRUN(0, 2, 10);

  motora.rotate(0);
  motorb.rotate(0);
  auto zcanCmder_cfg = zcanCmder.createCFG(DEVICE_ID);
  zcanCmder.init(zcanCmder_cfg);

  /*******************************************************************************
   *                            zcanBasicOrderModule                             *
   *******************************************************************************/
  zcanBasicOrderModule.initialize(&zcanCmder);
  zcanBasicOrderModule.reg_set_io(1, [](bool val) { ZLOGI(TAG, "write io 1:%d", val); });
  zcanBasicOrderModule.reg_read_io(1, []() {
    ZLOGI(TAG, "read io 1");
    return 1;
  });
  zcanBasicOrderModule.reg_read_adc(1, []() {
    ZLOGI(TAG, "read adc 1");
    return 123;
  });

  /*******************************************************************************
   *                            zcanXYRobotCtrlModule                            *
   *******************************************************************************/
  ZGPIO input[8];
  input[0].initAsInput(ARM_SENSOR1_GPIO, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, false);
  input[1].initAsInput(ARM_SENSOR2_GPIO, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, false);
  input[2].initAsInput(ARM_SENSOR3_GPIO, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, false);
  input[3].initAsInput(ARM_SENSOR4_GPIO, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, false);
  input[4].initAsInput(ARM_SENSOR5_GPIO, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, false);
  input[5].initAsInput(ARM_SENSOR6_GPIO, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, false);
  input[6].initAsInput(ARM_SENSOR7_GPIO, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, false);
  input[7].initAsInput(ARM_SENSOR8_GPIO, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, false);

#if 0
  {
    xyRobotCtrlModule.initialize(&motora, &motorb, &input[6], &input[7], 1.0f);
    XYRobotCtrlModule::run_param_t param;
    xyRobotCtrlModule.read_run_param(param);
    param.robot_type     = XYRobotCtrlModule::corexy;
    param.distance_scale = 1000;  // = 1.0
    param.x_shaft        = false;
    param.y_shaft        = false;
    param.ihold          = 1;
    param.irun           = 3;
    param.maxspeed       = 1000000;
    param.acc            = 3000000;
    param.dec            = 3000000;
    xyRobotCtrlModule.set_run_param(param);

    XYRobotCtrlModule::run_to_zero_param_t run_to_zero_param;
    xyRobotCtrlModule.read_run_to_zero_param(run_to_zero_param);
    run_to_zero_param.move_to_zero_max_d    = 5120000;
    run_to_zero_param.leave_from_zero_max_d = 5120000;
    run_to_zero_param.speed                 = 30000;
    run_to_zero_param.dec                   = 600000;
    xyRobotCtrlModule.set_run_to_zero_param(run_to_zero_param);

    zcanXYRobotCtrlModule.initialize(&zcanCmder, 1, &xyRobotCtrlModule);
  }
  {
    stepMotorCtrlModule.initialize(1, &motora, &input[6], NULL);
    zcanStepMotorCtrlModule.initialize(&zcanCmder, 1, &stepMotorCtrlModule);
  }
#endif
  extern DMA_HandleTypeDef hdma_usart2_rx;
  extern DMA_HandleTypeDef hdma_usart2_tx;

  FeiTeServoMotor servo;
  servo.initialize(&huart2, &hdma_usart2_rx, &hdma_usart2_tx);
  OSDefaultSchduler::getInstance()->regPeriodJob(
      [&](OSDefaultSchduler::Context& context) {  //
        // FeiTeServoMotor::status_t status = {0};
        // servo.read_status(1, &status);
        // servo.dump_status(&status);

        // FeiTeServoMotor::detailed_status_t detailed_status = {0};
        // servo.read_detailed_status(1, &detailed_status);
        // servo.dump_detailed_status(&detailed_status);

      },
      1000);
  //
  servo.ping(1);
  // servo.reCalibration(1, 1000);
  // servo.moveTo(1, 4000, 0, 0);
  // servo.moveWithTorque(1, -1000);
  int16_t poscalibration = 0;
  // servo.getServoCalibration(1, poscalibration);
  // ZLOGI(TAG, "poscalibration:%d", poscalibration);

  while (true) {
    OSDefaultSchduler::getInstance()->loop();
    zcanCmder.loop();

// zcanCmder.sendPacket(data, 4);
#if 0
    static uint8_t  rxbuf[1024];
    static uint16_t rxlen;
    rxlen = 0;
    HAL_UARTEx_ReceiveToIdle(&huart2, rxbuf, 1000, &rxlen, 1000);
    if (rxlen > 0) {
      for (size_t i = 0; i < rxlen; i++) {
        printf("0x%02x ", rxbuf[i]);
      }
      printf("\n");
    }
#endif

#if 0
    osDelay(100);
    ZLOGI(TAG, "input:%d %d %d %d %d %d %d %d",  //
          input[0].getState(), input[1].getState(), input[2].getState(), input[3].getState(), input[4].getState(), input[5].getState(), input[6].getState(), input[7].getState());
#endif
  }
}