/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * File Name          : app_freertos.c
  * Description        : Code for freertos applications
  ******************************************************************************
  */
/* USER CODE END Header */

#include "FreeRTOS.h"
#include "task.h"
#include "main.h"
#include "cmsis_os.h"
#include "queue.h"

/* USER CODE BEGIN Includes */
#include "adc.h"
#include "usart.h"
#include "oled.h"
#include "led.h"
#include "readpin.h"
#include "variables.h"
#include <stdint.h>
#include <stdio.h>
#include <string.h>
#include "car_move.h"
#include "callback.h"
#include "tim.h"
#include "attitude.h"
#include "esp8266.h"
#include "gas_sensor.h"
/* USER CODE END Includes */

osThreadId_t displayHandle;
const osThreadAttr_t display_attributes = {
  .name = "display",
  .priority = (osPriority_t) osPriorityNormal,
  .stack_size = 256 * 4
};

osThreadId_t pinreadHandle;
const osThreadAttr_t pinread_attributes = {
  .name = "pinread",
  .priority = (osPriority_t) osPriorityHigh,
  .stack_size = 128 * 4
};

osThreadId_t ledHandle;
const osThreadAttr_t led_attributes = {
  .name = "led",
  .priority = (osPriority_t) osPriorityNormal,
  .stack_size = 128 * 4
};

const osThreadAttr_t UART1_receive_attributes = {
  .name = "UART1_receive",
  .priority = (osPriority_t) osPriorityAboveNormal,
  .stack_size = 128 * 4
};

osThreadId_t UART1_receiveHandle;

const osThreadAttr_t MotorCtrl_attributes = {
  .name = "MotorCtrl",
  .priority = (osPriority_t) osPriorityAboveNormal,
  .stack_size = 256 * 4
};
osThreadId_t MotorCtrlHandle;

const osThreadAttr_t EncoderSpeedTask_attributes = {
  .name = "EncoderSpeed",
  .priority = (osPriority_t) osPriorityNormal,
  .stack_size = 256 * 4
};

osThreadId_t EncoderSpeedTaskHandle;

const osThreadAttr_t AttitudeTask_attributes = {
  .name = "Attitude",
  .priority = (osPriority_t) osPriorityNormal,
  .stack_size = 256 * 4
};
osThreadId_t AttitudeTaskHandle;

const osThreadAttr_t ADCTask_attributes = {
  .name = "ADCTask",
  .priority = (osPriority_t) osPriorityNormal,
  .stack_size = 256 * 4
};

osThreadId_t ADCTaskHandle;

const osThreadAttr_t ESP8266Task_attributes = {
  .name = "ESP8266",
  .priority = (osPriority_t) osPriorityBelowNormal,
  .stack_size = 512 * 4
};
osThreadId_t ESP8266TaskHandle;

const osThreadAttr_t GasSensorTask_attributes = {
  .name = "GasSensor",
  .priority = (osPriority_t) osPriorityBelowNormal,
  .stack_size = 256 * 4
};
osThreadId_t GasSensorTaskHandle;

void pinread_function(void *argument);
void display_function(void *argument);
void led_function(void *argument);
void UART1_receive_function(void *argument);
void MotorCtrl_function(void *argument);
void EncoderSpeedTask_function(void *argument);
void AttitudeTask_function(void *argument);
void ADCTask_function(void *argument);
void ESP8266Task_function(void *argument);
void GasSensorTask_function(void *argument);

void MX_FREERTOS_Init(void);

#define NET_CMD_FRAME_LEN        10U
#define NET_FB_FRAME_LEN         21U
#define NET_CMD_TIMEOUT_MS       350U
#define NET_FB_PERIOD_MS         100U
#define NET_MOVE_STOP            0x00U
#define NET_LED_OFF              0x00U
#define NET_LED_ON               0xFFU

typedef struct
{
  uint8_t buf[NET_CMD_FRAME_LEN];
  uint8_t idx;
} NetCmdParser_t;

static NetCmdParser_t g_net_parser = {{0U}, 0U};

static void oled_line(unsigned char x, unsigned char y, const char *text)
{
  char buf[24] = {0};
  (void)snprintf(buf, sizeof(buf), "%-23s", text);
  plusstring(buf, x, y);
}

