// #include <Preferences.h>

#include <cmath>

#include "encoders/abs_inc_combine/AbsIncCombineSensor.h"
#include "esp32-hal.h"
#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 <SPI.h>

#include "canard.c"
#include "udral_servo.hpp"
#define spi_dev SPI
// #include "Wire.h"
#include <SimpleFOC.h>

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

#include <uavcan/pnp/NodeIDAllocationData_2_0.h>

#include "DRV8323S.hpp"
#include "HybridStepperMotor.h"
#include "esp32-hal-adc.h"
#include "esp32-hal-gpio.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;
const float max_voltage_limit = 6;

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

static bool init_done = false;

void mutexed_state_sync(UdralServoState *const state, const float hz) {
  xSemaphoreTake(mutex, portMAX_DELAY);
  if (!init_done) {
    init_done = true;
    state->pid_position.value.count = 3;
    state->pid_position.value.elements[0] = motor.P_angle.P;
    state->pid_position.value.elements[1] = motor.P_angle.I;
    state->pid_position.value.elements[2] = motor.P_angle.D;
    state->pid_velocity.value.count = 7;
    state->pid_velocity.value.elements[0] = motor.PID_velocity.P;
    state->pid_velocity.value.elements[1] = motor.PID_velocity.I;
    state->pid_velocity.value.elements[2] = motor.PID_velocity.D;
    state->pid_velocity.value.elements[3] = motor.LPF_velocity.Tf;
    state->pid_velocity.value.elements[4] = motor.PID_velocity.output_ramp;
    state->pid_velocity.value.elements[5] = motor.PID_velocity.limit;
    motor.motion_downsample = state->pid_velocity.value.elements[6] = 0;
  }
  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_position = state->pid_position;
  servo_state.pid_velocity = state->pid_velocity;

  float curr_angle = servo_state.curr_position;
  static float curr_angle_last = curr_angle;
  float curr_vel = (curr_angle - curr_angle_last) * hz;
  curr_angle_last = curr_angle;

  static float curr_vel_last = curr_vel;
  float curr_accel = (curr_vel - curr_vel_last) * hz;
  curr_vel_last = curr_vel;

  servo_state.curr_acceleration = curr_accel;
  servo_state.curr_velocity = curr_vel;

  *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 = max_voltage_limit;
  // 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

  while (!init_done) {
    delay(10);
  }
  bool armed = false;

  /* adc reading */
  drv8323s.focdriver->voltage_power_supply = vdrive_read;
  const float curr_pcb_temp_kelvin = temp_pcb + 273.15f;

  xSemaphoreTake(mutex, portMAX_DELAY);
  armed = servo_state.arming.armed;

  float target;
  if (!std::isnan(servo_state.target_position)) {
    if (motor.controller != MotionControlType::angle) {
      motor.controller = MotionControlType::angle;
      Serial.println("MotionControlType::angle");
    }
    target = servo_state.target_position;

    motor.velocity_limit = std::isnan(servo_state.target_velocity) ? 10000 : servo_state.target_velocity;
    motor.voltage_limit = std::isnan(servo_state.target_force)
                              ? max_voltage_limit
                              : std::min(max_voltage_limit, std::abs(servo_state.target_force));
  } else if (!std::isnan(servo_state.target_velocity)) {
    if (motor.controller != MotionControlType::velocity) {
      motor.controller = MotionControlType::velocity;
      Serial.println("MotionControlType::velocity");
    }
    target = servo_state.target_velocity;
    motor.voltage_limit = std::isnan(servo_state.target_force)
                              ? max_voltage_limit
                              : std::min(max_voltage_limit, std::abs(servo_state.target_force));
  } else {
    motor.voltage_limit = std::isnan(servo_state.target_force)
                              ? max_voltage_limit
                              : std::min(max_voltage_limit, std::abs(servo_state.target_force));
    target = servo_state.target_force;
    if (motor.controller != MotionControlType::torque) {
      motor.controller = MotionControlType::torque;
      Serial.printf("MotionControlType::torque %f\n", target);
    }
  }

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

  float curr_angle = encoder_calibrated.getAngle();
  servo_state.curr_position = curr_angle;
  servo_state.curr_force = motor.voltage.d;

  servo_state.vcc_volts = drv8323s.focdriver->voltage_power_supply;
  servo_state.current = motor.current.d;

  motor.PID_velocity.P = servo_state.pid_velocity.value.count <= 0 ? 0 : servo_state.pid_velocity.value.elements[0];
  motor.PID_velocity.I = servo_state.pid_velocity.value.count <= 1 ? 0 : servo_state.pid_velocity.value.elements[1];
  motor.PID_velocity.D = servo_state.pid_velocity.value.count <= 2 ? 0 : servo_state.pid_velocity.value.elements[2];
  motor.LPF_velocity.Tf = servo_state.pid_velocity.value.count <= 3 ? 0.01 : servo_state.pid_velocity.value.elements[3];
  motor.PID_velocity.output_ramp =
      servo_state.pid_velocity.value.count <= 4 ? 300 : servo_state.pid_velocity.value.elements[4];
  motor.PID_velocity.limit =
      servo_state.pid_velocity.value.count <= 5 ? 1000 : servo_state.pid_velocity.value.elements[5];
  motor.motion_downsample = servo_state.pid_velocity.value.count <= 6 ? 0 : servo_state.pid_velocity.value.elements[6];

  motor.P_angle.P = servo_state.pid_position.value.count <= 0 ? 0 : servo_state.pid_position.value.elements[0];
  motor.P_angle.I = servo_state.pid_position.value.count <= 1 ? 0 : servo_state.pid_position.value.elements[1];
  motor.P_angle.D = servo_state.pid_position.value.count <= 2 ? 0 : servo_state.pid_position.value.elements[2];
  // motor.P_angle.output_ramp = 10000; // default 1e6 rad/s^2
  /* angle low pass filtering, use only for very noisy position sensors - try to avoid and keep the values very small */
  motor.LPF_angle.Tf = 0;  // default 0[=disabled]
  // setting the limits

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

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