#ifndef _DRV8323S_H
#define _DRV8323S_H

#include <Arduino.h>
#include <SPI.h>
#include <cstdint>
#include <drivers/BLDCDriver3PWM.h>

enum {
  DRV8323S_FAULT_STATUS1_REG = 0x00,
  DRV8323S_FAULT_STATUS2_REG = 0x01,
  DRV8323S_DRIVER_CONTROL_REG = 0x02,
  DRV8323S_GATE_DRIVE_HS_REG = 0x03,
  DRV8323S_GATE_DRIVE_LS_REG = 0x04,
  DRV8323S_OCP_CONTROL_REG = 0x05,
  DRV8323S_CSA_CONTROL_REG = 0x06,
};

enum /* 0x00 */ {
  DRV8323S_FAULT_STATUS1_FAULT_MASK = 1 << 10,  /* logic or of all faults */
  DRV8323S_FAULT_STATUS1_VDS_OCP_MASK = 1 << 9, /* vds monitor overcurrent */
  DRV8323S_FAULT_STATUS1_GDF_MASK = 1 << 8,     /* gate drive fault */
  DRV8323S_FAULT_STATUS1_UVLO_MASK = 1 << 7,    /* undervoltage lockout fault */
  DRV8323S_FAULT_STATUS1_OTSD_MASK = 1 << 6,    /* over temperature shutdown */
  DRV8323S_FAULT_STATUS1_VDS_HA_MASK = 1 << 5,  /* overcurrent on FET */
  DRV8323S_FAULT_STATUS1_VDS_LA_MASK = 1 << 4,
  DRV8323S_FAULT_STATUS1_VDS_HB_MASK = 1 << 3,
  DRV8323S_FAULT_STATUS1_VDS_LB_MASK = 1 << 2,
  DRV8323S_FAULT_STATUS1_VDS_HC_MASK = 1 << 1,
  DRV8323S_FAULT_STATUS1_VDS_LC_MASK = 1 << 0,
};

enum /* 0x01 */ {
  DRV8323S_FAULT_STATUS1_SA_OC_MASK = 1 << 10, /* Sense amp OC */
  DRV8323S_FAULT_STATUS1_SB_OC_MASK = 1 << 9,
  DRV8323S_FAULT_STATUS1_SC_OC_MASK = 1 << 8,
  DRV8323S_FAULT_STATUS1_OTW_MASK = 1 << 7,    /* over temp warning */
  DRV8323S_FAULT_STATUS1_CPUV_MASK = 1 << 6,   /* charge pump fault */
  DRV8323S_FAULT_STATUS1_VGS_HA_MASK = 1 << 5, /* FET gate drive failt */
  DRV8323S_FAULT_STATUS1_VGS_LA_MASK = 1 << 4,
  DRV8323S_FAULT_STATUS1_VGS_HB_MASK = 1 << 3,
  DRV8323S_FAULT_STATUS1_VGS_LB_MASK = 1 << 2,
  DRV8323S_FAULT_STATUS1_VGS_HC_MASK = 1 << 1,
  DRV8323S_FAULT_STATUS1_VGS_LC_MASK = 1 << 0,
};

enum /* 0x02 */ {
  DRV8323S_DRIVER_CONTROL_DIS_CPUV_MASK =
      1 << 9, /* chrg pump uvlo fault disable */
  DRV8323S_DRIVER_CONTROL_DIS_GDF_MASK = 1 << 8, /* gate drive fault disable */
  DRV8323S_DRIVER_CONTROL_OTW_REP_MASK = 1
                                         << 7, /* over temp wariing reported */
  DRV8323S_DRIVER_CONTROL_PWM_MODE_MASK = 0b11 << 5, /* PWM mode */
  DRV8323S_DRIVER_CONTROL_1PWM_COM_MASK = 1 << 4, /* 1PWM mode diode freewheel*/
  DRV8323S_DRIVER_CONTROL_1PWM_DIR_MASK = 1 << 3, /* 1PWM mode direction */
  DRV8323S_DRIVER_CONTROL_COAST_MASK = 1 << 2,    /* all FETs in Hi-Z */
  DRV8323S_DRIVER_CONTROL_BRAKE_MASK = 1 << 1,    /* all low FETs on */
  DRV8323S_DRIVER_CONTROL_CLR_FLT_MASK = 1 << 1,  /* clear latched fault bits */
};

