#include "main.h"
#include "motorCommand.h"
#include "quadEncoder.h"
#include "captDistIR.h"
#include "VL53L0X.h"
#include "groveLCD.h"
#include <stdbool.h>
#include <string.h>

#define ARRAY_LEN 100
#define SAMPLING_PERIOD_ms 5
#define FIND_MOTOR_INIT_POS 0

//################################################
#define EX1 1
#define EX2 2
#define EX3 3
// time constant of the two motors
#define TAU_L	230
#define TAU_R	230

// movement behavior
#define VMAX      1600 //tr.min⁻¹
#define CRITICAL_DIST 500
#define TRACKING_CST_X       0.6
#define MAX_ERROR_X	       320
// states defines
#define AUTO      1
#define REMOTE    2
#define COLOR     3

// #define DEBUG


#define SYNCHRO_EX EX3
//################################################
//################################################
// PARAMETRE A MODIFIER EN FONCTION DU N° ROBOT
#define ROS_DOMAIN_ID 12
//################################################
extern UART_HandleTypeDef huart1;
extern UART_HandleTypeDef huart2;
extern DMA_HandleTypeDef hdma_usart1_rx;
extern DMA_HandleTypeDef hdma_usart1_tx;
extern DMA_HandleTypeDef hdma_usart2_rx;
extern DMA_HandleTypeDef hdma_usart2_tx;

extern I2C_HandleTypeDef hi2c1;


// donnee envoyé à l'ihm
int Motor_speed_Right = 0;
int Motor_speed_Left = 0;
int sensor_left = 0;
int sensor_right = 0;
int sensor_back = 0;



/* Definitions for defaultTask */
osThreadId_t defaultTaskHandle;
const osThreadAttr_t defaultTask_attributes = {
  .name = "defaultTask",
  .stack_size = 3000 * 4,
  .priority = (osPriority_t) osPriorityNormal,
};

// Déclaration des objets synchronisants !! Ne pas oublier de les créer
xSemaphoreHandle xSemaphore = NULL;
xQueueHandle qh = NULL;
xQueueHandle data_lcd = NULL;

struct AMessage // Exemple de type de message pour queues, on peut mettre ce qu'on veut
{
	char command;
	int data;
};


void SystemClock_Config(void);
void microros_task(void *argument);
//void subscription_callback_raspberry(const void * msgin);


// Buffer
int log_consigne[100];
int log_vitesse[100];
int log_time_ms[100];



// color mode vars
int errorY= 0;
int errorX= 160; // 0;320
// remote mode vars
int direction_remote = 0;// + 150 Droite
int speed_remote = 0; // OK 0 arret ||  VMAX OK
int speed_tracking = 0;
// current mode
int state = AUTO;
bool update_lcd = true;

// current max speed
int max_speed= VMAX/4;

int Left_first_index_reached = 0;
int Right_first_index_reached = 0;



// globals data

//========================================================

