#include "imd_monitoring.h"

#include "main.h"

#define FREQ_TIMER 1000 // Hz

#define FREQ_TOLERANCE 1     // Hz
#define FREQ_NORMAL 10       // Hz
#define FREQ_UNDERVOLTAGE 20 // Hz
#define FREQ_SST 30          // Hz
#define FREQ_DEV_ERROR 40    // Hz
#define FREQ_GND_FAULT 50    // Hz

#define RISO_MIN_DUTY_CYCLE 8 // %
#define RISO_MAX 50000        // kOhm

#define PWM_TIMEOUT 200 // ms

IMDData imd_data;

static TIM_HandleTypeDef *htim;

void imd_init(TIM_HandleTypeDef *handle) {
  htim = handle;
  HAL_TIM_IC_Start_IT(htim, TIM_CHANNEL_1);
  HAL_TIM_IC_Start(htim, TIM_CHANNEL_2);

  imd_data.state = IMD_STATE_UNKNOWN;
}

void HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef *handle) {
  if (handle != htim || htim->Channel != HAL_TIM_ACTIVE_CHANNEL_1) {
    return;
  }
  uint32_t period = HAL_TIM_ReadCapturedValue(htim, TIM_CHANNEL_1);
  if (period == 0) {
    // First edge, ignore
    return;
  }
  imd_data.last_high = HAL_GetTick();

  imd_data.freq = FREQ_TIMER / period;
  uint32_t high_time = HAL_TIM_ReadCapturedValue(htim, TIM_CHANNEL_2);
  imd_data.duty_cycle = (100 * high_time) / period;

  // Check PWM frequency for state determination
  if (imd_data.freq > FREQ_NORMAL - FREQ_TOLERANCE &&
      imd_data.freq < FREQ_NORMAL + FREQ_TOLERANCE) {
    imd_data.state = IMD_STATE_NORMAL;
  } else if (imd_data.freq > FREQ_UNDERVOLTAGE - FREQ_TOLERANCE &&
             imd_data.freq < FREQ_UNDERVOLTAGE + FREQ_TOLERANCE) {
    imd_data.state = IMD_STATE_UNDERVOLTAGE;
  } else if (imd_data.freq > FREQ_SST - FREQ_TOLERANCE &&
             imd_data.freq < FREQ_SST + FREQ_TOLERANCE) {
    imd_data.state = IMD_STATE_SST;
  } else if (imd_data.freq > FREQ_DEV_ERROR - FREQ_TOLERANCE &&
             imd_data.freq < FREQ_DEV_ERROR + FREQ_TOLERANCE) {
    imd_data.state = IMD_STATE_DEV_ERROR;
  } else if (imd_data.freq > FREQ_GND_FAULT - FREQ_TOLERANCE &&
             imd_data.freq < FREQ_GND_FAULT + FREQ_TOLERANCE) {
    imd_data.state = IMD_STATE_GND_FAULT;
  } else {
    imd_data.state = IMD_STATE_UNKNOWN;
  }

  // Calculate R_iso
  if (imd_data.state == IMD_STATE_NORMAL ||
      imd_data.state == IMD_STATE_UNDERVOLTAGE) {
    if (imd_data.duty_cycle < RISO_MIN_DUTY_CYCLE) {
      imd_data.r_iso = RISO_MAX;
    } else {
      imd_data.r_iso = (90 * 1200) / (imd_data.duty_cycle - 5) - 1200;
    }
  }
}

void imd_update() {
  imd_data.ok = HAL_GPIO_ReadPin(IMD_OK_GPIO_Port, IMD_OK_Pin);
  if (HAL_GetTick() - imd_data.last_high > PWM_TIMEOUT) {
    if (HAL_GPIO_ReadPin(IMD_M_GPIO_Port, IMD_M_Pin) == GPIO_PIN_SET) {
      imd_data.state = IMD_STATE_SHORTCIRCUIT_SUPPLY;
    } else {
      imd_data.state = IMD_STATE_SHORTCIRCUIT_GND;
    }
  }
}