static uint16_t adc_read_average(uint8_t times)
{
  uint32_t sum = 0U;
  uint8_t ok_cnt = 0U;

  for (uint8_t i = 0U; i < times; i++)
  {
    if (HAL_ADC_Start(&hadc1) != HAL_OK)
    {
      continue;
    }

    if (HAL_ADC_PollForConversion(&hadc1, 10U) == HAL_OK)
    {
      sum += HAL_ADC_GetValue(&hadc1);
      ok_cnt++;
    }

    (void)HAL_ADC_Stop(&hadc1);
  }

  if (ok_cnt == 0U)
  {
    return 0U;
  }

  return (uint16_t)(sum / ok_cnt);
}

static void net_cmd_parser_reset(void)
{
  g_net_parser.idx = 0U;
}

static void net_cmd_apply_frame(const uint8_t frame[NET_CMD_FRAME_LEN])
{
  net_seq = frame[2];
  net_move_cmd = frame[3];
  net_led_cmd = frame[4];
  net_yun_cmd = frame[5];
  net_last_cmd_tick = HAL_GetTick();
  net_frame_ok_count++;
}

static void net_cmd_parser_push(uint8_t byte)
{
  switch (g_net_parser.idx)
  {
    case 0U:
      if (byte == 0x5AU)
      {
        g_net_parser.buf[g_net_parser.idx++] = byte;
      }
      break;

    case 1U:
      if (byte == 0xA5U)
      {
        g_net_parser.buf[g_net_parser.idx++] = byte;
      }
      else if (byte == 0x5AU)
      {
        g_net_parser.buf[0] = byte;
        g_net_parser.idx = 1U;
      }
      else
      {
        net_cmd_parser_reset();
      }
      break;

    default:
      g_net_parser.buf[g_net_parser.idx++] = byte;
      if (g_net_parser.idx >= NET_CMD_FRAME_LEN)
      {
        net_cmd_apply_frame(g_net_parser.buf);
        net_cmd_parser_reset();
      }
      break;
  }
}

static void net_poll_rx_frames(void)
{
  uint8_t rx_buf[64];
  uint16_t rx_len = 0U;

  do
  {
    rx_len = ESP8266_ReadRaw(rx_buf, sizeof(rx_buf));
    for (uint16_t i = 0U; i < rx_len; i++)
    {
      net_cmd_parser_push(rx_buf[i]);
    }
  } while (rx_len > 0U);
}

static void net_send_feedback_frame(void)
{
  uint8_t tx_frame[NET_FB_FRAME_LEN] = {0U};
  const uint16_t adc_now = adc_value;

  tx_frame[0] = 0xA5U;
  tx_frame[1] = 0x5AU;
  tx_frame[2] = net_seq;
  tx_frame[3] = (uint8_t)((adc_now >> 8) & 0xFFU);
  tx_frame[4] = (uint8_t)(adc_now & 0xFFU);
  tx_frame[5] = esp_link_ready;
  tx_frame[6] = (uint8_t)(net_frame_ok_count & 0xFFU);
  tx_frame[7] = (uint8_t)(net_feedback_ok_count & 0xFFU);
  tx_frame[8] = (uint8_t)(net_feedback_fail_count & 0xFFU);

  tx_frame[9] = (uint8_t)((gas_slot1.conc_raw >> 8) & 0xFFU);
  tx_frame[10] = (uint8_t)(gas_slot1.conc_raw & 0xFFU);
  tx_frame[11] = gas_slot1.gas_type;
  tx_frame[12] = (uint8_t)((gas_slot1.fmt_reg >> 8) & 0xFFU);
  tx_frame[13] = (uint8_t)(gas_slot1.fmt_reg & 0xFFU);
  tx_frame[14] = gas_slot1.state;

  tx_frame[15] = (uint8_t)((gas_slot2.conc_raw >> 8) & 0xFFU);
  tx_frame[16] = (uint8_t)(gas_slot2.conc_raw & 0xFFU);
  tx_frame[17] = gas_slot2.gas_type;
  tx_frame[18] = (uint8_t)((gas_slot2.fmt_reg >> 8) & 0xFFU);
  tx_frame[19] = (uint8_t)(gas_slot2.fmt_reg & 0xFFU);
  tx_frame[20] = gas_slot2.state;

  if (ESP8266_SendRaw(tx_frame, sizeof(tx_frame), 0U) != 0U)
  {
    net_feedback_ok_count++;
  }
  else
  {
    net_feedback_fail_count++;
  }
}