enum /* 0x03 */ {
  DRV8323S_GATE_DRIVE_HS_LOCK_MASK = 0b111 << 8,        /* lock register */
  DRV8323S_GATE_DRIVE_HS_IDRIVEP_HS_MASK = 0b1111 << 4, /* HS positiv current */
  DRV8323S_GATE_DRIVE_HS_IDRIVEN_HS_MASK = 0b1111 << 0, /* HS negativ current */
};

enum /* 0x04 */ {
  DRV8323S_GATE_DRIVE_LS_CBC_MASK = 1 << 10,      /* cycle-by cycle operation */
  DRV8323S_GATE_DRIVE_LS_TDRIVE_MASK = 0b11 << 8, /* peak gate curr drive t*/
  DRV8323S_GATE_DRIVE_LS_IDRIVEP_LS_MASK = 0b1111 << 4, /* LS positiv current */
  DRV8323S_GATE_DRIVE_LS_IDRIVEN_LS_MASK = 0b1111 << 0, /* LS negativ current */
};

enum /* 0x05 */ {
  DRV8323S_OCP_CONTROL_TRETRY_MASK = 1 << 10,      /* 0=4ms, 1=50us */
  DRV8323S_OCP_CONTROL_DEAD_TIME_MASK = 0b11 << 8, /* 50ns*2**x dead time */
  DRV8323S_OCP_CONTROL_OCP_MODE_MASK = 0b11 << 6,  /* OCP Mode */
  DRV8323S_OCP_CONTROL_OCP_DEG_MASK = 0b11 << 4,   /* OCP deglitch t=2ns*(1+x)*/
  DRV8323S_OCP_CONTROL_VDS_LVL_MASK = 0b1111 << 0, /* VDS LVL */
};

enum /* 0x06 */ {
  DRV8323S_CSA_CONTROL_CSA_FET_MASK = 1 << 10, /* 0:sense posi in = SPx 1:SHx */
  DRV8323S_CSA_CONTROL_VREF_DIV_MASK = 1 << 9, /* sense ref is Vref/2 */
  DRV8323S_CSA_CONTROL_LS_REF_MASK = 1
                                     << 8, /* 0:VDS_OCP over SHx-SPx 1:SHx-SNx*/
  DRV8323S_CSA_CONTROL_CSA_GAIN_MASK = 0b11 << 6, /* 2**x*5V/V */
  DRV8323S_CSA_CONTROL_DIS_SEN_MASK = 1 << 5,     /* disable OC sense */
  DRV8323S_CSA_CONTROL_CSA_CAL_A_MASK = 1 << 4,   /* cal sense offset A */
  DRV8323S_CSA_CONTROL_CSA_CAL_B_MASK = 1 << 3,   /* cal sense offset B */
  DRV8323S_CSA_CONTROL_CSA_CAL_C_MASK = 1 << 2,   /* cal sense offset C */
  DRV8323S_CSA_CONTROL_SEN_LVL_MASK = 0b11 << 6,  /* Sense OCP = 0.25V*(1+x)*/
};

enum drv8323s_ocp_mode_e {
  DRV8323S_OCP_MODE_LATCHED = 0b00 << 6, // Overcurrent causes a latched fault
  DRV8323S_OCP_MODE_RETRY =
      0b01 << 6, // Overcurrent causes an automatic retrying fault
  DRV8323S_OCP_MODE_REPORT_ONLY =
      0b10 << 6, // Overcurrent is report only but no action is taken
  DRV8323S_OCP_MODE_IGNORE =
      0b11 << 6, // Overcurrent is not reported and no action is take};
};
enum drv8323s_pwm_mode_e {
  DRV8323S_PWM_MODE_6PWM = 0b00 << 5,
  DRV8323S_PWM_MODE_3PWM = 0b01 << 5,
  DRV8323S_PWM_MODE_1PWM = 0b10 << 5,
  DRV8323S_PWM_MODE_INDEPENDENT_PWM = 0b11 << 5,
};

enum drv8323s_sen_lvl_e {
  DRV8323S_SEN_LVL_0_25V = 0b00 << 0,
  DRV8323S_SEN_LVL_0_5V = 0b01 << 0,
  DRV8323S_SEN_LVL_0_75V = 0b10 << 0,
  DRV8323S_SEN_LVL_1V = 0b11 << 0,
};

enum drv8323s_lock_sequence_e {
  DRV8323S_LOCK = 0b011 << 8,
  DRV8323S_UNLOCK = 0b110 << 8,
};

enum drv8323s_idrive_shift_e {
  DRV8323S_IDRIVEP = 4,
  DRV8323S_IDRIVEN = 0,
};

const unsigned int drv8323s_idrivep_mA_map[] = {
    10, 30, 60, 80, 120, 140, 170, 190, 260, 330, 370, 440, 570, 680, 820, 1000,
};
const unsigned int drv8323s_idriven_mA_map[] = {
    20,  60,  120, 160, 240,  280,  340,  380,
    520, 660, 740, 880, 1140, 1360, 1640, 2000,
};

struct drv8323s_foc_driver {
  SPIClass *spi;
  int CSn;
  int en;
  BLDCDriver3PWM *focdriver;
};
uint16_t drv8323s_read_spi(struct drv8323s_foc_driver *dev, uint8_t addr,
                           uint16_t *result) {
  digitalWrite(dev->CSn, 0);
  dev->spi->beginTransaction(SPISettings(500000, MSBFIRST, SPI_MODE1));
  uint16_t data = ((uint16_t)(1 << 15)) | ((((uint16_t)addr)) << 11);
  *result = dev->spi->transfer16(data);
  dev->spi->endTransaction();
  digitalWrite(dev->CSn, 1);
  return 0;
}

uint16_t drv8323s_write_spi(struct drv8323s_foc_driver *dev, uint8_t addr,
                            uint16_t value, uint16_t *result) {
  digitalWrite(dev->CSn, 0);
  dev->spi->beginTransaction(SPISettings(500000, MSBFIRST, SPI_MODE1));
  uint16_t data = ((addr & 0b1111) << 11) | (value & 0b1111111111);
  *result = dev->spi->transfer16(data);
  dev->spi->endTransaction();
  digitalWrite(dev->CSn, 1);
  return 0;
}

void drv8323s_lock_spi(struct drv8323s_foc_driver *dev,
                           bool lock) {
  uint16_t result = 0;
  drv8323s_read_spi(dev, DRV8323S_GATE_DRIVE_HS_REG, &result);
  result &= ~DRV8323S_GATE_DRIVE_HS_LOCK_MASK;
  result |= (lock?DRV8323S_LOCK:DRV8323S_UNLOCK) & DRV8323S_GATE_DRIVE_HS_LOCK_MASK;
  drv8323s_write_spi(dev, DRV8323S_GATE_DRIVE_HS_REG, result, &result);
};

void drv8323s_set_PWM_mode(struct drv8323s_foc_driver *dev,
                           enum drv8323s_pwm_mode_e mode) {
  uint16_t result = 0;
  drv8323s_read_spi(dev, DRV8323S_DRIVER_CONTROL_REG, &result);
  result &= ~DRV8323S_DRIVER_CONTROL_PWM_MODE_MASK;
  result |= mode & DRV8323S_DRIVER_CONTROL_PWM_MODE_MASK;
  drv8323s_write_spi(dev, DRV8323S_DRIVER_CONTROL_REG, result, &result);
}