// Commanding Task E
static void task_command(void *pvParameters)
{
    struct AMessage pxMessageR, pxMessageL;

    int speedL = 1500;
    int speedR = 1500;
    int tab[2];


    // Prepare messages for tasks F and G
    pxMessageR.command = 'r';
    pxMessageR.data = 0;
    pxMessageL.command = 'l';
    pxMessageL.data = 0;
    vTaskDelay(1000);

    for (;;)
    {

    	captDistIR_Get(tab);
        // Toggle GPIO to indicate activity
        HAL_GPIO_TogglePin(GPIOB, GPIO_PIN_4);

        switch (state){
        case AUTO:
        	sensor_right = tab[1];
			sensor_left = tab[0];
			sensor_back = VL53L0X_readRangeContinuousMillimeters();
          bool left_limit  = sensor_left > CRITICAL_DIST;
          bool right_limit = sensor_right > CRITICAL_DIST;

          // Proportional Corrector
          float corrL = 0.0f;
          float corrR = 0.0f;

          if (left_limit || right_limit) {

              // norm dist in [0;1]
              float normL = 0.0f;
              float normR = 0.0f;

              if (sensor_left > CRITICAL_DIST)
                  normL = (float)(sensor_left - CRITICAL_DIST) / (float)CRITICAL_DIST;

              if (sensor_right > CRITICAL_DIST)
                  normR = (float)(sensor_right - CRITICAL_DIST) / (float)CRITICAL_DIST;

              // I need a factor
              float k = 1.5f;

              corrL = -k * normL * max_speed;
              corrR = -k * normR * max_speed;

              speedL = max_speed + corrL;
              speedR = max_speed + corrR;

          } else {
              speedL = max_speed;
              speedR = max_speed;
          }
          break;
        case REMOTE:
          speedL = (speed_remote / 100.0) * max_speed - (direction_remote / 100.0) * max_speed;
          speedR = (speed_remote / 100.0) * max_speed + (direction_remote / 100.0) * max_speed;
          break;
        case COLOR:{
			// speed moduled by the error with an offset
			// TODO: review Offsets
			int error = MAX_ERROR_X/2 - errorX;
			if (error < -(MAX_ERROR_X/4) || error > (MAX_ERROR_X/4)) { error = error * 2; }
		  speedL = (speed_tracking*max_speed/100) + TRACKING_CST_X * error;
		  speedR = (speed_tracking*max_speed/100) - TRACKING_CST_X * error;
		  break;
        	}
        }
        // Speed comp. with VMAX and -VMAX
         if (speedL > VMAX){
       	  speedL = VMAX;
         }else if(speedL < -VMAX){
       	  speedL = -VMAX;
         }
         if(speedR > VMAX){
       	  speedR = VMAX;
         }else if(speedR < -VMAX){
       	  speedR = -VMAX;
         }

        // update messages
        pxMessageL.data = speedL;
        pxMessageR.data = speedR;

        // START tasks of the two motors
        xQueueSend(qh, (void *)&pxMessageR, portMAX_DELAY);
        xSemaphoreTake(xSemaphore, portMAX_DELAY);
        xQueueSend(qh, (void *)&pxMessageL, portMAX_DELAY);
        xSemaphoreTake(xSemaphore, portMAX_DELAY);
        if (update_lcd){
          update_lcd = false;
          xQueueSend(data_lcd, (void *)&state, portMAX_DELAY);
        }
        // Toggle GPIO to indicate activity
        HAL_GPIO_TogglePin(GPIOB, GPIO_PIN_4);
        vTaskDelay(SAMPLING_PERIOD_ms);
    }
}

static void task_MotorR(void *pvParameters)
{
    struct AMessage pxRxedMessage;
   	int speed = 0;
    int consigne = 1500;
    int pwm_output = 0;
    static int error = 0;

    const float Kp = 0.05;
    const float Ki = SAMPLING_PERIOD_ms/(0.1*TAU_L);
    const int max_pwm = 100;
    const int min_pwm = -100;
    static float Ui = 0;
    static float Up = 0;

    for (;;)
    {
        xQueueReceive(qh, &pxRxedMessage, portMAX_DELAY);

        speed = quadEncoder_GetSpeedR();
        Motor_speed_Right = speed;
        consigne = pxRxedMessage.data;

        // push in buffer


		error = consigne - speed;
		Up = Kp * (float) error ;
		Ui = Ui + Kp * Ki * (float) error;
		pwm_output = (int) (Up + Ui);
		if (pwm_output > max_pwm)
		{
			pwm_output = max_pwm;
		}
		else if (pwm_output < min_pwm)
		{
			pwm_output = min_pwm;
		}
		motorRight_SetDuty(pwm_output+100);
        xSemaphoreGive(xSemaphore);
    }
}

