// #include <Preferences.h>

#include "encoders/abs_inc_combine/AbsIncCombineSensor.h"
#include "esp32-hal.h"
#include <cmath>
#define NODE_NAME "N17BLDC"

#define VERSION_MAJOR 0
#define VERSION_MINOR 1
#define VCS_REVISION_ID 0

#include "Arduino.h"
#include "pin_def.h"


#define USE_USBSERIAL
#ifdef USE_USBSERIAL
#include "USB.h"
USBCDC usbserial;
#ifdef Serial
#undef Serial
#endif
#define Serial usbserial
#else
#define Serial Serial1
#endif
// #define Serial Serial

#include "canard.c"

#include <SPI.h>
#define spi_dev SPI
// #include "Wire.h"
#include <SimpleFOC.h>

#include "SimpleFOCDrivers.h"
#include "encoders/calibrated/CalibratedSensor.h"
#include "encoders/abs_inc_combine/AbsIncCombineSensor.h"
#include "encoders/as5047/MagneticSensorAS5047.h"
// #include "encoders/esp32hwencoder/ESP32HWEncoder.h"

#include "DRV8323S.hpp"

#include "HybridStepperMotor.h"
#include "esp32-hal-adc.h"
#include "esp32-hal-gpio.h"

#include "udral_servo.hpp"

#include <uavcan/pnp/NodeIDAllocationData_2_0.h>

#define TEMP(v0, b, rt, r1, vdd)                                               \
  ((1.0 /                                                                      \
    ((1.0 / 298.15) + ((1.0 / b) * log((v0 * r1) / (rt * (vdd - v0)))))) -     \
   273.15)
#define temp_pcb                                                               \
  (TEMP(analogRead(TERMISTOR_PCB) * 3.3 / 4096.0, 3435.0, 10000.0, 10000.0,    \
        3.3))

#define vdrive_read (analogRead(VSENSE_PIN) * 20.08f / 845)
//* 20.8f/ 21033.75)

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

// ESP32HWEncoder encoder =
// ESP32HWEncoder(ENC_A, ENC_B, 4096 / 4); // The Index pin can be omitted

MagneticSensorAS5047 encoder_absolute(18);
Encoder encoder(ENC_A, ENC_B, 4096 / 4, 0);
AbsIncCombineSensor abs_inc_sensor(encoder, encoder_absolute);

float encoder_calibration_lut[90];
CalibratedSensor encoder_calibrated(encoder_absolute, sizeof(encoder_calibration_lut) /
                                  sizeof(encoder_calibration_lut[0]), encoder_calibration_lut);
// MagneticSensorAS5600 encoder = MagneticSensorAS5600(0x36);

#ifdef MOTOR
struct drv8323s_foc_driver drv8323s;
// BLDCMotor motor = BLDCMotor(100); // 24N22P
HybridStepperMotor motor = HybridStepperMotor(50); //, 3.7, 10, 4.5 / 1000);
#endif

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) {
    return;
  }
  delayMicroseconds(50);
  int ret = Wire.requestFrom(addr, len);
  Wire.readBytes(buf, ret);
  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) {
    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) {
    return;
  }
}

SemaphoreHandle_t mutex;
struct UdralServoState servo_state = {0};

void mutexed_state_sync(UdralServoState *const state) {
  xSemaphoreTake(mutex, portMAX_DELAY);
  servo_state.target_force = state->target_force;
  servo_state.target_acceleration = state->target_acceleration;
  servo_state.target_velocity = state->target_velocity;
  servo_state.target_position = state->target_position;
  servo_state.arming = state->arming;
  servo_state.pid = state->pid;
  *state = servo_state;
  xSemaphoreGive(mutex);
}

TaskHandle_t taskCommHandle;
void comm_task(void *parameter) { udral_loop(mutexed_state_sync); }

void doA1() { encoder.handleA(); }
void doB1() { encoder.handleB(); }