void MX_FREERTOS_Init(void)
{
  pinreadHandle = osThreadNew(pinread_function, NULL, &pinread_attributes);
  displayHandle = osThreadNew(display_function, NULL, &display_attributes);
  ledHandle = osThreadNew(led_function, NULL, &led_attributes);
  UART1_receiveHandle = osThreadNew(UART1_receive_function, NULL, &UART1_receive_attributes);
  MotorCtrlHandle = osThreadNew(MotorCtrl_function, NULL, &MotorCtrl_attributes);
  EncoderSpeedTaskHandle = osThreadNew(EncoderSpeedTask_function, NULL, &EncoderSpeedTask_attributes);
  AttitudeTaskHandle = osThreadNew(AttitudeTask_function, NULL, &AttitudeTask_attributes);
  ADCTaskHandle = osThreadNew(ADCTask_function, NULL, &ADCTask_attributes);
  ESP8266TaskHandle = osThreadNew(ESP8266Task_function, NULL, &ESP8266Task_attributes);
  GasSensorTaskHandle = osThreadNew(GasSensorTask_function, NULL, &GasSensorTask_attributes);
}

void pinread_function(void *argument)
{
  (void)argument;

  for (;;)
  {
    pin_value = ReadPin();
    osDelay(20);
  }
}

void display_function(void *argument)
{
  (void)argument;

  char line[24];
  uint8_t esp_state = 0U;

  for (;;)
  {
    OLED_Clear();

    (void)snprintf(line, sizeof(line), "S:%u %u", (unsigned)UART1[0], (unsigned)UART1[1]);
    oled_line(0, 0, line);

    if (adc_ready != 0U)
    {
      const int batt10 = (int)(battery_voltage * 10.0f + 0.5f);
      (void)snprintf(line, sizeof(line), "V:%d.%d", batt10 / 10, batt10 % 10);
    }
    else
    {
      (void)snprintf(line, sizeof(line), "V:ERR");
    }
    oled_line(60, 0, line);

    if (esp_link_stage >= 6U)
    {
      esp_state = 2U;
    }
    else if (esp_link_stage == 5U)
    {
      esp_state = 1U;
    }
    else
    {
      esp_state = 0U;
    }

    (void)snprintf(line, sizeof(line), "8266:%u", (unsigned)esp_state);
    oled_line(0, 15, line);

    (void)snprintf(line, sizeof(line), "RX:%lu TX:%lu",
                   (unsigned long)(net_frame_ok_count & 0xFFFFUL),
                   (unsigned long)(net_feedback_ok_count & 0xFFFFUL));
    oled_line(0, 30, line);

    if (mpu_online != 0U)
    {
      const int pitch10 = (int)(pitch_deg * (-10.0f));
      (void)snprintf(line, sizeof(line), "P:%d.%d", pitch10 / 10, (pitch10 < 0 ? -pitch10 : pitch10) % 10);
    }
    else
    {
      (void)snprintf(line, sizeof(line), "P:ERR");
    }
    oled_line(0, 45, line);

    osDelay(150);
  }
}

void led_function(void *argument)
{
  (void)argument;

  for (;;)
  {
    const uint32_t now = HAL_GetTick();

    if ((net_last_cmd_tick != 0U) && ((now - net_last_cmd_tick) <= NET_CMD_TIMEOUT_MS))
    {
      if (net_led_cmd == NET_LED_ON)
      {
        led(14U, 1U);
      }
      else if (net_led_cmd == NET_LED_OFF)
      {
        led(0U, 1U);
      }
      else
      {
        led((uint8_t)pin_value, 1U);
      }
    }
    else
    {
      led((uint8_t)pin_value, 1U);
    }

    osDelay(50);
  }
}

void UART1_receive_function(void *argument)
{
  (void)argument;

  UART1_RegisterTaskHandle(osThreadGetId());
  UART1_StartRxIT();

  for (;;)
  {
    (void)osThreadFlagsWait(0x01U, osFlagsWaitAny, osWaitForever);
  }
}

#define ENCODER_CPR_OUTPUT  (3960.0f)

static inline int16_t diff_u16(uint16_t now, uint16_t last)
{
  return (int16_t)(now - last);
}

static inline int32_t diff_u32(uint32_t now, uint32_t last)
{
  return (int32_t)(now - last);
}

void MotorCtrl_function(void *argument)
{
  (void)argument;
  uint16_t speed[2] = {0U, 0U};

  for (;;)
  {
    const uint32_t now = HAL_GetTick();

    if ((net_last_cmd_tick != 0U) && ((now - net_last_cmd_tick) <= NET_CMD_TIMEOUT_MS))
    {
      car_move_cmd((uint8_t)net_move_cmd);
    }
    else
    {
      if ((net_last_cmd_tick != 0U) && (net_move_cmd != NET_MOVE_STOP))
      {
        net_move_cmd = NET_MOVE_STOP;
      }
      car_move((uint8_t)pin_value, speed);
    }

    osDelay(10);
  }
}

void EncoderSpeedTask_function(void *argument)
{
  (void)argument;

  uint32_t last_l = __HAL_TIM_GET_COUNTER(&htim2);
  uint16_t last_r = (uint16_t)__HAL_TIM_GET_COUNTER(&htim4);

  const uint32_t period_ms = 10U;
  const float Ts = (float)period_ms / 1000.0f;

  for (;;)
  {
    const uint32_t now_l = __HAL_TIM_GET_COUNTER(&htim2);
    const uint16_t now_r = (uint16_t)__HAL_TIM_GET_COUNTER(&htim4);

    const int32_t dl = diff_u32(now_l, last_l);
    const int16_t dr = diff_u16(now_r, last_r);

    last_l = now_l;
    last_r = now_r;

    rpm_left = ((float)dl * 60.0f) / (ENCODER_CPR_OUTPUT * Ts);
    rpm_right = ((float)dr * 60.0f) / (ENCODER_CPR_OUTPUT * Ts);

    osDelay(period_ms);
  }
}

void AttitudeTask_function(void *argument)
{
  (void)argument;

  const float dt = 0.01f;

  osDelay(200);
  Attitude_Init();

  if (mpu_online != 0U)
  {
    float sum = 0.0f;
    const int sample_count = 100;

    for (int i = 0; i < sample_count; i++)
    {
      Attitude_Update(dt);
      sum += pitch_deg;
      osDelay(10);
    }

    pitch_offset = sum / (float)sample_count;
  }

  for (;;)
  {
    if (mpu_online == 0U)
    {
      Attitude_Init();
      osDelay(500);
      continue;
    }

    Attitude_Update(dt);
    osDelay(10);
  }
}

void ADCTask_function(void *argument)
{
  (void)argument;
  const uint32_t period_ms = 20U;

  if (HAL_ADCEx_Calibration_Start(&hadc1, ADC_SINGLE_ENDED) == HAL_OK)
  {
    adc_ready = 1U;
  }
  else
  {
    adc_ready = 0U;
  }

  for (;;)
  {
    adc_raw = adc_read_average(8U);
    adc_value = adc_raw;
    adc_voltage = ((float)adc_raw / 4095.0f) * 3.3f;
    battery_voltage = adc_voltage * 11.0f;

    if (adc_raw != 0U)
    {
      adc_ready = 1U;
    }

    osDelay(period_ms);
  }
}

void ESP8266Task_function(void *argument)
{
  (void)argument;
  uint32_t last_feedback_tick = 0U;

  ESP8266_RegisterTaskHandle(osThreadGetId());
  UART4_StartRxIT();
  osDelay(200);

  for (;;)
  {
    if (ESP8266_IsTransparentReady() == 0U)
    {
      esp_link_retry_count++;
      last_feedback_tick = HAL_GetTick();

      if (ESP8266_Bringup() == 0U)
      {
        osDelay(3000);
        continue;
      }
    }

    ESP8266_Service();
    net_poll_rx_frames();

    if ((HAL_GetTick() - last_feedback_tick) >= NET_FB_PERIOD_MS)
    {
      net_send_feedback_frame();
      last_feedback_tick = HAL_GetTick();
    }

    osDelay(10);
  }
}

void GasSensorTask_function(void *argument)
{
  (void)argument;

  GasSensor_Init();
  UART2_StartRxIT();
  UART3_StartRxIT();
  osDelay(300);

  for (;; )
  {
    GasSensor_Service();
    osDelay(10);
  }
}
