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

#include "AMS_HighLevel.h"
#include "ADBMS_Abstraction.h"
#include "ADBMS_LL_Driver.h"
#include "TMP1075.h"
#include "can-halal.h"
#include "errors.h"
#include "stm32f3xx_hal.h"

Cell_Module module = {};
uint32_t balancedCells = 0;
bool balancingActive = false;

uint16_t amsuv = 0;
uint16_t amsov = 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) {
  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:
      break;
    case AMSERROR:
      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; //we don't receive data for the wakeup command
    set_error_source(ERROR_SOURCE_INTERNAL);         //so we can't tell if we timed out
  }
  
  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);

  if (packetChecksumFails > MAX_PACKET_CHECKSUM_FAILS) {
    error_data.data_kind = SEK_INTERNAL_BMS_CHECKSUM_FAIL;
    set_error_source(ERROR_SOURCE_INTERNAL);
  }
  
  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);

  if ((module.overVoltage | module.underVoltage)) {
    
  }

  return 0;
}

uint8_t AMS_Warning_Loop() { 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() {
  //TODO: implement
  return 0;
}