/*
 * ADBMS_LL_Driver.c
 *
 *  Created on: 05.06.2022
 *      Author: max
 */

#include "ADBMS_LL_Driver.h"
#include "config_ADBMS6830.h"
#include "stm32h7xx_hal.h"
#include <stdint.h>
#include <strings.h>

#define SWO_LOG_PREFIX "[ADBMS] "
#include "swo_log.h"

#define INITIAL_COMMAND_PEC        0x0010
#define INITIAL_DATA_PEC           0x0010 

// CRC polynomial constants
#define CRC15_POLY 0xC599
#define CRC10_POLY 0x8F
#define CRC15_REMAINDER_MASK 0x4000
#define CRC10_REMAINDER_MASK 0x200
#define CRC10_RESULT_MASK 0x3FF

SPI_HandleTypeDef* adbmsspi;

uint8_t adbmsDriverInit(SPI_HandleTypeDef* hspi) {
  mcuAdbmsCSLow();
  HAL_Delay(1);
  mcuAdbmsCSHigh();
  adbmsspi = hspi;
  return 0;
}

static HAL_StatusTypeDef mcuSPITransmit(uint8_t* buffer, uint8_t buffersize) {
    HAL_StatusTypeDef status;
    status = HAL_SPI_Transmit(adbmsspi, buffer, buffersize, ADBMS_SPI_TIMEOUT);
    __HAL_SPI_CLEAR_OVRFLAG(adbmsspi);
    return status;
}

static HAL_StatusTypeDef mcuSPIReceive(uint8_t* buffer, uint8_t buffersize) {
    return HAL_SPI_Receive(adbmsspi, buffer, buffersize, ADBMS_SPI_TIMEOUT);
}

static HAL_StatusTypeDef mcuSPITransmitReceive(uint8_t* rxbuffer, uint8_t* txbuffer, uint8_t buffersize) {
    return HAL_SPI_TransmitReceive(adbmsspi, txbuffer, rxbuffer, buffersize, ADBMS_SPI_TIMEOUT);
}

//command PEC calculation
//CRC-15
//x^15 + x^14 + x^10 + x^8 + x^7 + x^4 + x^3 + 1

static uint16_t computeCRC15(const uint8_t* data, size_t length) {
    uint16_t remainder = INITIAL_COMMAND_PEC;
    for (size_t i = 0; i < length; i++) {
        remainder ^= (data[i] << 7);
        for (int b = 0; b < 8; b++) {
            if (remainder & CRC15_REMAINDER_MASK) {
                remainder = (uint16_t)((remainder << 1) ^ CRC15_POLY);
            } else {
                remainder <<= 1;
            }
        }
    }
    return remainder;
}

static uint8_t calculateCommandPEC(uint8_t* data, uint8_t datalen) {
    if (datalen < 3) { return 1; }
    uint16_t pec = computeCRC15(data, datalen - 2);
    data[datalen - 2] = (uint8_t)((pec >> 7) & 0xFF);
    data[datalen - 1] = (uint8_t)((pec << 1) & 0xFF);
    return 0;
}

static uint8_t checkCommandPEC(uint8_t* data, uint8_t datalen) {
    if (datalen <= 3) { return 255; }
    uint16_t pec = computeCRC15(data, datalen - 2);
    uint8_t pech = (uint8_t)((pec >> 7) & 0xFF);
    uint8_t pecl = (uint8_t)((pec << 1) & 0xFF);
    return ((pech == data[datalen - 2]) && (pecl == data[datalen - 1])) ? 0 : 1;
}

//data PEC calculation
//CRC-10
//x^10 + x^7 + x^3 + x^2 + x + 1

static uint16_t computeCRC10(const uint8_t* data, size_t length, bool rx_cmd) {
  uint16_t remainder = INITIAL_DATA_PEC;

  for (size_t i = 0; i < length; i++) {
      remainder ^= (uint16_t)(data[i] << 2);
      for (int b = 0; b < 8; b++) {
          if (remainder & CRC10_REMAINDER_MASK) {
              remainder = (uint16_t)((remainder << 1) ^ CRC10_POLY);
          } else {
              remainder <<= 1;
          }
      }
  }

  // Handle last bits of the last byte if rx_cmd is true
  if (rx_cmd) { 
      remainder ^= (uint16_t)((data[length] & 0xFC) << 2);
      for (int b = 0; b < 6; b++) {
          if (remainder & CRC10_REMAINDER_MASK) {
              remainder = (uint16_t)((remainder << 1) ^ CRC10_POLY);
          } else {
              remainder <<= 1;
          }
      }
  }
  return (uint16_t)(remainder & CRC10_RESULT_MASK);
}