void setup() {
  pinMode(LED_PIN, OUTPUT);
  mutex = xSemaphoreCreateMutex();
#ifdef USE_USBSERIAL
  Serial.begin();
  USB.begin();
  Serial.setDebugOutput(
      true); // sends all log_e(), log_i() messages to USB HW CDC
  Serial.setTxTimeoutMs(0);
#else
  Serial.begin(460800, SERIAL_8N1, 44, 43);
#endif
  digitalWrite(LED_PIN, 0); // enable
  delay(500);
  digitalWrite(LED_PIN, 1); // disable
  delay(100);

  Serial.println("Start");
  delay(1000);

  pinMode(SPI_DRV_SC, OUTPUT);

  pinMode(INHA, OUTPUT);
  // digitalWrite(INHA, 0); // enable
  pinMode(INHB, OUTPUT);
  // digitalWrite(INHB, 0); // enable
  pinMode(INHC, OUTPUT);
  // digitalWrite(INHC, 0); // enable
  pinMode(INLABC, OUTPUT);
  digitalWrite(INLABC, 0); // enable
  pinMode(CAL_PIN, OUTPUT);
  digitalWrite(CAL_PIN, 0); // enable

  pinMode(SPI_MISO, INPUT_PULLUP);
  SPI.begin(SPI_CLK, SPI_MISO, SPI_MOSI, SPI_DRV_SC);
  //SPI.setFrequency(100000);
  // initialise magnetic sensor hardware
  encoder.pullup = Pullup::USE_INTERN;
  encoder.init();
  encoder.enableInterrupts(doA1, doB1);
  encoder_absolute.init(&spi_dev);

  abs_inc_sensor.init();

  Serial.printf("offset: %f\nvalue: %d", abs_inc_sensor.offset, abs_inc_sensor.getAngle());
#ifdef MOTOR
  SimpleFOCDebug::enable(&Serial);
  drv8323s_init(&drv8323s, INHA, INHB, INHC, SPI_DRV_SC, DRVEN, &spi_dev);
  digitalWrite(INLABC, 1); // enable
  // driver.init(&SPI);
  // status();
  motor.linkSensor(&encoder_calibrated);
  motor.linkDriver(drv8323s.focdriver);
  // motor.foc_modulation = FOCModulationType::SinePWM;
  motor.foc_modulation = FOCModulationType::SpaceVectorPWM;

  motor.voltage_sensor_align = 4;
  motor.voltage_limit = 6;
  // motor.controller = MotionControlType::velocity_openloop;
  // motor.controller = MotionControlType::velocity;
  motor.controller = MotionControlType::torque;
  motor.torque_controller = TorqueControlType::voltage;
  motor.useMonitoring(Serial);
  drv8323s.focdriver->voltage_power_supply = vdrive_read;
  motor.init();
  delay(1000);

  static Preferences pref;
  pref.begin("simpleFOC");

  // set voltage to run calibration
  encoder_calibrated.voltage_calibration = 3;
  encoder_calibrated.init();
  // Running calibration
  const char *encoder_calibration_lut_str = "enc_cal_lut";
  const char *zero_electric_angle_str = "zero_el_angle";
  const char *sensor_direction_str = "enc_dir_str";
  const int settle_time_ms = 150;
  if (!pref.isKey(encoder_calibration_lut_str) ||
      pref.getBytesLength(encoder_calibration_lut_str) !=
          sizeof(encoder_calibration_lut)) {
    encoder_calibrated.calibrate(motor, settle_time_ms);
    pref.putBytes(encoder_calibration_lut_str,
                  &encoder_calibration_lut[0],
                  sizeof(encoder_calibration_lut));
    pref.putFloat(zero_electric_angle_str, motor.zero_electric_angle);
    pref.putInt(sensor_direction_str, (int)motor.sensor_direction);

  } else {
    pref.getBytes(encoder_calibration_lut_str, &encoder_calibration_lut[0],
                  sizeof(encoder_calibration_lut));
    Serial.println("Skipping calibration");
  }

  if (!pref.isKey(zero_electric_angle_str)) {
    motor.initFOC();
    pref.putFloat(zero_electric_angle_str, motor.zero_electric_angle);
    pref.putInt(sensor_direction_str, (int)motor.sensor_direction);
  } else {
    motor.zero_electric_angle = pref.getFloat(zero_electric_angle_str);
    motor.sensor_direction = (Direction)pref.getInt(sensor_direction_str);
    motor.initFOC();
  }
  pref.end();

  motor.move(0);
  digitalWrite(INLABC, 0); // enable
#endif

  xTaskCreatePinnedToCore(&comm_task, // Function name of the task
                          "comm",     // Name of the task (e.g. for debugging)
                          65536,      // Stack size (bytes)
                          NULL,       // Parameter to pass
                          10,          // Task priority
                          &taskCommHandle, // 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
}

int i = 0;
void loop() {
#ifdef MOTOR
  bool armed = false;

  float curr_pcb_temp_kelvin = temp_pcb + 273.15f;
  xSemaphoreTake(mutex, portMAX_DELAY);
  armed = servo_state.arming.armed;
  float target = servo_state.target_force;

  servo_state.controller_temperature = curr_pcb_temp_kelvin; 
  servo_state.motor_temperature = NAN;

  float curr_angle = encoder_calibrated.getAngle();
  float curr_vel = encoder_calibrated.getVelocity();

  unsigned long now = millis();
  static unsigned long last_update = now;
  unsigned long tdelta = now - last_update;
  last_update = now;
  float curr_accel =
      tdelta ? (curr_vel - servo_state.curr_velocity) / tdelta : 0;

  servo_state.curr_acceleration = curr_accel;
  servo_state.curr_velocity = curr_vel;
  servo_state.curr_position = curr_angle;
  servo_state.curr_force = servo_state.target_force;


  servo_state.curr_velocity = encoder.getAngle();
  servo_state.curr_acceleration = encoder_absolute.getAngle();
  servo_state.curr_force = abs_inc_sensor.getAngle();

  servo_state.vcc_volts = drv8323s.focdriver->voltage_power_supply;
  servo_state.current = NAN;
  xSemaphoreGive(mutex);

  if (!armed) {
    if (motor.enabled) {
      motor.move(0);
      motor.disable();
      digitalWrite(INLABC, 0);
    }
  } else {
    if (!motor.enabled) {
      digitalWrite(INLABC, 1);
      motor.enable();
    }
    motor.move(target);
  }

  digitalWrite(LED_PIN, armed ? LOW : HIGH);
  drv8323s.focdriver->voltage_power_supply = vdrive_read;

  motor.loopFOC();
#else
  encoder.update(); // optional: Update manually if not using loopfoc()
#endif
}