/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * File Name          : app_freertos.c
  * Description        : Code for freertos applications
  ******************************************************************************
  * @attention
  *
  * Copyright (c) 2026 STMicroelectronics.
  * All rights reserved.
  *
  * This software is licensed under terms that can be found in the LICENSE file
  * in the root directory of this software component.
  * If no LICENSE file comes with this software, it is provided AS-IS.
  *
  ******************************************************************************
  */
/* USER CODE END Header */

/* Includes ------------------------------------------------------------------*/
#include "FreeRTOS.h"
#include "task.h"
#include "main.h"
#include "cmsis_os.h"
#include "queue.h"
/* Private includes ----------------------------------------------------------*/
/* 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 "car_move.h"
#include "callback.h"
#include "tim.h"
#include "attitude.h"
/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */

/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN Variables */

/* USER CODE END Variables */
/* Definitions for defaultTask */
osThreadId_t displayHandle;
const osThreadAttr_t display_attributes = {
  .name = "display",
  .priority = (osPriority_t) osPriorityNormal,
  .stack_size = 128 * 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) osPriorityNormal,
  .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 = 128 * 4
};

osThreadId_t ADCTaskHandle;



/* Private function prototypes -----------------------------------------------*/
/* USER CODE BEGIN FunctionPrototypes */

/* USER CODE END FunctionPrototypes */


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 MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */
void AttitudeTask_function(void *argument);
void ADCTask_function(void *argument);



/**
  * @brief  FreeRTOS initialization
  * @param  None
  * @retval None
  */
void MX_FREERTOS_Init(void) {
  /* USER CODE BEGIN Init */

  /* USER CODE END Init */

  /* USER CODE BEGIN RTOS_MUTEX */
  /* add mutexes, ... */
  /* USER CODE END RTOS_MUTEX */

  /* USER CODE BEGIN RTOS_SEMAPHORES */
  /* add semaphores, ... */
  /* USER CODE END RTOS_SEMAPHORES */

  /* USER CODE BEGIN RTOS_TIMERS */
  /* start timers, add new ones, ... */
  /* USER CODE END RTOS_TIMERS */

  /* USER CODE BEGIN RTOS_QUEUES */
  /* add queues, ... */
  /* USER CODE END RTOS_QUEUES */

  /* Create the thread(s) */
  /* creation of defaultTask */
  
	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);
  /* USER CODE BEGIN RTOS_THREADS */
  /* add threads, ... */
  /* USER CODE END RTOS_THREADS */

  /* USER CODE BEGIN RTOS_EVENTS */
  /* add events, ... */
  /* USER CODE END RTOS_EVENTS */

}

/* USER CODE BEGIN Header_StartDefaultTask */
/**
  * @brief  Function implementing the defaultTask thread.
  * @param  argument: Not used
  * @retval None
  */
/* USER CODE END Header_StartDefaultTask */


/* Private application code --------------------------------------------------*/
/* USER CODE BEGIN Application */
void pinread_function(void *argument)
{
  /* USER CODE BEGIN StartDefaultTask */
  /* Infinite loop */
  for(;;)
  {
    pin_value=ReadPin();
		osDelay(50);
  }
  /* USER CODE END StartDefaultTask */
}


void display_function(void *argument)
{
  /* USER CODE BEGIN StartDefaultTask */
  /* Infinite loop */
  for(;;)
  {
		char result[30]="";
		plusstring("s: ", 0,0);
		//plusstring("     ", 20,0);
		sprintf(result, "%d %d",UART1[0],UART1[1]);
		plusstring(result, 25,0);
		
		plusstring("V: ", 60,0);
		//plusstring("        ", 80,0);		
		sprintf(result, "%.1f",adc_voltage * 11);
		plusstring(result, 80,0);	
		
		plusstring("lr: ", 0,15);	
		//plusstring("        ", 5,15);		
		sprintf(result, "%.1f",rpm_left * (-1));
		plusstring(result, 25,15);
		
		plusstring("rr: ", 0,30);	
		//plusstring("        ", 20,30);		
		sprintf(result, "%.1f",rpm_right);
		plusstring(result, 20,30);
		
		plusstring("d: ", 0,45);		
		//plusstring("        ", 20,45);		
		sprintf(result, "%.1f",pitch_deg * (-1 ));
		plusstring(result, 20,45);
		

    osDelay(400);
  }
  /* USER CODE END StartDefaultTask */
}

void led_function(void *argument)
{
  /* USER CODE BEGIN StartDefaultTask */
  /* Infinite loop */
  for(;;)
  {
		led(pin_value, 1);
    osDelay(100);
  }
  /* USER CODE END StartDefaultTask */
}

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


  UART1_RegisterTaskHandle(osThreadGetId());



  extern void UART1_StartRxIT(void);
  UART1_StartRxIT();

  for (;;)
  {
		motor_ctrl_buffer[0] = UART1[0];
		motor_ctrl_buffer[1] = UART1[1];
    osThreadFlagsWait(0x01, osFlagsWaitAny, osWaitForever);
		
    
  }
}

void MotorCtrl_function(void *argument)
{
  (void)argument;

  uint16_t speed[2] = {0, 0};

  for (;;)
  {
    car_move(0, speed);                 
    osDelay(10);                        
  }
}

#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 EncoderSpeedTask_function(void *argument)
{
  (void)argument;

  HAL_TIM_Encoder_Start(&htim2, TIM_CHANNEL_ALL); 
  HAL_TIM_Encoder_Start(&htim4, TIM_CHANNEL_ALL); 

  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 = 10;
  const float Ts = (float)period_ms / 1000.0f;

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

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

    last_l = now_l;
    last_r = now_r;

    
    float rpmL = ((float)dl * 60.0f) / (ENCODER_CPR_OUTPUT * Ts);
    float rpmR = ((float)dr * 60.0f) / (ENCODER_CPR_OUTPUT * Ts);

    
    rpm_left  = rpmL;
    rpm_right = rpmR;

    osDelay(period_ms);
  }
}

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

  osDelay(200);        

  Attitude_Init();

  osDelay(500);          

  osDelay(1000);            

  float sum = 0.0f;
  const int N = 200;
  const float dt = 0.01f;

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

  pitch_offset = sum / N;               

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


void ADCTask_function(void *argument)
{
  (void)argument;

  const uint32_t period_ms = 10;   

  for (;;)
  {
    HAL_ADC_Start(&hadc1);

    if (HAL_ADC_PollForConversion(&hadc1, 10) == HAL_OK)
    {
        adc_raw = HAL_ADC_GetValue(&hadc1);

        adc_voltage = (adc_raw / 4095.0f) * 3.3f;
    }

    HAL_ADC_Stop(&hadc1);

    osDelay(period_ms);
  }
}