static uint8_t calculateDataPEC(uint8_t* data, uint8_t datalen) {
    if (datalen < 3) { return 1; }
    uint16_t crcVal = computeCRC10(data, datalen - 2, true);
    data[datalen - 2] = (uint8_t)((crcVal >> 8) & 0xFF);
    data[datalen - 1] = (uint8_t)(crcVal & 0xFF);
    return 0;
}

static uint8_t checkDataPEC(uint8_t* data, uint8_t len) {
    if (len <= 2) { return 255; }
    // Zero remainder means a valid CRC.
    return (computeCRC10(data, len, false) == 0) ? 0 : 1;
}

static const char* const HAL_Statuses[] = {"HAL_OK", "HAL_ERROR", "HAL_BUSY", "HAL_TIMEOUT"};

static void print_spi_details() {
  uint32_t spi_error = HAL_SPI_GetError(adbmsspi);

  typedef struct {
    uint32_t mask;
    const char *label;
  } SPIError;

  const SPIError errors[] = {
    {HAL_SPI_ERROR_NONE,            "NONE"},
    {HAL_SPI_ERROR_MODF,            "MODF"},
    {HAL_SPI_ERROR_CRC,             "CRC"},
    {HAL_SPI_ERROR_OVR,             "OVR"},
    {HAL_SPI_ERROR_FRE,             "FRE"},
    {HAL_SPI_ERROR_DMA,             "DMA"},
    {HAL_SPI_ERROR_FLAG,            "FLAG"},
    {HAL_SPI_ERROR_ABORT,           "ABORT"},
    {HAL_SPI_ERROR_UDR,             "UDR"},
    {HAL_SPI_ERROR_TIMEOUT,         "TIMEOUT"},
    {HAL_SPI_ERROR_UNKNOW,          "UNKNOWN"},
    {HAL_SPI_ERROR_NOT_SUPPORTED,   "NOT_SUPPORTED"},
    {HAL_SPI_ERROR_RELOAD,          "RELOAD"},
#ifdef HAL_SPI_ERROR_INVALID_CALLBACK
    {HAL_SPI_ERROR_INVALID_CALLBACK,"INVALID_CALLBACK"},
#endif
  };

  constexpr size_t numErrors = sizeof(errors) / sizeof(errors[0]);
  for (size_t i = 0; i < numErrors; i++) {
    if (spi_error & errors[i].mask) {
      debug_log_cont(LOG_LEVEL_ERROR, "%s ", errors[i].label);
    }
  }
}

HAL_StatusTypeDef ___writeCMD(uint16_t command, uint8_t * args, size_t arglen) {
  HAL_StatusTypeDef ret;
  if (arglen > 0) {
    args[0] = (command >> 8) & 0xFF;
    args[1] = (command) & 0xFF;

    if (DEBUG_CHANNEL_ENABLED(LOG_LEVEL_NOISY)) {
      debug_log(LOG_LEVEL_NOISY, "%lu W | %02X %02X ", HAL_GetTick(), args[0], args[1]);

      //print out data bytes
      if (arglen > 0) {
        for (size_t i = 0; i < N_BMS; i++) {
          debug_log_cont(LOG_LEVEL_NOISY, "%d: ", i);
          for (size_t j = 0; j < arglen; j++) {
            debug_log_cont(LOG_LEVEL_NOISY, "%02X ", args[BUFFER_BMS_OFFSET(i, arglen) + j]);
          }
        }
      }
    }

    calculateCommandPEC(args, 4);

    for (size_t i = 0; i < N_BMS; i++) {
      args[BUFFER_BMS_OFFSET(i, arglen) + arglen] = 0; // zero out the PEC
      args[BUFFER_BMS_OFFSET(i, arglen) + arglen + 1] = 0; 
      calculateDataPEC(&args[4 + (i * (arglen + 2))], arglen + 2); //DPEC is calculated over the data, not the command, and placed at the end of the data
    }
    
    mcuAdbmsCSLow();
    ret = mcuSPITransmit(args, CMD_BUFFER_SIZE(arglen));
    mcuAdbmsCSHigh();
  } else {
    args[0] = (command >> 8) & 0xFF;
    args[1] = (command) & 0xFF;
    calculateCommandPEC(args, 4);

    mcuAdbmsCSLow();
    ret = mcuSPITransmit(args, 4);
    mcuAdbmsCSHigh();
  }

  if (ret != HAL_OK) {
    debug_log(LOG_LEVEL_ERROR, "STM32 SPI HAL returned error %s", HAL_Statuses[ret]);
    debug_log(LOG_LEVEL_ERROR, "SPI error bits: ");
    print_spi_details();
  }

  return ret;
}

