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

#include "ADBMS_LL_Driver.h"

#define INITIAL_COMMAND_PEC        0x0010
#define INITIAL_DATA_PEC           0x0010 
#define ADBMS_SPI_TIMEOUT          100 // Timeout in ms
#warning ask about the timeout value

SPI_HandleTypeDef* adbmsspi;

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

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

uint8 calculateCommandPEC(uint8_t* data, uint8_t datalen) {
  uint16 currentpec = INITIAL_COMMAND_PEC;
  if (datalen >= 3) {
    for (int i = 0; i < (datalen - 2); i++) {
      for (int n = 0; n < 8; n++) {
          const uint8 din = data[i] << (n);
        currentpec = updateCommandPEC(currentpec, din);
      }
    }

    data[datalen - 2] = (currentpec >> 7) & 0xFF;
    data[datalen - 1] = (currentpec << 1) & 0xFF;
    return 0;
  } else {
    return 1;
  }
}

uint8 checkCommandPEC(uint8* data, uint8 datalen) {
  if (datalen <= 3) {
    return 255;
  }

  uint16 currentpec = INITIAL_COMMAND_PEC;

  for (int i = 0; i < (datalen - 2); i++) {
    for (int n = 0; n < 8; n++) {
        const uint8 din = data[i] << (n);
      currentpec = updateCommandPEC(currentpec, din);
    }
  }

  const uint8 pechigh = (currentpec >> 7) & 0xFF;
  const uint8 peclow = (currentpec << 1) & 0xFF;

  if ((pechigh == data[datalen - 2]) && (peclow == data[datalen - 1])) {
    return 0;
  }

  return 1;
}

uint16 updateCommandPEC(uint16 currentPEC, uint8 din) {
  din = (din >> 7) & 0x01;
  const uint8 in0 = din ^ ((currentPEC >> 14) & 0x01);
  const uint8 in3 = in0 ^ ((currentPEC >> 2) & 0x01);
  const uint8 in4 = in0 ^ ((currentPEC >> 3) & 0x01);
  const uint8 in7 = in0 ^ ((currentPEC >> 6) & 0x01);
  const uint8 in8 = in0 ^ ((currentPEC >> 7) & 0x01);
  const uint8 in10 = in0 ^ ((currentPEC >> 9) & 0x01);
  const uint8 in14 = in0 ^ ((currentPEC >> 13) & 0x01);

  uint16 newPEC = 0;

  newPEC |= in14 << 14;
  newPEC |= (currentPEC & (0x01 << 12)) << 1;
  newPEC |= (currentPEC & (0x01 << 11)) << 1;
  newPEC |= (currentPEC & (0x01 << 10)) << 1;
  newPEC |= in10 << 10;
  newPEC |= (currentPEC & (0x01 << 8)) << 1;
  newPEC |= in8 << 8;
  newPEC |= in7 << 7;
  newPEC |= (currentPEC & (0x01 << 5)) << 1;
  newPEC |= (currentPEC & (0x01 << 4)) << 1;
  newPEC |= in4 << 4;
  newPEC |= in3 << 3;
  newPEC |= (currentPEC & (0x01 << 1)) << 1;
  newPEC |= (currentPEC & (0x01)) << 1;
  newPEC |= in0;

  return newPEC;
}

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

uint16_t pec10_calc(bool rx_cmd, int len, const uint8_t* data) {
  uint16_t remainder = 16;       /* PEC_SEED;   0000010000 */
  const uint16_t polynom = 0x8F; /* x10 + x7 + x3 + x2 + x + 1 <- the CRC15 polynomial
                              100 1000 1111   48F */

  /* Perform modulo-2 division, a byte at a time. */
  for (uint8_t pbyte = 0; pbyte < len; ++pbyte) {
    /* Bring the next byte into the remainder. */
    remainder ^= (uint16_t)(data[pbyte] << 2);
    /* Perform modulo-2 division, a bit at a time.*/
    for (uint8_t bit_ = 8; bit_ > 0; --bit_) {
      /* Try to divide the current data bit. */
      if ((remainder & 0x200) >
          0) // equivalent to remainder & 2^14 simply check for MSB
      {
        remainder = (uint16_t)((remainder << 1));
        remainder = (uint16_t)(remainder ^ polynom);
      } else {
        remainder = (uint16_t)(remainder << 1);
      }
    }
  }
  if (rx_cmd == true) {
    remainder ^= (uint16_t)((data[len] & 0xFC) << 2);
    /* Perform modulo-2 division, a bit at a time */
    for (uint8_t bit_ = 6; bit_ > 0; --bit_) {
      /* Try to divide the current data bit */
      if ((remainder & 0x200) >
          0) // equivalent to remainder & 2^14 simply check for MSB
      {
        remainder = (uint16_t)((remainder << 1));
        remainder = (uint16_t)(remainder ^ polynom);
      } else {
        remainder = (uint16_t)((remainder << 1));
      }
    }
  }
  return ((uint16_t)(remainder & 0x3FF));
}

