/*
 * AMS_HighLevel.c
 *
 *  Created on: 20.07.2022
 *      Author: max
 */

#include "AMS_HighLevel.h"


Cell_Module module = {};
uint32_t balancedCells = 0;
uint8_t BalancingActive = 0;
uint8_t stateofcharge = 100;
int64_t currentintegrator = 0;
uint32_t lastticks = 0;
uint32_t currenttick = 0;
uint8_t eepromconfigured = 0;

uint8_t internalbalancingalgo = 1;
uint16_t startbalancingthreshold = 41000;
uint16_t stopbalancingthreshold = 30000;
uint16_t balancingvoltagedelta = 10;

uint16_t amsuv = 0;
uint16_t amsov = 0;

uint8_t amserrorcode = 0;
uint8_t amswarningcode = 0;

uint8_t numberofCells = 13;
uint8_t numberofAux = 0;

uint8_t packetChecksumFails = 0;
#define MAX_PACKET_CHECKSUM_FAILS 5

uint8_t deviceSleeps = 0;
#define MAX_DEVICE_SLEEP 3 //TODO: change to correct value

amsState currentAMSState = AMSDEACTIVE;
amsState lastAMSState = AMSDEACTIVE;

struct pollingTimes {
  uint32_t S_ADC_OW_CHECK;
  uint32_t TMP1075;
};

struct pollingTimes pollingTimes = {0, 0};

void AMS_Init(SPI_HandleTypeDef* hspi) {
  if (eepromconfigured == 1) {
    /*amsov = eepromcellovervoltage>>4;
    amsuv = (eepromcellundervoltage-1)>>4;
    numberofCells = eepromnumofcells;
    numberofAux = eepromnumofaux;
    initAMS(hspi, eepromnumofcells, eepromnumofaux);*/
    amsConfigOverUnderVoltage(amsov, amsuv);
  } else {
    initAMS(hspi, numberofCells, numberofAux);
    amsov = DEFAULT_OV;
    amsuv = DEFAULT_UV;
  }

  pollingTimes = (struct pollingTimes) {HAL_GetTick(), HAL_GetTick()};

  currentAMSState = AMSIDLE;
}

void AMS_Loop() {

  // On Transition Functions called ones if the State Changed

  if (currentAMSState != lastAMSState) {
    switch (currentAMSState) {
    case AMSIDLE:
      break;
    case AMSDEACTIVE:
      break;
    case AMSCHARGING:
      break;
    case AMSIDLEBALANCING:
      break;
    case AMSDISCHARGING:
      break;
    case AMSWARNING:
      writeWarningLog(0x01);
      break;
    case AMSERROR:
      writeErrorLog(amserrorcode);
      break;
    }
    lastAMSState = currentAMSState;
  }

  // Main Loops for different AMS States

  switch (currentAMSState) {
  case AMSIDLE:
    AMS_Idle_Loop();
    break;
  case AMSDEACTIVE:
    break;
  case AMSCHARGING:
    break;
  case AMSIDLEBALANCING:
    AMS_Idle_Loop();
    break;
  case AMSDISCHARGING:
    break;
  case AMSWARNING:
    AMS_Warning_Loop();
    break;
  case AMSERROR:
    break;
  }
}

uint8_t AMS_Idle_Loop() {
  if (!amsWakeUp()) {
    //error_data.data_kind = SEK_INTERNAL_BMS_TIMEOUT;
    //set_error_source(ERROR_SOURCE_INTERNAL);
  }
  
  packetChecksumFails += amsAuxAndStatusMeasurement(&module);

  if (module.status.SLEEP) {
    deviceSleeps++;
    if (deviceSleeps > MAX_DEVICE_SLEEP) {
      error_data.data_kind = SEK_INTERNAL_BMS_TIMEOUT;
      set_error_source(ERROR_SOURCE_INTERNAL);
    } else {
      amsReset();
    }
  }

  if (module.status.CS_FLT || module.status.SPIFLT || module.status.CMED ||
      module.status.SMED || module.status.VDE || module.status.VDEL ||
     module.status.OSCCHK || module.status.TMODCHK) {
    error_data.data_kind = SEK_INTERNAL_BMS_FAULT;
    set_error_source(ERROR_SOURCE_INTERNAL);
  }

  if (module.status.THSD) {
    error_data.data_kind = SEK_INTERNAL_BMS_OVERTEMP;
    set_error_source(ERROR_SOURCE_INTERNAL);
  }

  packetChecksumFails += amsCellMeasurement(&module);
  packetChecksumFails += amsCheckUnderOverVoltage(&module);
  packetChecksumFails += integrateCurrent();

  if (packetChecksumFails > MAX_PACKET_CHECKSUM_FAILS) {
    error_data.data_kind = SEK_INTERNAL_BMS_CHECKSUM_FAIL;
    set_error_source(ERROR_SOURCE_INTERNAL);
  }
  
  tmp1075_measure();

  int any_voltage_error = 0;
  for (size_t i = 0; i < numberofCells; i++) {
    if (module.cellVoltages[i] < 2500) {
      any_voltage_error = 1;
      error_data.data_kind = SEK_UNDERVOLT;
      error_data.data[0] = i;
      uint8_t* ptr = &error_data.data[1];
      ptr = ftcan_marshal_unsigned(ptr, module.cellVoltages[i], 2);
    } else if (module.cellVoltages[i] > 4200) {
      any_voltage_error = 1;
      error_data.data_kind = SEK_OVERVOLT;
      error_data.data[0] = i;
      uint8_t* ptr = &error_data.data[1];
      ptr = ftcan_marshal_unsigned(ptr, module.cellVoltages[i], 2);
    }
  }

  if (module.internalDieTemp > 28000) { //TODO: change to correct value
    error_data.data_kind = SEK_INTERNAL_BMS_OVERTEMP;
    uint8_t* ptr = &error_data.data[0];
    ptr = ftcan_marshal_unsigned(ptr, module.internalDieTemp, 2);

    set_error_source(ERROR_SOURCE_INTERNAL);
  } else {
    clear_error_source(ERROR_SOURCE_INTERNAL);
  }

  if (any_voltage_error) {
    set_error_source(ERROR_SOURCE_VOLTAGES);
  } else {
    clear_error_source(ERROR_SOURCE_VOLTAGES);
  }

  mcuDelay(10);
  return 0;
}

