#include "shunt_monitoring.h"

#include <string.h>

#include "can.h"
#include "ts_state_machine.h"
#include "util.h"

#include "can-halal.h"

ShuntData shunt_data;

void shunt_init() {
  shunt_data.current = 0;
  shunt_data.voltage_veh = 0;
  shunt_data.voltage_bat = 0;
  shunt_data.voltage3 = 0;
  shunt_data.busbartemp = 0;
  shunt_data.power = 0;
  shunt_data.energy = 0;
  shunt_data.current_counter = 0;
  shunt_data.last_message = 0;
}

void shunt_check() {
  int is_error = 0;
  if (shunt_data.current >= THRESH_OVERCURRENT) {
    is_error = 1;
    can_send_error(TS_ERRORKIND_SHUNT_OVERTEMP, 0);
  } else if (shunt_data.busbartemp >= THRESH_OVERTEMP) {
    is_error = 1;
    can_send_error(TS_ERRORKIND_SHUNT_OVERTEMP, 0);
  }
  ts_sm_set_error_source(TS_ERROR_SOURCE_SHUNT, is_error);
}

void shunt_handle_can_msg(uint16_t id, const uint8_t *data) {
  // All result messages contain a big-endian 6-byte integer
  uint64_t result = ftcan_unmarshal_unsigned(&data, 6);

  switch (id) {
  case CAN_ID_SHUNT_CURRENT:
    shunt_data.current = result;
    break;
  case CAN_ID_SHUNT_VOLTAGE1:
    shunt_data.voltage_bat = result;
    break;
  case CAN_ID_SHUNT_VOLTAGE2:
    shunt_data.voltage_veh = result;
    break;
  case CAN_ID_SHUNT_VOLTAGE3:
    shunt_data.voltage3 = result;
    break;
  case CAN_ID_SHUNT_TEMP:
    shunt_data.busbartemp = result;
    break;
  case CAN_ID_SHUNT_POWER:
    shunt_data.power = result;
    break;
  case CAN_ID_SHUNT_CURRENT_COUNTER:
    shunt_data.current_counter = result;
    break;
  case CAN_ID_SHUNT_ENERGY_COUNTER:
    shunt_data.energy = result;
    break;
  }
}