// Task G: Motor control or similar function
static void task_MotorL(void *pvParameters)
{
    struct AMessage pxRxedMessage;
   	int speed = 0;
    int consigne = 1500;
    int pwm_output = 0;
    static int error = 0;

    const float Kp = 0.05;
    const float Ki = SAMPLING_PERIOD_ms/(0.1*TAU_L);
    const int max_pwm = 100;
    const int min_pwm = -100;
    static float Ui = 0;
    static float Up = 0;

    for (;;)
    {
        // Receive the message sent by Task E
        xQueueReceive(qh, &pxRxedMessage, portMAX_DELAY);
        speed = quadEncoder_GetSpeedL();
        Motor_speed_Left = speed;
        consigne = pxRxedMessage.data;

        error = consigne - speed;
        Up = Kp * (float) error ;
        Ui = Ui + Kp * Ki * (float) error;
        pwm_output = (int) (Up + Ui);
        if (pwm_output > max_pwm)
                	        {
                	            pwm_output = max_pwm;
                	        }
                	        else if (pwm_output < min_pwm)
                	        {
                	            pwm_output = min_pwm;
                	        }
        motorLeft_SetDuty(pwm_output+100);
        xSemaphoreGive(xSemaphore);
    }
}

static void task_LCD(void *pvParameters){
  for (;;){
    int state;
    xQueueReceive(data_lcd, &state, portMAX_DELAY);
    switch (state){
        case AUTO:
            groveLCD_setCursor(0,0);
            groveLCD_term_printf("AUTO      ");
          break;
        case REMOTE:
          groveLCD_setCursor(0,0);
          groveLCD_term_printf("REMOTE      ");
          break;
        case COLOR:
          groveLCD_setCursor(0,0);
          groveLCD_term_printf("COLOR       ");
          break;
    }
  }
}

//=========================================================================
int main(void)
{
  int ret=0;
  HAL_Init();
  SystemClock_Config();
  MX_GPIO_Init();
  MX_DMA_Init();
  MX_USART1_UART_Init();
  MX_USART2_UART_Init();
  MX_I2C1_Init();

  motorCommand_Init();
  quadEncoder_Init();
  captDistIR_Init();

  HAL_Delay(500);

  VL53L0X_init();
  ret = VL53L0X_validateInterface();
  VL53L0X_startContinuous(0);

  groveLCD_begin(16,2,0);
  HAL_Delay(100);
  groveLCD_display();
  groveLCD_setCursor(0,0);

  if(ret ==0) 	{ groveLCD_term_printf("VL53L0X OK"); }
  else 			{ groveLCD_term_printf("VL53L0X HS"); }

  HAL_Delay(4000);

#if FIND_MOTOR_INIT_POS

	Left_first_index_reached = 0;
	 while( Left_first_index_reached != 1 )
	 {
		motorLeft_SetDuty(130);
		HAL_Delay(SAMPLING_PERIOD_ms);
		speed = quadEncoder_GetSpeedL();
	 }

	Right_first_index_reached = 0;
	 while( Right_first_index_reached != 1 )
	 {
		motorRight_SetDuty(130);
		HAL_Delay(SAMPLING_PERIOD_ms);
		speed = quadEncoder_GetSpeedR();
	 }

	 motorLeft_SetDuty(100);
	 motorRight_SetDuty(100);
	 HAL_Delay(200);

	 speed = quadEncoder_GetSpeedL();
	 speed = quadEncoder_GetSpeedR();
#endif

  osKernelInitialize();

  xTaskCreate( microros_task, ( const portCHAR * ) "microros_task", 3000 /* stack size */, NULL,  24, NULL );

	xTaskCreate( task_command, ( const portCHAR * ) "task C", 128 /* stack size */, NULL, 28, NULL );
	xTaskCreate( task_MotorR, ( const portCHAR * ) "task R", 128 /* stack size */, NULL, 28, NULL );
	xTaskCreate(task_MotorL, ( const portCHAR * ) "task L", 128 /* stack size */, NULL, 28, NULL);
	xTaskCreate(task_LCD, ( const portCHAR * ) "task D", 128 /* stack size */, NULL, 21, NULL);

	vSemaphoreCreateBinary(xSemaphore);
	xSemaphoreTake( xSemaphore, portMAX_DELAY );

	qh = xQueueCreate( 1, sizeof(struct AMessage ) ); // queue for speed command
  data_lcd = xQueueCreate(1, sizeof(int));

  osKernelStart();

  while (1)
  {}

}