uint8_t AMS_Warning_Loop() {

  amsWakeUp();
  amsConfigOverUnderVoltage(amsov, amsuv);
  amsClearAux();
  amsCellMeasurement(&module);
  amsAuxAndStatusMeasurement(&module);
  amsCheckUnderOverVoltage(&module);

  if (!(module.overVoltage | module.underVoltage)) {
    currentAMSState = AMSIDLE;
    // amsClearWarning();
  }
  amsStopBalancing();

  return 0;
}

uint8_t AMS_Error_Loop() { return 0; }

uint8_t AMS_Charging_Loop() { return 0; }

uint8_t AMS_Discharging_Loop() { return 0; }

uint8_t AMS_Balancing_Loop() {
  uint8_t balancingdone = 1;
  if ((eepromconfigured == 1) && (internalbalancingalgo == 1) &&
      (module.internalDieTemp <
       28000 /*Thermal Protection 93°C*/)) // If the EEPROM is configured and
                                           // the internal Balancing Algorithm
                                           // should be used
  {
    uint16_t highestcellvoltage = module.cellVoltages[0];
    uint16_t lowestcellvoltage = module.cellVoltages[0];
    uint8_t highestcell = 0;
    uint8_t lowestcell = 0;

    for (uint8_t n = 0; n < numberofCells; n++) {
      if (module.cellVoltages[n] > highestcellvoltage) {
        highestcellvoltage = module.cellVoltages[n];
        highestcell = n;
      }
      if (module.cellVoltages[n] < lowestcellvoltage) {
        lowestcellvoltage = module.cellVoltages[n];
        lowestcell = n;
      }
    }

    if (currentAMSState ==
        AMSCHARGING) // Balancing is only Active if the BMS is in Charging Mode
    {

      uint32_t channelstobalance = 0;

      if (highestcellvoltage > startbalancingthreshold) {
        for (uint8_t n = 0; n < numberofCells; n++) {
          if (module.cellVoltages[n] > stopbalancingthreshold) {
            uint16_t dv = module.cellVoltages[n] - lowestcellvoltage;
            if (dv > (balancingvoltagedelta * 1000)) {
              balancingdone = 0;
              channelstobalance |= 1 << n;
            }
          }
        }
      }

      amsConfigBalancing(channelstobalance, 0x0F);
      amsStartBalancing(100);
    }

    else if (currentAMSState == AMSIDLEBALANCING) {

      uint32_t channelstobalance = 0;

      if (lowestcellvoltage <
          stopbalancingthreshold) // If under Voltage of one Cell is reached
      {
        amsStopBalancing();
        balancingdone = 1;
      } else // otherwise continue with regular Balancing Algorithm
      {
        for (uint8_t n = 0; n < numberofCells; n++) {
          uint16_t dv = module.cellVoltages[n] - lowestcellvoltage;
          if (dv > balancingvoltagedelta) {
            balancingdone = 0;
            channelstobalance |= 1 << n;
          }
        }

        amsConfigBalancing(channelstobalance, 0x0F);
        amsStartBalancing(100);
      }
    }
  } else {
    amsStopBalancing();
    balancingdone = 1;
  }
  return balancingdone;
}

uint8_t writeWarningLog(uint8_t warningCode) {
  // eepromWriteWarningLog(warningCode);
  return 0;
}
uint8_t writeErrorLog(uint8_t errorCode) {
  // eepromWriteErrorLog(errorCode);
  return 0;
}

uint8_t integrateCurrent() {
  lastticks = currenttick;
  currenttick = HAL_GetTick();
  if (currenttick < lastticks) {
    currentintegrator += (module.auxVoltages[0] - module.auxVoltages[2]) *
                         (currenttick - lastticks);
  }
  return 0;
}