HAL_StatusTypeDef ___readCMD(uint16_t command, uint8_t * buffer, size_t arglen) {
  buffer[0] = (command >> 8) & 0xFF;
  buffer[1] = (command)&0xFF;
  calculateCommandPEC(buffer, 4);

  mcuAdbmsCSLow();
  HAL_StatusTypeDef status = mcuSPITransmitReceive(buffer, buffer, CMD_BUFFER_SIZE(arglen));
  mcuAdbmsCSHigh();

  if (status != HAL_OK) {
    debug_log(LOG_LEVEL_ERROR, "STM32 SPI HAL returned error %s", HAL_Statuses[status]);
    debug_log(LOG_LEVEL_ERROR, "SPI error bits: ");
    print_spi_details();
    return status;
  }

  //[[maybe_unused]] uint8_t commandCounter = buffer[sizeof(buffer) - 2] & 0xFC; //command counter is bits 7-2
                                                                                 //TODO: check command counter?

  if (DEBUG_CHANNEL_ENABLED(LOG_LEVEL_NOISY)) {
    debug_log(LOG_LEVEL_NOISY, "%lu R | %02X %02X ", HAL_GetTick(), command >> 8, command & 0xFF);

    //print out data bytes
    if (arglen > 0) {
      for (size_t i = 0; i < N_BMS; i++) {
        debug_log_cont(LOG_LEVEL_NOISY, "%d: ", i);
        for (size_t j = 0; j < arglen; j++) {
          debug_log_cont(LOG_LEVEL_NOISY, "%02X ", buffer[BUFFER_BMS_OFFSET(i, arglen) + j]);
        }
      }
    }
  }

  if (arglen == 0) {
    return HAL_OK; //no data to check
  }

  //check data PEC
  for (size_t i = 0; i < N_BMS; i++) {
    size_t offset = BUFFER_BMS_OFFSET(i, arglen);
    if (checkDataPEC(&buffer[offset], arglen + 2) != 0) {
      debug_log(LOG_LEVEL_ERROR, "Invalid data PEC when reading BMS %d", i);
      debug_log(LOG_LEVEL_ERROR, "Received: ");
      for (size_t j = 0; j < arglen + 2; j++) {
        debug_log_cont(LOG_LEVEL_ERROR, "%02X ", buffer[offset + j]);
      }
      debug_log_cont(LOG_LEVEL_ERROR, "| %02X %02X ", buffer[offset + arglen], buffer[offset + arglen + 1]); //print out the DPEC
      debug_log(LOG_LEVEL_ERROR, "          DATA ^");
      //print out spaces until start of DPEC
      for (size_t j = 0; j < arglen - 1; j++) {
        debug_log_cont(LOG_LEVEL_ERROR, (arglen < 2) ? "" : "^^^");
      }
      debug_log_cont(LOG_LEVEL_ERROR, "^^ ");
      debug_log_cont(LOG_LEVEL_ERROR, "  PEC ^");
      return HAL_ERROR;
    }
  }

  return HAL_OK;
}

//check poll command - no data PEC sent back, waitTime is in SPI clock cycles
HAL_StatusTypeDef __pollCMD(uint16_t command, uint8_t waitTime) {
  uint8_t buffer[4 + (waitTime / 8) + 1] = {}; //8 cycles per byte, +1 as we round up 
  buffer[0] = (command >> 8) & 0xFF;
  buffer[1] = (command) & 0xFF;
  calculateCommandPEC(buffer, 4);

  mcuAdbmsCSLow();
  HAL_StatusTypeDef status = mcuSPITransmitReceive(buffer, buffer, sizeof buffer);
  mcuAdbmsCSHigh();

  if (status != HAL_OK) {
    debug_log(LOG_LEVEL_ERROR, "STM32 SPI HAL returned error %s", HAL_Statuses[status]);
    debug_log(LOG_LEVEL_ERROR, "SPI error bits: ");
    print_spi_details();
    return status;
  }

  return ((buffer[sizeof buffer] & 0x0F) == 0x0) ? HAL_BUSY : HAL_OK; //SDO goes high when data is ready
}