//=========================================================================
bool cubemx_transport_open(struct uxrCustomTransport * transport);
bool cubemx_transport_close(struct uxrCustomTransport * transport);
size_t cubemx_transport_write(struct uxrCustomTransport* transport, const uint8_t * buf, size_t len, uint8_t * err);
size_t cubemx_transport_read(struct uxrCustomTransport* transport, uint8_t* buf, size_t len, int timeout, uint8_t* err);

void * microros_allocate(size_t size, void * state);
void microros_deallocate(void * pointer, void * state);
void * microros_reallocate(void * pointer, size_t size, void * state);
void * microros_zero_allocate(size_t number_of_elements, size_t size_of_element, void * state);

void subscription_callback(const void * msgin)
{

/*
 * Parsing
 *
 * 'M<mode>'
 *                   - "manual"
 *                   - "random"
 *                   - "tracking"
 *
 * 'V<valeur>'     Speed (%int)
 *
 * 'S<valeur>'     MaxSpeed (int)
 *
 * 'S<valeur>'     TrackingSpeed (%int)
 *
 * 'D<valeur>'     Direction (int)
 *
 * 'T<valeur>'     Tracking error (no error is TRACKING_OFFSET/2),
 *                        TRACKING_OFFSET = error_range/2
 *
 */


    const std_msgs__msg__String * msg = (const std_msgs__msg__String *)msgin;

    // if message is null
    if (!msg || !msg->data.data || msg->data.size == 0)
        return;

    const char *message = msg->data.data;
    int value;

    switch (message[0]) {
        case 'M':
            if (strcmp(message, "Mmanual") == 0) {    // manual
                state = REMOTE;
                update_lcd = true;
                speed_remote = 0;
                direction_remote = 0;
            } else if (strcmp(message, "Mrandom") == 0) { // random
                state = AUTO;
                update_lcd = true;

            } else if (strcmp(message, "Mtracking") == 0) {   // tracking
                state = COLOR;
                update_lcd = true;
            }
        break;
        case 'V':
			if (sscanf(message, "V%d", &value) == 1)
				speed_remote = -(value - 100) * max_speed /40;
			break;

		case 'T':
			if (sscanf(message, "T%d", &value) == 1)
				errorX = value;
			break;

		case 'S':
			if (sscanf(message, "S%d", &value) == 1)
				max_speed = value;
			break;

		case 'D':
			if (sscanf(message, "D%d", &value) == 1)
				direction_remote = value - 100;
			break;

		case 'B':
			if (sscanf(message, "B%d", &value) == 1)
				speed_tracking = value * 4;
			break;
        default:
        break;
    }
}


