#include <stdint.h>
#include <stdbool.h>
#include "libc.h"
#include "stm32f4xx_hal.h"
#include "defines.h"
#include "config.h"
#include "setup.h"
#include "util.h"
#include "bldc/BLDC_controller.h"      /* BLDC's header file */
#include "bldc/rtwtypes.h"
#include "version.h"
#include "obj/gitversion.h"
#include "comms.h"
#include "drivers/clock.h"
#include "early_init.h"
#include "drivers/i2c_soft.h"
#include "drivers/angle_sensor.h"
#include "boards.h"


//------------------------------------------------------------------------
// Global variables set externally
//------------------------------------------------------------------------
extern TIM_HandleTypeDef htim_left;
extern TIM_HandleTypeDef htim_right;
extern ADC_HandleTypeDef hadc;
extern volatile adc_buf_t adc_buffer;

// Matlab defines - from auto-code generation
//---------------
extern P    rtP_Left;                   /* Block parameters (auto storage) */
extern P    rtP_Right;                  /* Block parameters (auto storage) */
extern ExtY rtY_Left;                   /* External outputs */
extern ExtY rtY_Right;                  /* External outputs */
extern ExtU rtU_Left;                   /* External inputs */
extern ExtU rtU_Right;                  /* External inputs */
//---------------

// TODO: remove both, unneeded. Also func in util.c too
extern int16_t speedAvg;                // Average measured speed
extern int16_t speedAvgAbs;             // Average measured speed in absolute

extern volatile int pwml;               // global variable for pwm left. -1000 to 1000
extern volatile int pwmr;               // global variable for pwm right. -1000 to 1000

extern uint8_t enable_motors;                  // global variable for motor enable

extern int16_t batVoltage;              // global variable for battery voltage

extern int32_t motPosL;
extern int32_t motPosR;

extern board_t board;

//------------------------------------------------------------------------
// Global variables set here in main.c
//------------------------------------------------------------------------
extern volatile uint32_t buzzerTimer;
volatile uint32_t torque_cmd_timeout = 0U;
volatile uint32_t ignition_off_counter = 0U;

uint16_t cnt_press = 0;

int16_t batVoltageCalib;         // global variable for calibrated battery voltage
int16_t board_temp_deg_c;        // global variable for calibrated temperature in degrees Celsius
volatile int16_t cmdL;                    // global variable for Left Command
volatile int16_t cmdR;                    // global variable for Right Command

uint8_t hw_type;                 // type of the board detected(0 - base, 3 - knee)
uint8_t ignition = 0;            // global variable for ignition on SBU2 line
uint8_t charger_connected = 0;   // status of the charger port
uint8_t pkt_idx = 0;             // For CAN msg counter
//------------------------------------------------------------------------
// Local variables
//------------------------------------------------------------------------
static uint32_t tick_prev = 0U;

static uint32_t main_loop_1Hz = 0U;
static uint32_t main_loop_1Hz_runtime = 0U;

static uint32_t main_loop_10Hz = 0U;
static uint32_t main_loop_10Hz_runtime = 0U;

static uint32_t main_loop_100Hz = 0U;
static uint32_t main_loop_100Hz_runtime = 0U;

void __initialize_hardware_early(void) {
  early_initialization();
}

int main(void) {
  HAL_Init();
  HAL_NVIC_SetPriorityGrouping(NVIC_PRIORITYGROUP_4);
  HAL_NVIC_SetPriority(MemoryManagement_IRQn, 0, 0);
  HAL_NVIC_SetPriority(BusFault_IRQn, 0, 0);
  HAL_NVIC_SetPriority(UsageFault_IRQn, 0, 0);
  HAL_NVIC_SetPriority(SVCall_IRQn, 0, 0);
  HAL_NVIC_SetPriority(DebugMonitor_IRQn, 0, 0);
  HAL_NVIC_SetPriority(PendSV_IRQn, 0, 0);
  HAL_NVIC_SetPriority(SysTick_IRQn, 0, 0);

  SystemClock_Config();
  MX_GPIO_Clocks_Init();

  __HAL_RCC_DMA2_CLK_DISABLE();

  board_detect();
  MX_GPIO_Common_Init();
  MX_TIM_Init();
  MX_ADC_Init();
  BLDC_Init();

  HAL_ADC_Start(&hadc);

  if (hw_type == HW_TYPE_BASE) {
    out_enable(POWERSWITCH, true);
    out_enable(IGNITION, ignition);
    out_enable(TRANSCEIVER, true);
    // Loop until button is released, only for base board
    while(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { HAL_Delay(10); }
  } else {
    out_enable(POWERSWITCH, false);
    ignition = 1;
    knee_detected = 1;
  }
  // Reset LEDs on startup
  out_enable(LED_RED, false);
  out_enable(LED_GREEN, false);
  out_enable(LED_BLUE, false);

  llcan_set_speed(board.CAN, 5000, false, false);
  llcan_init(board.CAN);

  SW_I2C_init();
  IMU_soft_init();

  poweronMelody();

  int32_t board_temp_adcFixdt = adc_buffer.temp << 16;  // Fixed-point filter output initialized with current ADC converted to fixed-point
  int16_t board_temp_adcFilt  = adc_buffer.temp;

  uint16_t sensor_angle[2] = { 0 };
  uint16_t hall_angle_offset[2] = { 0 };

  uint16_t unknown_imu_data[6] = { 0 };

  if (hw_type == HW_TYPE_KNEE) {
    angle_sensor_read(sensor_angle); // Initial data to set offsets between angle sensor and hall sensor
    hall_angle_offset[0] = (sensor_angle[0] * ANGLE_TO_DEGREES);
    hall_angle_offset[1] = (sensor_angle[1] * ANGLE_TO_DEGREES);
  }

  while(1) {
    if ((HAL_GetTick() - tick_prev) >= 1) { // 1kHz loop
      // runs at 100Hz
      if ((HAL_GetTick() - (main_loop_100Hz - main_loop_100Hz_runtime)) >= 10) {
        main_loop_100Hz_runtime = HAL_GetTick();

        calcAvgSpeed();

        if (ignition == 0) {
          cmdL = cmdR = 0;
          enable_motors = 0;
        }

        if (!enable_motors || (torque_cmd_timeout > 10)) {
          cmdL = cmdR = 0;
        }

        if (ignition == 1 && enable_motors == 0 && (!rtY_Left.z_errCode && !rtY_Right.z_errCode) && (ABS(cmdL) < 50 && ABS(cmdR) < 50)) {
          beepShort(6); // make 2 beeps indicating the motor enable
          beepShort(4);
          HAL_Delay(100);
          cmdL = cmdR = 0;
          enable_motors = 1; // enable motors
        }

        if (hw_type == HW_TYPE_KNEE) {
          // Safety to stop operation if angle sensor reading failed TODO: adjust sensivity and add lowpass to angle sensor?
          fault_status.left_angle = (ABS((hall_angle_offset[0] + ((motPosL / 15 / GEARBOX_RATIO_LEFT) % 360)) - (sensor_angle[0] * ANGLE_TO_DEGREES)) > 5);
          fault_status.right_angle = (ABS((hall_angle_offset[1] + ((motPosR / 15 / GEARBOX_RATIO_RIGHT) % 360)) - (sensor_angle[1] * ANGLE_TO_DEGREES)) > 5);

          if (fault_status.left_angle || fault_status.right_angle) {
            cmdL = cmdR = 0;
          }
          // Safety to stop movement when reaching dead angles, around 20 and 340 degrees
          if (((sensor_angle[0] < 900) && (cmdL < 0)) || ((sensor_angle[0] > 15500) && (cmdL > 0))) {
            cmdL = 0;
          }
          if (((sensor_angle[1] < 900) && (cmdR < 0)) || ((sensor_angle[1] > 15500) && (cmdR > 0))) {
            cmdR = 0;
          }
        }

        if (ABS(cmdL) < 10) {
          rtP_Left.n_cruiseMotTgt   = 0;
          rtP_Left.b_cruiseCtrlEna  = 1;
        } else {
          rtP_Left.b_cruiseCtrlEna  = 0;
          if (hw_type == HW_TYPE_KNEE) {
            pwml = -CLAMP((int)cmdL, -TRQ_LIMIT_LEFT, TRQ_LIMIT_LEFT);
          } else {
            pwml = CLAMP((int)cmdL, -TORQUE_BASE_MAX, TORQUE_BASE_MAX);
          }
        }
        if (ABS(cmdR) < 10) {
          rtP_Right.n_cruiseMotTgt  = 0;
          rtP_Right.b_cruiseCtrlEna = 1;
        } else {
          rtP_Right.b_cruiseCtrlEna = 0;
          if (hw_type == HW_TYPE_KNEE) {
            pwmr = -CLAMP((int)cmdR, -TRQ_LIMIT_RIGHT, TRQ_LIMIT_RIGHT);
          } else {
            pwmr = -CLAMP((int)cmdR, -TORQUE_BASE_MAX, TORQUE_BASE_MAX);
          }
        }

        if (ignition_off_counter <= IGNITION_OFF_DELAY) {
          // MOTORS_DATA: speed_L(2), speed_R(2), counter(1), checksum(1)
          uint8_t dat[8];
          uint16_t speedL = rtY_Left.n_mot;
          uint16_t speedR = -(rtY_Right.n_mot); // Invert speed sign for the right wheel
          dat[0] = (speedL >> 8U) & 0xFFU;
          dat[1] = speedL & 0xFFU;
          dat[2] = (speedR >> 8U) & 0xFFU;
          dat[3] = speedR & 0xFFU;
          dat[4] = 0;
          dat[5] = 0;
          dat[6] = pkt_idx;
          dat[7] = crc_checksum(dat, 7, crc_poly);
          can_send_msg((0x201U + board.can_addr_offset), ((dat[7] << 24U) | (dat[6] << 16U) | (dat[5]<< 8U) | dat[4]), ((dat[3] << 24U) | (dat[2] << 16U) | (dat[1] << 8U) | dat[0]), 8U);
          ++pkt_idx;
          pkt_idx &= 0xFU;

          //MOTORS_CURRENT: left_pha_ab(2), left_pha_bc(2), right_pha_ab(2), right_pha_bc(2)
          dat[0] = (rtU_Left.i_phaAB >> 8U) & 0xFFU;
          dat[1] = rtU_Left.i_phaAB & 0xFFU;
          dat[2] = (rtU_Left.i_phaBC >> 8U) & 0xFFU;
          dat[3] = rtU_Left.i_phaBC & 0xFFU;
          dat[4] = (rtU_Right.i_phaAB >> 8U) & 0xFFU;
          dat[5] = rtU_Right.i_phaAB & 0xFFU;
          dat[6] = (rtU_Right.i_phaBC >> 8U) & 0xFFU;
          dat[7] = rtU_Right.i_phaBC & 0xFFU;
          can_send_msg((0x204U + board.can_addr_offset), ((dat[7] << 24U) | (dat[6] << 16U) | (dat[5] << 8U) | dat[4]), ((dat[3] << 24U) | (dat[2] << 16U) | (dat[1] << 8U) | dat[0]), 8U);

          uint16_t left_hall_angle;
          uint16_t right_hall_angle;
          if (hw_type == HW_TYPE_KNEE) {
            angle_sensor_read(sensor_angle);
            left_hall_angle = hall_angle_offset[0] + ((motPosL / 15 / GEARBOX_RATIO_LEFT) % 360);
            right_hall_angle = hall_angle_offset[1] + ((motPosR / 15 / GEARBOX_RATIO_RIGHT) % 360);
          } else {
            left_hall_angle = motPosL / 15;
            right_hall_angle = -motPosR / 15;
          }
          //MOTORS_ANGLE: left angle sensor(2), right angle sensor(2), left hall angle(2), right hall angle(2)
          dat[0] = (sensor_angle[0]>>8U) & 0xFFU;
          dat[1] = sensor_angle[0] & 0xFFU;
          dat[2] = (sensor_angle[1]>>8U) & 0xFFU;
          dat[3] = sensor_angle[1] & 0xFFU;
          dat[4] = (left_hall_angle>>8U) & 0xFFU;
          dat[5] = left_hall_angle & 0xFFU;
          dat[6] = (right_hall_angle>>8U) & 0xFFU;
          dat[7] = right_hall_angle & 0xFFU;
          can_send_msg((0x205U + board.can_addr_offset), ((dat[7] << 24U) | (dat[6] << 16U) | (dat[5] << 8U) | dat[4]), ((dat[3] << 24U) | (dat[2] << 16U) | (dat[1] << 8U) | dat[0]), 8U);

          IMU_soft_sensor_read(unknown_imu_data);
          //BOARD_IMU_RAW1: FIXME: add comment after discovering data, looks like magnetometer
          dat[0] = (unknown_imu_data[0]>>8U) & 0xFFU;
          dat[1] = unknown_imu_data[0] & 0xFFU;
          dat[2] = (unknown_imu_data[1]>>8U) & 0xFFU;
          dat[3] = unknown_imu_data[1] & 0xFFU;
          dat[4] = (unknown_imu_data[2]>>8U) & 0xFFU;
          dat[5] = unknown_imu_data[2] & 0xFFU;
          can_send_msg((0x206U + board.can_addr_offset), ((dat[5] << 8U) | dat[4]), ((dat[3] << 24U) | (dat[2] << 16U) | (dat[1] << 8U) | dat[0]), 6U);

          //BOARD_IMU_RAW2: FIXME: add comment after discovering data, looks like acceleration?
          dat[0] = (unknown_imu_data[3]>>8U) & 0xFFU;
          dat[1] = unknown_imu_data[3] & 0xFFU;
          dat[2] = (unknown_imu_data[4]>>8U) & 0xFFU;
          dat[3] = unknown_imu_data[4] & 0xFFU;
          dat[4] = (unknown_imu_data[5]>>8U) & 0xFFU;
          dat[5] = unknown_imu_data[5] & 0xFFU;
          can_send_msg((0x207U + board.can_addr_offset), ((dat[5] << 8U) | dat[4]), ((dat[3] << 24U) | (dat[2] << 16U) | (dat[1] << 8U) | dat[0]), 6U);
        }
        torque_cmd_timeout = (torque_cmd_timeout < MAX_uint32_T) ? (torque_cmd_timeout+1) : 0;
        main_loop_100Hz_runtime = HAL_GetTick() - main_loop_100Hz_runtime;
        main_loop_100Hz = HAL_GetTick();
      }

      // runs at 10Hz
       if ((HAL_GetTick() - (main_loop_10Hz - main_loop_10Hz_runtime)) >= 100) {
        main_loop_10Hz_runtime = HAL_GetTick();
        if (ignition_off_counter <= IGNITION_OFF_DELAY) {
          // VAR_VALUES: fault_status(0:4), enable_motors(0:1), ignition(0:1), left motor error(1), right motor error(1)
          uint8_t dat[2];
          dat[0] = ((fault_status.right_angle << 5U) | (fault_status.right_i2c << 4U) | (fault_status.left_angle << 3U) | (fault_status.left_i2c << 2U) | (enable_motors << 1U) | ignition);
          dat[1] = rtY_Left.z_errCode;
          dat[2] = rtY_Right.z_errCode;
          can_send_msg((0x202U + board.can_addr_offset), 0x0U, ((dat[2] << 16U) | (dat[1] << 8U) | dat[0]), 3U);
        }
        out_enable(LED_GREEN, ignition);

        main_loop_10Hz_runtime = HAL_GetTick() - main_loop_10Hz_runtime;
        main_loop_10Hz = HAL_GetTick();
      }

      // runs at 1Hz
      if ((HAL_GetTick() - (main_loop_1Hz - main_loop_1Hz_runtime)) >= 1000) {
        main_loop_1Hz_runtime = HAL_GetTick();
         // ####### CALC BOARD TEMPERATURE #######
        filtLowPass32(adc_buffer.temp, TEMP_FILT_COEF, &board_temp_adcFixdt);
        board_temp_adcFilt  = (int16_t)(board_temp_adcFixdt >> 16);  // convert fixed-point to integer
        board_temp_deg_c    = (TEMP_CAL_HIGH_DEG_C - TEMP_CAL_LOW_DEG_C) * (board_temp_adcFilt - TEMP_CAL_LOW_ADC) / (TEMP_CAL_HIGH_ADC - TEMP_CAL_LOW_ADC) + TEMP_CAL_LOW_DEG_C;

        // ####### CALC CALIBRATED BATTERY VOLTAGE #######
        batVoltageCalib = batVoltage * BAT_CALIB_REAL_VOLTAGE / BAT_CALIB_ADC;

        charger_connected = !HAL_GPIO_ReadPin(CHARGER_PORT, CHARGER_PIN);
        uint8_t battery_percent = 100 - (((420 * BAT_CELLS) - batVoltageCalib) / BAT_CELLS / VOLTS_PER_PERCENT / 100); // Battery % left

        // BODY_DATA: MCU temp(2), battery voltage(2), battery_percent(0:7), charger_connected(0:1)
        uint8_t dat[4];
        dat[0] = board_temp_deg_c & 0xFFU;
        dat[1] = (batVoltageCalib >> 8U) & 0xFFU;
        dat[2] = batVoltageCalib & 0xFFU;
        dat[3] = (((battery_percent & 0x7FU) << 1U) | charger_connected);
        can_send_msg((0x203U + board.can_addr_offset), 0x0U, ((dat[3] << 24U) | (dat[2] << 16U) | (dat[1] << 8U) | dat[0]), 4U);

        out_enable(LED_BLUE, false); // Reset LED after CAN RX
        out_enable(LED_GREEN, true); // Always use LED to show that body is on

        if ((hw_type == HW_TYPE_BASE) && ignition) {
          ignition_off_counter = 0;
        } else {
          ignition_off_counter = (ignition_off_counter < MAX_uint32_T) ? (ignition_off_counter+1) : 0;
        }

        if ((TEMP_POWEROFF_ENABLE && board_temp_deg_c >= TEMP_POWEROFF && speedAvgAbs < 20) || (batVoltage < BAT_DEAD && speedAvgAbs < 20)) {  // poweroff before mainboard burns OR low bat 3
          poweroff();
        } else if (rtY_Left.z_errCode || rtY_Right.z_errCode) { // 1 beep (low pitch): Motor error, disable motors
          enable_motors = 0;
          beepCount(1, 24, 1);
        } else if (fault_status.left_angle || fault_status.left_i2c || fault_status.right_angle || fault_status.right_i2c) { // 2 beeps (low pitch): Motor error, disable motors
          beepCount(2, 24, 1);
        } else if (TEMP_WARNING_ENABLE && board_temp_deg_c >= TEMP_WARNING) { // 5 beeps (low pitch): Mainboard temperature warning
          beepCount(5, 24, 1);
        } else if (batVoltage < BAT_LVL1) { // 1 beep fast (medium pitch): Low bat 1
          beepCount(0, 10, 6);
          out_enable(LED_RED, true);
        } else if (batVoltage < BAT_LVL2) { // 1 beep slow (medium pitch): Low bat 2
          beepCount(0, 10, 30);
        } else {  // do not beep
          beepCount(0, 0, 0);
          out_enable(LED_RED, false);
        }

        main_loop_1Hz_runtime = HAL_GetTick() - main_loop_1Hz_runtime;
        main_loop_1Hz = HAL_GetTick();
      }

      if (hw_type == HW_TYPE_BASE) {
        if (HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) {
          cnt_press += 1;
          if (cnt_press == (2 * 1008)) {
            poweroff();
          }
        } else if (cnt_press >= 10) {
          ignition = !ignition;
          out_enable(IGNITION, ignition);
          beepShort(5);
          cnt_press = 0;
        }
      }

      process_can();
      tick_prev = HAL_GetTick();
    }
  }
}