/*
 * AMS_CAN.c
 *
 *  Created on: Mar 19, 2022
 *      Author: jasper
 */

#include "AMS_CAN.h"

#include "ADBMS_Abstraction.h"

#include "AMS_HighLevel.h"
#include "TMP1075.h"
#include "common_defs.h"
#include "eeprom.h"
#include "errors.h"
#include "main.h"
#include "stm32f3xx.h"
#include "stm32f3xx_hal.h"
#include "stm32f3xx_hal_can.h"

#include "can-halal.h"
#include "stm32f3xx_hal_gpio.h"

#include <stdint.h>
#include <string.h>
#include <stdbool.h>

#define CAN_ID_SLAVE_PANIC       0x009
#define CAN_ID_SLAVE_STATUS_BASE 0x080
#define CAN_ID_SLAVE_LOG_BASE    0x600

rx_status_frame rxstate = {};
volatile uint8_t canmsg_received = 0;
void ams_can_init(CAN_HandleTypeDef* hcan) { ftcan_init(hcan); }

#define ITER_COUNT 10
static uint8_t count = 0;
static bool isOn = false;

void ams_can_send_status() {
  if (count == ITER_COUNT) {
    HAL_GPIO_WritePin(STATUS_LED_G_GPIO_Port, STATUS_LED_G_Pin, isOn ? GPIO_PIN_SET : GPIO_PIN_RESET);

    count = 0;
    isOn = !isOn;
  } else {
    count++;
  }

  static uint8_t data[8];
  int error = error_data.error_sources != 0;
  data[0] = eeprom_config.id | (error << 7);
  data[1] = stateofcharge;
  uint8_t* ptr = &data[2];
  uint16_t min_volt = 0xFFFF;
  uint16_t max_volt = 0;
  for (size_t i = 0; i < numberofCells; i++) {
    if (module.cellVoltages[i] < min_volt) {
      min_volt = module.cellVoltages[i];
    }
    if (module.cellVoltages[i] > max_volt) {
      max_volt = module.cellVoltages[i];
    }
  }
  ptr = ftcan_marshal_unsigned(ptr, min_volt, 2);
  ptr = ftcan_marshal_unsigned(ptr, max_volt, 2);
  int16_t max_temp = -0x8000;
  for (size_t i = 0; i < N_TEMP_SENSORS; i++) {
    if (tmp1075_temps[i] > max_temp &&
        (tmp1075_failed_sensors & (1 << i)) == 0) {
      max_temp = tmp1075_temps[i];
    }
  }
  ftcan_marshal_unsigned(ptr, max_temp, 2);
  uint16_t id = CAN_ID_SLAVE_STATUS_BASE | eeprom_config.id;
  ftcan_transmit(id, data, sizeof(data));
}

void ams_can_send_error() {
  static uint8_t data[6];
  data[0] = eeprom_config.id;
  data[1] = error_data.data_kind;
  memcpy(&data[2], error_data.data, 4);
  ftcan_transmit(CAN_ID_SLAVE_PANIC, data, sizeof(data));
}

void ams_can_send_log() {
  static uint8_t call_count = 0;
  static uint8_t data[8];

  uint16_t can_addr =
      CAN_ID_SLAVE_LOG_BASE | (eeprom_config.id << 4) | call_count;
  uint8_t* ptr = &data[0];

  if (call_count < N_CELLS / 4) {
    for (size_t i = 0; i < 4; i++) {
      size_t offset = call_count * 4;
      ptr = ftcan_marshal_unsigned(ptr, module.cellVoltages[offset + i], 2);
    }
    ftcan_transmit(can_addr, data, sizeof(data));
  } else if (call_count == N_CELLS / 4) {
    // Send last cell &  failed temperature sensors
    ptr = ftcan_marshal_unsigned(ptr, module.cellVoltages[N_CELLS - 1], 2);
    ptr = ftcan_marshal_unsigned(ptr, tmp1075_failed_sensors, 4);
    ftcan_transmit(can_addr, data, 6);
  } else {
    size_t offset = (call_count - N_CELLS / 4 - 1) * 8;
    for (size_t i = 0; i < 8; i++) {
      data[i] = tmp1075_temps[offset + i] >> 4;
    }
    ftcan_transmit(can_addr, data, sizeof(data));
  }
  call_count = (call_count + 1) % (N_CELLS / 4 + 1 + N_TEMP_SENSORS / 8);
}