// #include <Preferences.h>

#include "Arduino.h"
#include "SPI.h"
#include "USB.h"
// #include "Wire.h"
// #include "Wire.h"
#include <SimpleFOC.h>
// #include <INA3221.h>

#include "SimpleFOCDrivers.h"
#include "encoders/esp32hwencoder/ESP32HWEncoder.h"

#include "motors/HybridStepperMotor/HybridStepperMotor.h"

#include "drivers/drv8316/drv8316.h"

#include <cmath>

// #include "encoders/sc60228/MagneticSensorSC60228.h"
// #include "esp32-hal-adc.h"
// #include "esp32-hal-gpio.h"
// #include "esp32-hal-uart.h"
// #include "esp32-hal.h"
// #include "simplesync.hpp"

#define ENC_A 17
#define ENC_B 8

#define I2C_SDA 18
#define I2C_SCL 0

#define DRVOFF 46
#define TERP_PCB 2
#define TERP_EXT 1

#define SOA 3
#define SOB 9
#define SOC 10

#define INLA 21
#define INHA 47
#define INLB 13
#define INHB 14
#define INLC 11
#define INHC 12

#define CAN_TX 45
#define CAN_RX 48

#define SPI_MISO 6
#define SPI_MOSI 7
#define SPI_CLK 15
#define SPI_DRV_SC 16

#define MOTOR
#define SSIF_USBSERIAL
#define FW_NO_WATCHDOG

#ifndef FW_NO_WATCHDOG
#include <esp_task_wdt.h>
#define WDT_TIMEOUT_s 1
// struct esp_task_wdt_config_t wd_config = {
//     .timeout_ms = 100,
//     .idle_core_mask = ~0x00000000,
//     .trigger_panic = true
// };
#endif

const int voltage_lpf = 50;
// BLDC motor & driver instance
// BLDCMotor motor = BLDCMotor(pole pair number);
ESP32HWEncoder encoder =
    ESP32HWEncoder(ENC_A, ENC_B, 2048 / 2); // The Index pin can be omitted

#ifdef MOTOR
BLDCMotor motor = BLDCMotor(11); // 24N22P
// HybridStepperMotor motor = HybridStepperMotor(50, 3.7, 10, 4.5 / 1000);
DRV8316Driver3PWM driver = DRV8316Driver3PWM(INHA, INHB, INHC, SPI_DRV_SC);
// BLDCDriver3PWM driver = BLDCDriver3PWM(INHA,INHB,INHC);
#endif
// BLDCDriver3PWM driver = BLDCDriver3PWM(pwmA, pwmB, pwmC, Enable(optional));

#ifdef SSIF_USBSERIAL
USBCDC usbserial;
#define SSYNCIF usbserial
#else
#define SSYNCIF Serial1
#endif
// #define SSYNCIF Serial

void read_i2c(uint8_t addr, uint8_t reg, uint8_t *buf, uint8_t len) {
  Wire.beginTransmission(addr);
  Wire.write(reg); // MAN:0x2E
  int error = Wire.endTransmission();
  if (error != 0) {
    // Serial.println("read error!");
    return;
  }
  delayMicroseconds(50);
  int ret = Wire.requestFrom(addr, len);
  // Serial.print("0x");
  // Serial.print(reg, HEX);
  // Serial.print(": ");
  Wire.readBytes(buf, ret);
  // for (int i = 0; i < ret; ++i) {
  // Serial.print("0x");
  // Serial.print(Wire.read(), HEX);
  // Serial.print(" ");
  // }
  // Serial.println();
  return;
}

void write_i2c(uint8_t addr, uint8_t reg_lsb, uint8_t val) {
  Wire.beginTransmission(addr);
  uint8_t data[] = {reg_lsb, val};
  Wire.write(data, sizeof data);
  int error = Wire.endTransmission();
  if (error != 0) {
    // Serial.println("write error!");
    return;
  }
}
void write_dual_i2c(uint8_t addr, uint8_t reg_lsb, uint8_t val1, uint8_t val2) {
  Wire.beginTransmission(addr);
  uint8_t data[] = {reg_lsb, val1, val2};
  Wire.write(data, sizeof data);
  int error = Wire.endTransmission();
  if (error != 0) {
    // Serial.println("write error!");
    return;
  }
}

void status() {

  DRV8316Status status = driver.getStatus();
  SSYNCIF.println("DRV8316 Status:");
  SSYNCIF.print("Fault: ");
  SSYNCIF.println(status.isFault());
  SSYNCIF.print("Buck Error: ");
  SSYNCIF.print(status.isBuckError());
  SSYNCIF.print("  Undervoltage: ");
  SSYNCIF.print(status.isBuckUnderVoltage());
  SSYNCIF.print("  OverCurrent: ");
  SSYNCIF.println(status.isBuckOverCurrent());
  SSYNCIF.print("Charge Pump UnderVoltage: ");
  SSYNCIF.println(status.isChargePumpUnderVoltage());
  SSYNCIF.print("OTP Error: ");
  SSYNCIF.println(status.isOneTimeProgrammingError());
  SSYNCIF.print("OverCurrent: ");
  SSYNCIF.print(status.isOverCurrent());
  SSYNCIF.print("  Ah: ");
  SSYNCIF.print(status.isOverCurrent_Ah());
  SSYNCIF.print("  Al: ");
  SSYNCIF.print(status.isOverCurrent_Al());
  SSYNCIF.print("  Bh: ");
  SSYNCIF.print(status.isOverCurrent_Bh());
  SSYNCIF.print("  Bl: ");
  SSYNCIF.print(status.isOverCurrent_Bl());
  SSYNCIF.print("  Ch: ");
  SSYNCIF.print(status.isOverCurrent_Ch());
  SSYNCIF.print("  Cl: ");
  SSYNCIF.println(status.isOverCurrent_Cl());
  SSYNCIF.print("OverTemperature: ");
  SSYNCIF.print(status.isOverTemperature());
  SSYNCIF.print("  Shutdown: ");
  SSYNCIF.print(status.isOverTemperatureShutdown());
  SSYNCIF.print("  Warning: ");
  SSYNCIF.println(status.isOverTemperatureWarning());
  SSYNCIF.print("OverVoltage: ");
  SSYNCIF.println(status.isOverVoltage());
  SSYNCIF.print("PowerOnReset: ");
  SSYNCIF.println(status.isPowerOnReset());
  SSYNCIF.print("SPI Error: ");
  SSYNCIF.print(status.isSPIError());
  SSYNCIF.print("  Address: ");
  SSYNCIF.print(status.isSPIAddressError());
  SSYNCIF.print("  Clock: ");
  SSYNCIF.print(status.isSPIClockFramingError());
  SSYNCIF.print("  Parity: ");
  SSYNCIF.println(status.isSPIParityError());
  DRV8316_PWMMode val = driver.getPWMMode();
  SSYNCIF.print("PWM Mode: ");
  SSYNCIF.println(val);
  SSYNCIF.print("Recirculation: ");
  SSYNCIF.println(driver.getRecirculationMode());
}
// Preferences prefs;

SemaphoreHandle_t mutex;
struct sync_params {};

TaskHandle_t taskFocHandle;
void foc_task(void *parameter) {
  long last_comm = 0;
  while (true) {
#ifdef MOTOR
    // drivers[0].voltage_power_supply = s_foc.vcc_voltage_mV / 1000.0f;
    // for (int i = 1; i < NUM_MOTORS; i += 1)
    //   drivers[i].voltage_power_supply = drivers[0].voltage_power_supply;
#endif

#ifdef MOTOR
    // motors.target = s_foc.vt[i] / anglefactor;
    // motors.loopFOC();
    // motors.move();
#else
    // sensors.update();
#endif
    vTaskDelay(1);
  }
}
TaskHandle_t taskCommHandle;
void comm_task(void *parameter) {
  while (true) {
    long now = millis();

    vTaskDelay(1);
  }
}