void microros_task(void *argument)
{
  rmw_uros_set_custom_transport( true, (void *) &huart1, cubemx_transport_open,  cubemx_transport_close,  cubemx_transport_write, cubemx_transport_read);

  rcl_allocator_t freeRTOS_allocator = rcutils_get_zero_initialized_allocator();
  freeRTOS_allocator.allocate = microros_allocate;
  freeRTOS_allocator.deallocate = microros_deallocate;
  freeRTOS_allocator.reallocate = microros_reallocate;
  freeRTOS_allocator.zero_allocate =  microros_zero_allocate;

  if (!rcutils_set_default_allocator(&freeRTOS_allocator)) {
      printf("Error on default allocators (line %d)\n", __LINE__);
  }

  // micro-ROS app
  rclc_support_t support;
  rcl_allocator_t allocator;
  allocator = rcl_get_default_allocator();

  // create node
  rcl_node_t node;
  rcl_node_options_t node_ops = rcl_node_get_default_options();
  rcl_init_options_t init_options = rcl_get_zero_initialized_init_options();
  rcl_init_options_init(&init_options, allocator);
  rcl_init_options_set_domain_id(&init_options, ROS_DOMAIN_ID);

  rclc_support_init_with_options(&support, 0, NULL, &init_options, &allocator);
  rclc_node_init_default(&node, "STM32_Node","", &support);

  rcl_publisher_t publisher;
  std_msgs__msg__String sensor_dist_back_msg;
  rclc_publisher_init_default(&publisher,
		  &node,
		  ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, String),
		  "/feedback");

  rcl_subscription_t subscriber;
  std_msgs__msg__String str_msg;
  rclc_subscription_init_default(&subscriber,
		  &node,ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, String),
		  "/orders");

  rclc_executor_t executor;
  rclc_executor_init(&executor, &support.context, 1, &allocator); // ! 'NUMBER OF HANDLES' A MODIFIER EN FONCTION DU NOMBRE DE TOPICS
  rclc_executor_add_subscription(&executor, &subscriber, &str_msg, &subscription_callback, ON_NEW_DATA);


  str_msg.data.data = (char * ) malloc(ARRAY_LEN * sizeof(char));
  str_msg.data.size = 0;
  str_msg.data.capacity = ARRAY_LEN;

  for(;;)
  {
	  /*
		int Motor_speed_Right = 0;
		int Motor_speed_Left = 0;
		int sensor_left = 0;
		int sensor_right = 0;
		int sensor_back = 0;
	   */

      // Speed
    sprintf(str_msg.data.data, "Speed#%d", (int)((Motor_speed_Left + Motor_speed_Right) / 2) + 1000);
	str_msg.data.size = strlen(str_msg.data.data);
	rcl_ret_t ret = rcl_publish(&publisher, &str_msg, NULL);
    if (ret != RCL_RET_OK) { groveLCD_setCursor(0,1); groveLCD_term_printf("Error com"); }

    // DistBack
	sprintf(str_msg.data.data, "SensorBack#%d", (int)sensor_back);
	str_msg.data.size = strlen(str_msg.data.data);
	ret = rcl_publish(&publisher, &str_msg, NULL);
	if (ret != RCL_RET_OK) { groveLCD_setCursor(0,1); groveLCD_term_printf("Error com"); }

	// Sensor Front Left
	sprintf(str_msg.data.data, "SensorL#%d", (int)sensor_left);
	str_msg.data.size = strlen(str_msg.data.data);
	ret = rcl_publish(&publisher, &str_msg, NULL);
	if (ret != RCL_RET_OK) { groveLCD_setCursor(0,1); groveLCD_term_printf("Error com"); }

	// Sensor Front Right
	sprintf(str_msg.data.data, "SensorR#%d", (int)sensor_right);
	str_msg.data.size = strlen(str_msg.data.data);
	ret = rcl_publish(&publisher, &str_msg, NULL);
	if (ret != RCL_RET_OK) { groveLCD_setCursor(0,1); groveLCD_term_printf("Error com"); }


    rclc_executor_spin_some(&executor, RCL_MS_TO_NS(10));
    osDelay(10);
  }
}
//=========================================================================


/**
  * @brief  Period elapsed callback in non blocking mode
  * @note   This function is called  when TIM1 interrupt took place, inside
  * HAL_TIM_IRQHandler(). It makes a direct call to HAL_IncTick() to increment
  * a global variable "uwTick" used as application time base.
  * @param  htim : TIM handle
  * @retval None
  */
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
  if (htim->Instance == TIM4)
  {
    HAL_IncTick();
  }
}
//=========================================================================
void Error_Handler(void)
{
  __disable_irq();
  while (1)
  {}
}
//=========================================================================
#ifdef  USE_FULL_ASSERT
/**
  * @brief  Reports the name of the source file and the source line number
  *         where the assert_param error has occurred.
  * @param  file: pointer to the source file name
  * @param  line: assert_param error line source number
  * @retval None
  */
void assert_failed(uint8_t *file, uint32_t line)
{
  /* USER CODE BEGIN 6 */
  /* User can add his own implementation to report the file name and line number,
     ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
  /* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */

