/*
 * BQ_Abstraction_Layer.c
 *
 *  Created on: 29.01.2022
 *      Author: max
 */

#include "BQ_Abstraction_Layer.h"

#include "BQ_Communication.h"
#include "BQ_Register_Definitions.h"

#include "stm32f4xx_hal.h"

uint16_t cell_voltages[N_CELLS];

BQ_Status bq_status;
BQ_Error_Description bq_error;
uint32_t lastmeasurementtime;

static void bq_set_error_without_loc(uint8_t kind) {
  bq_status = BQ_ERROR;
  bq_error.kind |= kind;
}
static void bq_set_error_with_loc(uint8_t kind, uint8_t loc) {
  bq_status = BQ_ERROR;
  bq_error.kind |= kind;
  if (kind == BQ_ERROR_KIND_HAL) {
    bq_error.hal_loc = loc;
  } else if (kind == BQ_ERROR_KIND_CRC) {
    bq_error.crc_loc = loc;
  }
}

void afe_init(UART_HandleTypeDef* uarthandle) {
  // Initialise underlying BQ Communication Functions
  init_BQCom(uarthandle);

  // Turn the AFE on off on to cycle a full reset
  afe_wakeup();
  HAL_Delay(10);
  afe_shutdown();
  HAL_Delay(100);
  afe_wakeup();
  HAL_Delay(10);

  bq_status = BQ_INIT_PHASE;

  afe_config_communication();
  afe_config_measurement_channels();

  afe_config_gpios();
  afe_activate_LED();

  afe_init_fault_thresholds();

  HAL_Delay(1000);

  afe_update_Checksum();

  afe_clear_all_faults();

  HAL_Delay(10);

  afe_check_faults();
}

void afe_shutdown() { BQ_Write_Register(DEV_CNTRL, DEV_CNTRL_SIZE, 0x40); }

void afe_wakeup() {
  HAL_GPIO_WritePin(WAKEUP_PORT, WAKEUP_PIN, GPIO_PIN_SET);
  HAL_Delay(1);
  HAL_GPIO_WritePin(WAKEUP_PORT, WAKEUP_PIN, GPIO_PIN_RESET);
}

void afe_measure() {
  uint8_t cellvoltagebuffer[2 * N_CELLS];
  BQ_Comm_Status retval = BQ_ReadMeasurements(cellvoltagebuffer, 2 * N_CELLS);

  lastmeasurementtime = HAL_GetTick();

  if (retval == BQ_COMM_OK) {
    for (int n = 0; n < N_CELLS; n++) {
      cell_voltages[N_CELLS - 1 - n] =
          (uint16_t)(cellvoltagebuffer[2 * n] << 8) +
          (uint16_t)cellvoltagebuffer[2 * n + 1];
    }
  } else if (retval == BQ_COMM_ERR_HAL) {
    bq_set_error_with_loc(BQ_COMM_ERR_HAL, BQ_ERROR_LOC_MEASURE);
  } else if (retval == BQ_COMM_ERR_CRC) {
    bq_set_error_with_loc(BQ_COMM_ERR_CRC, BQ_ERROR_LOC_MEASURE);
  }
}

void afe_selftest() {}

void afe_check_faults() {
  uint32_t faultflags = 0;
  BQ_Read_Register(FAULT_SUM, FAULT_SUM_SIZE, &faultflags);

  if (faultflags == 0) {
    return;
  }

  bq_set_error_without_loc(BQ_ERROR_KIND_FAULT);
  uint32_t data;
  if (faultflags & UV_FAULT_SUM) {
    BQ_Read_Register(FAULT_UV, FAULT_UV_SIZE, &data);
    bq_error.fault_uv = data & 0xFFFF;
  }
  if (faultflags & OV_FAULT_SUM) {
    BQ_Read_Register(FAULT_OV, FAULT_OV_SIZE, &data);
    bq_error.fault_ov = data & 0xFFFF;
  }
  if (faultflags & AUXUV_FAULT_SUM) {
    BQ_Read_Register(FAULT_AUX, FAULT_AUX_SIZE, &data);
    bq_error.fault_aux_uv = (data & 0xFF) >> 8;
  }
  if (faultflags & AUXOV_FAULT_SUM) {
    BQ_Read_Register(FAULT_AUX, FAULT_AUX_SIZE, &data);
    bq_error.fault_aux_ov = data & 0xFF;
  }
  if (faultflags & CMPUV_FAULT_SUM) {
    BQ_Read_Register(FAULT_2UV, FAULT_2UV_SIZE, &data);
    bq_error.fault_cmp_uv = data & 0xFFFF;
  }
  if (faultflags & CMPOV_FAULT_SUM) {
    BQ_Read_Register(FAULT_2OV, FAULT_2OV_SIZE, &data);
    bq_error.fault_cmp_ov = data & 0xFFFF;
  }
  if (faultflags & COMM_FAULT_SUM) {
    BQ_Read_Register(FAULT_COM, FAULT_COM_SIZE, &data);
    bq_error.fault_comm = data & 0xFFFF;
  }
  if (faultflags & SYS_FAULT_SUM) {
    BQ_Read_Register(FAULT_SYS, FAULT_SYS_SIZE, &data);
    bq_error.fault_sys = data & 0xFF;
  }
  if (faultflags & CHIP_FAULT_SUM) {
    BQ_Read_Register(FAULT_DEV, FAULT_DEV_SIZE, &data);
    bq_error.fault_dev = data & 0xFFFF;
  }
  if (faultflags & GPI_FAULT_SUM) {
    BQ_Read_Register(FAULT_GPI, FAULT_GPI_SIZE, &data);
    bq_error.fault_gpi = data & 0xFF;
  }
}

void afe_clear_all_faults() {
  BQ_Write_Register(FAULT_SUM, FAULT_SUM_SIZE, 0xFFC0); // Clear all Faults
  bq_status = BQ_STDBY;
  HAL_Delay(1);
  afe_check_faults();
  if (bq_status == BQ_STDBY) {
    bq_status = BQ_RDY;
  }
}

void afe_init_fault_thresholds() {

  BQ_Write_Register(FO_CTRL, FO_CTRL_SIZE,
                    0xC3C0); // Include UV Fault OV Fault COMM SYS CHIP GPI
                             // Faults to Fault Output

  BQ_Write_Register(CELL_UV, CELL_UV_SIZE, (CELL_UV_THRESHOLD & 0x03));
  BQ_Write_Register(CELL_OV, CELL_OV_SIZE, (CELL_OV_THRESHOLD & 0x03));
}

void afe_update_Checksum() {
  uint32_t checksum = 0;
  BQ_Read_Register(CSUM_RSLT, CSUM_RSLT_SIZE, &checksum);
  BQ_Write_Register(CSUM, CSUM_SIZE, checksum);
}

void afe_config_measurement_channels() {
  uint16_t cellmask = 0b1111111111;
  uint32_t channelmask = cellmask << 16;
  BQ_Write_Register(NCHAN, NCHAN_SIZE, N_CELLS);
  uint32_t channels = 0b1111111111 << 16;
  BQ_Write_Register(CHANNELS, CHANNELS_SIZE, channels);
  BQ_Write_Register(OVERSMPL, OVERSMPL_SIZE,
                    0xFA); // Oversampling enabled with 4 samples as average
}

void afe_config_communication() {
  BQ_Write_Register(
      COMCONFIG, COMCONFIG_SIZE,
      (1 << 12) |
          (1 << 7)); // Enables UART Transceiver Diables Differential UART
}

void afe_config_gpios() { BQ_Write_Register(GPIO_DIR, GPIO_DIR_SIZE, 0x01); }

void afe_activate_LED() { BQ_Write_Register(GPIO_OUT, GPIO_OUT_SIZE, 0x01); }

void afe_config_balancing() {
  BQ_Write_Register(CBCONFIG, CBCONFIG_SIZE, 0x10);
}

void afe_balance_channels(uint16_t channelstobalance) {
  BQ_Write_Register(CBENBL, CBENBL_SIZE, channelstobalance);
}