#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;
  shunt_data.last_current_message = 0;
}

void shunt_check() {
  int is_error = 0;
  if (HAL_GetTick() - shunt_data.last_message > SHUNT_TIMEOUT) {
    is_error = 1;
    can_send_error(TS_ERRORKIND_SHUNT_TIMEOUT, 0);
  } else if (shunt_data.current >= SHUNT_THRESH_OVERCURRENT) {
    is_error = 1;
    can_send_error(TS_ERRORKIND_SHUNT_OVERTEMP, 0);
  } else if (shunt_data.busbartemp >= SHUNT_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) {
  shunt_data.last_message = HAL_GetTick();

  // 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;
    if (shunt_data.last_current_message > 0) {
      uint32_t now = HAL_GetTick();
      float dt = (now - shunt_data.last_current_message) * 0.001f;
      shunt_data.current_counter += shunt_data.current * dt;
    }
    shunt_data.last_current_message = HAL_GetTick();
    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:
    // TODO: Use this when we get the shunt to emit current counter data (the
    // shunt apparently emits As, not mAs)

    // shunt_data.current_counter = result * 1000;
    break;
  case CAN_ID_SHUNT_ENERGY_COUNTER:
    shunt_data.energy = result;
    break;
  }
}