typedef uint16_t crc;
crc F_CRC_CalculaCheckSum(uint8_t const AF_Datos[], uint16_t VF_nBytes);

uint8 calculateDataPEC(uint8_t* data, uint8_t datalen) {

  if (datalen >= 3) {

      const crc currentpec = pec10_calc(true, datalen - 2, data) & 0x3FF; // mask to 10 bits

    // memory layout is [[zeroes], PEC[9:8]], [PEC[7:0]]
    data[datalen - 2] = (currentpec >> 8) & 0xFF;
    data[datalen - 1] = currentpec & 0xFF;

    return 0;
  } else {
    return 1;
  }
}

uint8 checkDataPEC(uint8* data, uint8 len) {
  if (len <= 2) {
    return 255;
  }

  const crc currentpec = F_CRC_CalculaCheckSum(data, len);

  return (currentpec == 0) ? 0 : 1;
}


static crc F_CRC_ObtenValorDeTabla(uint8_t VP_Pos_Tabla) {
  crc VP_CRCTableValue = 0;
  uint8_t VP_Pos_bit = 0;

  VP_CRCTableValue = ((crc)(VP_Pos_Tabla)) << (10 - 8);

  for (VP_Pos_bit = 0; VP_Pos_bit < 8; VP_Pos_bit++) {
    if (VP_CRCTableValue & (((crc)1) << (10 - 1))) {
      VP_CRCTableValue = (VP_CRCTableValue << 1) ^ 0x8F;
    } else {
      VP_CRCTableValue = (VP_CRCTableValue << 1);
    }
  }
  return ((VP_CRCTableValue));
}
crc F_CRC_CalculaCheckSum(uint8_t const AF_Datos[], uint16_t VF_nBytes) {
  crc VP_CRCTableValue = 16;
  int16_t VP_bytes = 0;

  for (VP_bytes = 0; VP_bytes < VF_nBytes; VP_bytes++) {

    VP_CRCTableValue = (VP_CRCTableValue << 8) ^
                       F_CRC_ObtenValorDeTabla(
                           ((uint8_t)((VP_CRCTableValue >> (10 - 8)) & 0xFF)) ^
                           AF_Datos[VP_bytes]);
  }

  if ((8 * sizeof(crc)) > 10) {
    VP_CRCTableValue = VP_CRCTableValue & ((((crc)(1)) << 10) - 1);
  }

  return (VP_CRCTableValue ^ 0x0000);
}

uint16 updateDataPEC(uint16 currentPEC, uint8 din) {
  din = (din >> 7) & 0x01;
  const uint8 in0 = din ^ ((currentPEC >> 9) & 0x01);
  const uint8 in2 = in0 ^ ((currentPEC >> 1) & 0x01);
  const uint8 in3 = in0 ^ ((currentPEC >> 2) & 0x01);
  const uint8 in7 = in0 ^ ((currentPEC >> 6) & 0x01);

  uint16 newPEC = 0;

  newPEC |= (currentPEC & (0x01 << 8)) << 1;
  newPEC |= (currentPEC & (0x01 << 7)) << 1;
  newPEC |= in7 << 7;
  newPEC |= (currentPEC & (0x01 << 5)) << 1;
  newPEC |= (currentPEC & (0x01 << 4)) << 1;
  newPEC |= in3 << 3;
  newPEC |= in2 << 2;
  newPEC |= (currentPEC & (0x01)) << 1;
  newPEC |= in0;

  return newPEC;
}

