#include "main.hpp"

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

#include "main.h"
#include "project.hpp"
//
#include "sdk/hal/zhal.hpp"
#include "sdk\components\tmc\ic\ztmc4361A.hpp"

#define TAG "main"
namespace iflytop {
Main gmain;
};

using namespace iflytop;
void Main::run() {
  iflytop_no_os_cfg_t oscfg = {
      .delayhtim = &DELAY_US_TIMER,
      .debuguart = &DEBUG_UART,
  };
  iflytop_no_os_init(&oscfg);

  ZLOGI(TAG, "robotic_core_xy:%s", VERSION);

  DEBUG_LIGHT_GPIO.initAsOutput(true);
  ZHAL_CORE_REG(200, { DEBUG_LIGHT_GPIO.toggleState(); });

  // while (true) {
  //   chip_delay_us(1000 * 1000);
  //   ZLOGI(TAG, ".....");
  // }
  TMC4361A motora;
  TMC4361A motorb;
  TMC_MOTOR1_SPI_SELECT1_IO.initAsOutput(true);
  TMC_MOTOR1_nFREEZE_IO.initAsOutput(true);
  TMC_MOTOR1_nRESET_IO.initAsOutput(true);
  TMC_MOTOR1_SUB_IC_ENN_IO.initAsOutput(true);
  TMC_MOTOR2_SPI_SELECT1_IO.initAsOutput(true);
  TMC_MOTOR2_nFREEZE_IO.initAsOutput(true);
  TMC_MOTOR2_nRESET_IO.initAsOutput(true);
  TMC_MOTOR2_SUB_IC_ENN_IO.initAsOutput(true);
  {
    TMC4361A::cfg_t motora_cfg = {.spi               = &TMC_MOTOR_SPI,
                                  .csgpio            = &TMC_MOTOR1_SPI_SELECT1_IO,
                                  .resetPin          = &TMC_MOTOR1_nRESET_IO,
                                  .fREEZEPin         = &TMC_MOTOR1_nFREEZE_IO,
                                  .ennPin            = NULL,
                                  .driverIC_ennPin   = &TMC_MOTOR1_SUB_IC_ENN_IO,
                                  .driverIC_resetPin = NULL};

    motora.initialize(&motora_cfg);
    int32_t ic4361Version = motora.readICVersion();
    int32_t ic2160Version = motora.readSubICVersion();
    ZLOGI(TAG, "motora:TMC4361Version:%lx  TMC2160VERSION:%lx", ic4361Version, ic2160Version);
  }

  {
    TMC4361A::cfg_t motorb_cfg = {.spi               = &TMC_MOTOR_SPI,
                                  .csgpio            = &TMC_MOTOR2_SPI_SELECT1_IO,
                                  .resetPin          = &TMC_MOTOR2_nRESET_IO,
                                  .fREEZEPin         = &TMC_MOTOR2_nFREEZE_IO,
                                  .ennPin            = NULL,
                                  .driverIC_ennPin   = &TMC_MOTOR2_SUB_IC_ENN_IO,
                                  .driverIC_resetPin = NULL};

    motorb.initialize(&motorb_cfg);
    int32_t ic4361Version = motorb.readICVersion();
    int32_t ic2160Version = motorb.readSubICVersion();
    ZLOGI(TAG, "motorb:TMC4361Version:%lx  TMC2160VERSION:%lx", ic4361Version, ic2160Version);
  }

  motora.setIHOLD_IRUN(1, 12, 0);  // 小臂
  motora.setMotorShaft(0);

  motorb.setIHOLD_IRUN(1, 12, 0);  // 小臂
  motorb.setMotorShaft(0);

  motora.rotate(1200000);
  motorb.rotate(1200000);

  while (1) {
    ZHALCORE::getInstance()->loop();
  }
}