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

#include "sdk/os/zos.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"

#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;

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);

  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);
    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);
    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;
  });

  uint16_t maincmdid = (((uint32_t)kcmd_xy_robot_ctrl_enable) >> 8) & 0xFFFF;
  uint8_t  subcmdId  = (((uint32_t)kcmd_xy_robot_ctrl_enable)) & 0xFF;

  printf("maincmdid:%d subcmdId:%d\n", maincmdid, subcmdId);

  /*******************************************************************************
   *                            zcanXYRobotCtrlModule                            *
   *******************************************************************************/
  xyRobotCtrlModule.initialize(&motora, &motorb, NULL, NULL, 1.0f);
  zcanXYRobotCtrlModule.initialize(&zcanCmder, 1, &xyRobotCtrlModule);

  while (true) {
    OSDefaultSchduler::getInstance()->loop();
    zcanCmder.loop();
    // zcanCmder.sendPacket(data, 4);
    // osDelay(100);
  }
}