void setup() {
  mutex = xSemaphoreCreateMutex();
#ifdef SSIF_USBSERIAL
  SSYNCIF.begin();
  USB.begin();
#else
  SSYNCIF.begin(460800, SERIAL_8N1, 44, 43);
#endif
  // prefs.begin("foc");
  // SSYNCIF.print(__cplusplus);

  // Wire.setTimeOut(10);
  Wire.begin(I2C_SDA, I2C_SCL, 400000);

  pinMode(DRVOFF, OUTPUT);
  digitalWrite(DRVOFF, 0); // enable
  pinMode(INLA, OUTPUT);
  digitalWrite(INLA, 1); // enable
  pinMode(INLB, OUTPUT);
  digitalWrite(INLB, 1); // enable
  pinMode(INLC, OUTPUT);
  digitalWrite(INLC, 1); // enable

  // write_i2c(0x36,0x07, 0b00000011);
  write_i2c(0x36, 0x09, 0xFF);

  encoder.init();
#ifdef MOTOR

  driver.voltage_power_supply = 15;
  SPI.begin(SPI_CLK, SPI_MISO, SPI_MOSI, SPI_DRV_SC);
  driver.init(&SPI);
  driver.setSlew(DRV8316_Slew::Slew_200Vus);
  status();
  motor.linkDriver(&driver);
  // motor.linkSensor(&encoder);
  motor.foc_modulation = FOCModulationType::SpaceVectorPWM;
  motor.voltage_sensor_align = 5;
  motor.voltage_limit = 6;
  motor.controller = MotionControlType::velocity_openloop;
  motor.init();
  // motor.initFOC();

  // sensor.init(&SPI);
  // motors[i].linkSensor(&sensors[i]);
#endif

  // ina.getCurrent(INA3221_CH1) * 1000; ina.getVoltage(INA3221_CH1);
  // ina.getCurrent(INA3221_CH2) * 1000; ina.getVoltage(INA3221_CH2);
  // ina.getCurrent(INA3221_CH3) * 1000; ina.getVoltage(INA3221_CH3);

  // initialise magnetic sensor hardware

#ifdef MOTOR
  // pinMode(nRESET, OUTPUT);
  // digitalWrite(nRESET, 1);
#endif
  // power supply voltage [V]
  // s_exchange.vcc_voltage_mV = GET_VCC;
  // delay(100);
  // s_exchange.vcc_voltage_mV = GET_VCC;
  // drivers[0].voltage_power_supply = s_exchange.vcc_voltage_mV / 1000.0f;
  // for (int i = 1; i < NUM_MOTORS; i += 1)
  // drivers[i].voltage_power_supply = drivers[0].voltage_power_supply;
  // s_foc = s_exchange;

  // for (int i = 0; i < NUM_MOTORS; i += 1) {
#ifdef MOTOR
  // drivers[i].init();
  // link driver
  // motors[i].linkDriver(&drivers[i]);
  // motors[i].voltage_sensor_align = 5;

  // set control loop type to be used
  // motors[i].torque_controller = TorqueControlType::voltage;
  // motors[i].controller = MotionControlType::velocity;

  // contoller configuration based on the control type
  // motors[i].PID_velocity.P = 0.35;
  // motors[i].PID_velocity.I = 8;
  // motors[i].PID_velocity.D = 0;

  // default voltage_power_supply
  // motors[i].voltage_limit = 3.4 * 3;

  // velocity low pass filtering time constant
  // motors[i].LPF_velocity.Tf = 0.01;
#endif
  // }
#ifdef MOTOR
  // drivers[2].init();
  // drivers[2].voltage_limit = 3.4 * 3;
  // drivers[2].setPwm(s_exchange.vcc_voltage_mV,s_exchange.vcc_voltage_mV,s_exchange.vcc_voltage_mV);

  // pinMode(4, OUTPUT);
  // digitalWrite(4, 1);
  // pinMode(5, OUTPUT);
  // digitalWrite(5, 1);
  // pinMode(6, OUTPUT);
  // digitalWrite(6, 1);
#endif
  // angle loop controller
  // motor.P_angle.P = 20;
  // angle loop velocity limit
  // motor.velocity_limit = 150;

  // initialise motor
  // align encoder and start FOC
#ifdef MOTOR
  // for (int i = 0; i < NUM_MOTORS; i += 1) {
  //   motors[i].init();
  //   // std::string pref_zero_key = "m" + std::to_string(i) + ".zero";
  //   // std::string pref_dir_key = "m" + std::to_string(i) + ".dir";
  //   // if (prefs.isKey(pref_zero_key.c_str()) &&
  //   // prefs.isKey(pref_dir_key.c_str())) {
  //   //     motors[i].zero_electric_angle =
  //   //     prefs.getFloat(pref_zero_key.c_str()); motors[i].sensor_direction
  //   =
  //   //     (Direction)prefs.getInt(pref_dir_key.c_str());
  //   // }
  //   motors[i].initFOC();
  //   // if (!prefs.isKey(pref_zero_key.c_str()) ||
  //   // !prefs.isKey(pref_dir_key.c_str())) {
  //   //     prefs.clear();
  //   //     prefs.putFloat(pref_zero_key.c_str(),
  //   motors[i].zero_electric_angle);
  //   //     prefs.putInt(pref_dir_key.c_str(),
  //   (int)motors[i].sensor_direction);
  //   // }
  // }
#endif
  // prefs.end();
  // set the inital target value
#ifdef MOTOR
  // for (int i = 0; i < NUM_MOTORS; i += 1)
  //   motors[i].target = 0;
#endif

  // xTaskCreatePinnedToCore(&comm_task, // Function name of the task
  //                         "Comm",     // Name of the task (e.g. for
  //                         debugging) 4096,       // Stack size (bytes) NULL,
  //                         // Parameter to pass 1,          // Task priority
  //                         &taskCommHandle, // Assign task handle
  //                         1                // Run on the non-Arduino (1) core
  // );
  // xTaskCreatePinnedToCore(&foc_task, // Function name of the task
  //                         "foc",     // Name of the task (e.g. for debugging)
  //                         2048,      // Stack size (bytes)
  //                         NULL,      // Parameter to pass
  //                         1,         // Task priority
  //                         &taskFocHandle, // Assign task handle
  //                         0               // Run on the non-Arduino (1) core
  // );
  // esp_task_wdt_init(WDT_TIMEOUT_s, true);  // enable panic so ESP32 restarts
  // esp_task_wdt_add(NULL);                  // add current thread to WDT watch
}

void loop() {
#ifdef MOTOR
  // motor.loopFOC();

  motor.move(3);
#endif
  // delay(100);
  // static int i = 0;
  // if (i++ % 100 == 0)
  //   status();
}

// int i = 0;
// void loop() {

//     //encoder.update(); // optional: Update manually if not using loopfoc()
//     //
//      // Access the sensor value
//     SSYNCIF.println(encoder.getAngle());
//     delay(10);
// }
// esp_task_wdt_reset();