uint8 writeCMD(uint16 command, const uint8 * args, uint8 arglen) {
  uint8 ret;
  if (arglen > 0) {
    uint8 buffer[6 + arglen]; //command + PEC (2 bytes) + data + DPEC (2 bytes)
    buffer[0] = (command >> 8) & 0xFF;
    buffer[1] = (command) & 0xFF;

    calculateCommandPEC(buffer, 4);

    for (uint8 i = 0; i < arglen; i++) {
      buffer[4 + i] = args[i];
    }

    calculateDataPEC(&buffer[4], arglen + 2); //DPEC is calculated over the data, not the command, and placed at the end of the data

    mcuAdbmsCSLow();
    ret = mcuSPITransmit(buffer, 6 + arglen);
    mcuAdbmsCSHigh();
  } else {
    uint8 buffer[4];
    buffer[0] = (command >> 8) & 0xFF;
    buffer[1] = (command) & 0xFF;
    calculateCommandPEC(buffer, 4);

    mcuAdbmsCSLow();

    ret = mcuSPITransmit(buffer, 4);

    mcuAdbmsCSHigh();
  }

  return ret;
}

uint8 readCMD(uint16 command, uint8 * buffer, uint8 buflen) {
  uint8 txbuffer[6 + buflen];
  uint8 rxbuffer[6 + buflen];

  txbuffer[0] = (command >> 8) & 0xFF;
  txbuffer[1] = (command)&0xFF;
  calculateCommandPEC(txbuffer, 4);

  mcuAdbmsCSLow();
  uint8 status = mcuSPITransmitReceive(rxbuffer, txbuffer, 6 + buflen);
  mcuAdbmsCSHigh();

  if (status != 0) {
    return status;
  }

  for (uint8 i = 0; i < buflen; i++) {
    buffer[i] = rxbuffer[i + 4];
  }

  [[maybe_unused]] uint8 commandCounter = rxbuffer[sizeof(rxbuffer) - 2] & 0xFC; //command counter is bits 7-2
                                                                                 //TODO: check command counter?
                                                                                 
  return checkDataPEC(&rxbuffer[4], buflen + 2);
}

//check poll command - no data PEC sent back
uint8 pollCMD(uint16 command) {
  uint8 txbuffer[5] = {};
  uint8 rxbuffer[5] = {};

  txbuffer[0] = (command >> 8) & 0xFF;
  txbuffer[1] = (command)&0xFF;
  calculateCommandPEC(txbuffer, 4);

  mcuAdbmsCSLow();
  uint8 status = mcuSPITransmitReceive(rxbuffer, txbuffer, 5);
  mcuAdbmsCSHigh();

  if (status != 0) {
    return status;
  }

  return rxbuffer[4]; //last byte will be poll response
}

void mcuAdbmsCSLow() {
  HAL_GPIO_WritePin(CSB_GPIO_Port, CSB_Pin, GPIO_PIN_RESET);
}

void mcuAdbmsCSHigh() {
  HAL_GPIO_WritePin(CSB_GPIO_Port, CSB_Pin, GPIO_PIN_SET);
}

uint8 mcuSPITransmit(uint8* buffer, uint8 buffersize) {
    uint8 rxbuf[buffersize];
    const HAL_StatusTypeDef status = HAL_SPI_TransmitReceive(adbmsspi, buffer, rxbuf, buffersize,
                                                             ADBMS_SPI_TIMEOUT);
  __HAL_SPI_CLEAR_OVRFLAG(adbmsspi);
  return status;
}

uint8 mcuSPIReceive(uint8* buffer, uint8 buffersize) {
    const HAL_StatusTypeDef status = HAL_SPI_Receive(adbmsspi, buffer, buffersize, ADBMS_SPI_TIMEOUT);
  return status;
}

uint8 mcuSPITransmitReceive(uint8* rxbuffer, uint8* txbuffer,
                            uint8 buffersize) {
    const HAL_StatusTypeDef status = HAL_SPI_TransmitReceive(adbmsspi, txbuffer, rxbuffer, buffersize,
                                                             ADBMS_SPI_TIMEOUT);
  return status;
}

inline void mcuDelay(uint16 delay) { HAL_Delay(delay); }