/*
 * ADBMS_Abstraction.c
 *
 *  Created on: 14.07.2022
 *      Author: max
 */

#include "ADBMS_Abstraction.h"
#include "ADBMS_CMD_MAKROS.h"
#include "ADBMS_LL_Driver.h"
#include <stddef.h>

uint8 numberofcells;
uint8 numberofauxchannels;

#define CHECK_RETURN(x)                                                        \
  {                                                                            \
    uint8 status = x;                                                          \
    if (status != 0)                                                           \
      return status;                                                           \
  }

uint8 amsReset() {
  amsWakeUp();
  readCMD(SRST, NULL, 0);
  mcuDelay(10);
  amsWakeUp();
  amsStopBalancing();
  amsConfigOverUnderVoltage(DEFAULT_OV, DEFAULT_UV);

  uint8 buffer[6] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};

  CHECK_RETURN(writeCMD(CLRFLAG, buffer, 6)); //clear flags,
  CHECK_RETURN(writeCMD(CLOVUV, buffer, 6));  //OVUV flags
  CHECK_RETURN(writeCMD(ADCV | ADCV_CONT | ADCV_RD, NULL, 0)); //start continuous cell voltage measurement with redundancy
  CHECK_RETURN(writeCMD(ADAX | ADAX_CONV_ALL, NULL, 0)); //start aux measurement

  return 0;
}

uint8 initAMS(SPI_HandleTypeDef* hspi, uint8 numofcells, uint8 numofaux) {
  adbmsDriverInit(hspi);
  numberofcells = numofcells;
  numberofauxchannels = numofaux;

  return amsReset();
}

uint8 amsWakeUp() {
  uint8 buf[6];
  return readCMD(RDCFGA, buf, 6);
}

uint8 amsCellMeasurement(Cell_Module* module) {
  #warning check conversion counter to ensure that continous conversion has not been stopped
  #warning check for OW conditions: ADSV | ADSV_OW_0 / ADSV_OW_1
  return amsReadCellVoltages(module);
}

uint8 amsConfigCellMeasurement(uint8 numberofChannels) {
  numberofcells = numberofChannels;
  return 0;
}

uint8 amsAuxAndStatusMeasurement(Cell_Module* module) {
  uint8 rxbuf[AUX_GROUP_A_SIZE] = {};

  CHECK_RETURN(readCMD(RDSTATC, rxbuf, STATUS_GROUP_C_SIZE));

  module->status.CS_FLT = rxbuf[0] | (rxbuf[1] << 8);
  module->status.CCTS   = rxbuf[2] | (rxbuf[3] << 8);
  module->status.VA_OV  = (rxbuf[4] >> 7) & 0x01;
  module->status.VA_UV  = (rxbuf[4] >> 6) & 0x01;
  module->status.VD_OV  = (rxbuf[4] >> 5) & 0x01;
  module->status.VD_UV  = (rxbuf[4] >> 4) & 0x01;
  module->status.CED    = (rxbuf[4] >> 3) & 0x01;
  module->status.CMED   = (rxbuf[4] >> 2) & 0x01;
  module->status.SED    = (rxbuf[4] >> 1) & 0x01;
  module->status.SMED   = (rxbuf[4] >> 0) & 0x01;
  module->status.VDEL   = (rxbuf[5] >> 7) & 0x01;
  module->status.VDE    = (rxbuf[5] >> 6) & 0x01;
  module->status.COMPARE= (rxbuf[5] >> 5) & 0x01;
  module->status.SPIFLT = (rxbuf[5] >> 4) & 0x01;
  module->status.SLEEP  = (rxbuf[5] >> 3) & 0x01;
  module->status.THSD   = (rxbuf[5] >> 2) & 0x01;
  module->status.TMODCHK= (rxbuf[5] >> 1) & 0x01;
  module->status.OSCCHK = (rxbuf[5] >> 0) & 0x01;

  if (pollCMD(PLAUX) == 0x0) { //TODO: check for SPI fault
    return 0; // aux ADC data not ready
  }

  CHECK_RETURN(readCMD(RDAUXA, rxbuf, AUX_GROUP_A_SIZE));

  module->auxVoltages[0] = mV_from_ADBMS6830(rxbuf[0] | (rxbuf[1] << 8));
  module->auxVoltages[1] = mV_from_ADBMS6830(rxbuf[2] | (rxbuf[3] << 8));
  module->auxVoltages[2] = mV_from_ADBMS6830(rxbuf[4] | (rxbuf[5] << 8));

  CHECK_RETURN(readCMD(RDAUXB, rxbuf, AUX_GROUP_A_SIZE));

  module->auxVoltages[3] = mV_from_ADBMS6830(rxbuf[0] | (rxbuf[1] << 8));
  module->auxVoltages[4] = mV_from_ADBMS6830(rxbuf[2] | (rxbuf[3] << 8));
  module->auxVoltages[5] = mV_from_ADBMS6830(rxbuf[4] | (rxbuf[5] << 8));

  CHECK_RETURN(readCMD(RDAUXC, rxbuf, AUX_GROUP_A_SIZE));

  module->auxVoltages[6] = mV_from_ADBMS6830(rxbuf[0] | (rxbuf[1] << 8));
  module->auxVoltages[7] = mV_from_ADBMS6830(rxbuf[2] | (rxbuf[3] << 8));
  module->auxVoltages[8] = mV_from_ADBMS6830(rxbuf[4] | (rxbuf[5] << 8));

  CHECK_RETURN(readCMD(RDAUXD, rxbuf, AUX_GROUP_A_SIZE));

  module->auxVoltages[9] = mV_from_ADBMS6830(rxbuf[0] | (rxbuf[1] << 8));

  uint8 rxbuffer[STATUS_GROUP_A_SIZE];

  CHECK_RETURN(readCMD(RDSTATA, rxbuffer, STATUS_GROUP_A_SIZE));

  module->internalDieTemp = rxbuffer[2] | (rxbuffer[3] << 8);

  CHECK_RETURN(readCMD(RDSTATB, rxbuffer, STATUS_GROUP_B_SIZE));
  module->digitalSupplyVoltage = mV_from_ADBMS6830(rxbuffer[0] | (rxbuffer[1] << 8));
  module->analogSupplyVoltage  = mV_from_ADBMS6830(rxbuffer[2] | (rxbuffer[3] << 8));
  module->refVoltage           = mV_from_ADBMS6830(rxbuffer[4] | (rxbuffer[5] << 8));

  CHECK_RETURN(writeCMD(ADAX | ADAX_CONV_ALL, NULL, 0)); //start aux measurement for next cycle

  return 0;
}

uint8 amsConfigBalancing(uint32 channels, uint8 dutyCycle) {
  uint8 buffer_a[PWM_GROUP_A_SIZE] = {};
  uint8 buffer_b[PWM_GROUP_B_SIZE] = {};
  CHECK_RETURN(readCMD(RDPWMA, buffer_a, CFG_GROUP_A_SIZE));
  CHECK_RETURN(readCMD(RDPWMB, buffer_b, CFG_GROUP_B_SIZE));

  if (dutyCycle > 0x0F) { // there are only 4 bits for duty cycle
    return 1;
  }

  #warning fixme

  for (size_t i = 0; i < 16; i += 2) {
    if (i < 12) { // cells 0, 1 are in regbuffer[0], cells 2, 3 in regbuffer[1], ...
      buffer_a[i / 2] = ((channels & (1 << (i + 1))) ? (dutyCycle << 4) : 0) |
                         ((channels & (1 << i)) ? dutyCycle : 0);
    } else {
      buffer_b[(i - 12) / 2] = ((channels & (1 << (i + 1))) ? (dutyCycle << 4) : 0) |
                                ((channels & (1 << i)) ? dutyCycle : 0);
    }
  }

  CHECK_RETURN(writeCMD(WRPWMA, buffer_a, CFG_GROUP_A_SIZE));
  CHECK_RETURN(writeCMD(WRPWMB, buffer_b, CFG_GROUP_B_SIZE));

  return 0;
}

uint8 amsStartBalancing(uint8 dutyCycle) { return writeCMD(UNMUTE, NULL, 0); }

uint8 amsStopBalancing() { return writeCMD(MUTE, NULL, 0); }

uint8 amsSelfTest() { return 0; }

uint8 amsConfigOverUnderVoltage(uint16 overVoltage, uint16 underVoltage) {
  uint8 buffer[CFG_GROUP_A_SIZE];

  if (underVoltage & 0xF000 || overVoltage & 0xF000) { // only 12 bits allowed
    return 1;
  }

  CHECK_RETURN(readCMD(RDCFGB, buffer, CFG_GROUP_A_SIZE));

  //UV
  buffer[0] = (uint8) (underVoltage & 0xFF);
  buffer[1] &= 0xF0;
  buffer[1] |= (uint8) ((underVoltage >> 8) & 0x0F);

  //OV
  buffer[1] &= 0x0F;
  buffer[1] |= (uint8) (overVoltage << 4);
  buffer[2] = (uint8) (overVoltage >> 4);

  return writeCMD(WRCFGB, buffer, CFG_GROUP_A_SIZE);
}

uint8 amsCheckUnderOverVoltage(Cell_Module* module) {
  uint8 regbuffer[STATUS_GROUP_D_SIZE];
  uint32 ov_uv_data = 0;
  CHECK_RETURN(readCMD(RDSTATD, regbuffer, STATUS_GROUP_D_SIZE));
  ov_uv_data = (regbuffer[0] <<  0) | (regbuffer[1] <<  8) | 
               (regbuffer[2] << 16) | (regbuffer[3] << 24);

  module->overVoltage = 0;
  module->underVoltage = 0;
  
  for (size_t i = 0; i < numberofcells; i++) { // ov/uv flags are 1-bit flags for each cell C0UV, C0OV, C1UV, C1OV, ...
    module->underVoltage |= (ov_uv_data >> (i * 2)) & 0x01;
    module->overVoltage  |= (ov_uv_data >> (i * 2 + 1)) & 0x01;
  }

  return 0;
}

uint8 amsClearAux() {
  uint8 buffer[6];
  return writeCMD(CLRAUX, buffer, 0);
}

uint8 amsClearCells() {
  uint8 buffer[6];
  return writeCMD(CLRCELL, buffer, 0);
}

uint8 amsReadCellVoltages(Cell_Module* module) {
  uint8 rxbuffer[CV_GROUP_A_SIZE];
  CHECK_RETURN(readCMD(RDCVA, rxbuffer, CV_GROUP_A_SIZE));
  module->cellVoltages[0] = mV_from_ADBMS6830(rxbuffer[0] | (rxbuffer[1] << 8));
  module->cellVoltages[1] = mV_from_ADBMS6830(rxbuffer[2] | (rxbuffer[3] << 8));
  module->cellVoltages[2] = mV_from_ADBMS6830(rxbuffer[4] | (rxbuffer[5] << 8));

  CHECK_RETURN(readCMD(RDCVB, rxbuffer, CV_GROUP_A_SIZE));
  module->cellVoltages[3] = mV_from_ADBMS6830(rxbuffer[0] | (rxbuffer[1] << 8));
  module->cellVoltages[4] = mV_from_ADBMS6830(rxbuffer[2] | (rxbuffer[3] << 8));
  module->cellVoltages[5] = mV_from_ADBMS6830(rxbuffer[4] | (rxbuffer[5] << 8));

  CHECK_RETURN(readCMD(RDCVC, rxbuffer, CV_GROUP_A_SIZE));
  module->cellVoltages[6] = mV_from_ADBMS6830(rxbuffer[0] | (rxbuffer[1] << 8));
  module->cellVoltages[7] = mV_from_ADBMS6830(rxbuffer[2] | (rxbuffer[3] << 8));
  module->cellVoltages[8] = mV_from_ADBMS6830(rxbuffer[4] | (rxbuffer[5] << 8));

  CHECK_RETURN(readCMD(RDCVD, rxbuffer, CV_GROUP_A_SIZE));
  module->cellVoltages[9]  = mV_from_ADBMS6830(rxbuffer[0] | (rxbuffer[1] << 8));
  module->cellVoltages[10] = mV_from_ADBMS6830(rxbuffer[2] | (rxbuffer[3] << 8));
  module->cellVoltages[11] = mV_from_ADBMS6830(rxbuffer[4] | (rxbuffer[5] << 8));

  CHECK_RETURN(readCMD(RDCVE, rxbuffer, CV_GROUP_A_SIZE));
  module->cellVoltages[12] = mV_from_ADBMS6830(rxbuffer[0] | (rxbuffer[1] << 8));
  module->cellVoltages[13] = mV_from_ADBMS6830(rxbuffer[2] | (rxbuffer[3] << 8));
  module->cellVoltages[14] = mV_from_ADBMS6830(rxbuffer[4] | (rxbuffer[5] << 8));

  CHECK_RETURN(readCMD(RDCVF, rxbuffer, CV_GROUP_A_SIZE));
  module->cellVoltages[15] = mV_from_ADBMS6830(rxbuffer[0] | (rxbuffer[1] << 8));

  return 0;
}