void drv8323s_set_OCP_mode(struct drv8323s_foc_driver *dev,
                           enum drv8323s_ocp_mode_e mode) {
  uint16_t result = 0;
  drv8323s_read_spi(dev, DRV8323S_OCP_CONTROL_REG, &result);
  result &= ~DRV8323S_OCP_CONTROL_OCP_MODE_MASK;
  result |= mode & DRV8323S_OCP_CONTROL_OCP_MODE_MASK;
  drv8323s_write_spi(dev, DRV8323S_OCP_CONTROL_REG, result, &result);
}

void drv8323s_set_sen_lvl(struct drv8323s_foc_driver *dev,
                          enum drv8323s_sen_lvl_e level) {
  uint16_t result = 0;
  drv8323s_read_spi(dev, DRV8323S_CSA_CONTROL_REG, &result);
  result &= ~DRV8323S_CSA_CONTROL_SEN_LVL_MASK;
  result |= level & DRV8323S_CSA_CONTROL_SEN_LVL_MASK;
  drv8323s_write_spi(dev, DRV8323S_CSA_CONTROL_REG, result, &result);
}

void drv8323s_init(struct drv8323s_foc_driver *dev, int phA, int phB, int phC,
                   int CSn, int en, SPIClass *spi) {
  dev->CSn = CSn;
  dev->spi = spi;
  dev->en = en;
  pinMode(dev->CSn, OUTPUT);
  pinMode(dev->en, OUTPUT);
  digitalWrite(dev->en, 1);
  delay(1);
  drv8323s_lock_spi(dev, false);
  delay(1);
  /* Set 3PWM Mode */
  drv8323s_set_PWM_mode(dev, DRV8323S_PWM_MODE_3PWM);
  delay(1);
  drv8323s_set_OCP_mode(dev, DRV8323S_OCP_MODE_LATCHED);
  delay(1);
  drv8323s_set_sen_lvl(dev, DRV8323S_SEN_LVL_0_25V);
  delay(1);
  drv8323s_lock_spi(dev, true);

  if(!dev->focdriver) {
    dev->focdriver = new BLDCDriver3PWM(phA, phB, phC);
  }
  dev->focdriver->init();

}

void drv8323s_set_idrive(struct drv8323s_foc_driver *dev, int positive_i,
                         int negative_i, bool high_side_fet) {
  uint16_t reg_value = 0x0; // TODO get reg
  uint16_t reg =
      high_side_fet ? DRV8323S_GATE_DRIVE_HS_REG : DRV8323S_GATE_DRIVE_LS_REG;
  drv8323s_read_spi(dev, reg, &reg_value);
  if (high_side_fet) {
    reg_value &= ~DRV8323S_GATE_DRIVE_HS_IDRIVEP_HS_MASK;
    reg_value &= ~DRV8323S_GATE_DRIVE_HS_IDRIVEN_HS_MASK;
  } else {
    reg_value &= ~DRV8323S_GATE_DRIVE_LS_IDRIVEP_LS_MASK;
    reg_value &= ~DRV8323S_GATE_DRIVE_LS_IDRIVEN_LS_MASK;
  }
  int i;
  for (i = 0;
       i < sizeof drv8323s_idrivep_mA_map / sizeof(drv8323s_idrivep_mA_map[0]);
       i += 1) {
    if (drv8323s_idrivep_mA_map[i] > positive_i)
      break;
  }
  reg_value |= i << DRV8323S_IDRIVEP;
  for (i = 0;
       i < sizeof drv8323s_idriven_mA_map / sizeof(drv8323s_idriven_mA_map[0]);
       i += 1) {
    if (drv8323s_idriven_mA_map[i] > negative_i)
      break;
  }
  reg_value |= i << DRV8323S_IDRIVEN;
  uint16_t result;
  drv8323s_write_spi(dev, reg, reg_value, &result);
}

#endif /* _DRV8323S_H */
