#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 15
#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      500 //tr.min⁻¹
#define DISTANCE_CRITIQUE 600 // to be modified !!!!!!!!!!!!
#define DELAY_WARNING     2500//ms  time to turn when distance critique are reached
#define DELAY_DT          1000//ms  time to turn when half turn is required
#define TRACKING_CST_Y       0.5 // constant for tracking proportionnal corrector
#define TRACKING_CST_X       0.1
// 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 5
//################################################
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;

/* 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);

//================== variables globale ===================
uint16_t mes_vl53=0;
// donnee envoyé à l'ihm
int Motor_speed_Right = 0;
int Motor_speed_Left = 0;
// color mode vars
int errorY= 0;
int errorX= 0;
// remote mode vars
int direction_remote =0;
int speed_remote = 0;
// current mode
int state = REMOTE;
bool update_lcd = true;

// current max speed
int max_speed= VMAX;

int Left_first_index_reached = 0;
int Right_first_index_reached = 0;

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

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

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

    bool demi_tour = false;
    int cnt_DT = 0;

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

    for (;;)
    {

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

        switch (state){
        case AUTO:
          captDistIR_Get(tab);
          int sensR = tab[0];
          int sensL = tab[1];
          bool distance_critique = sensL > DISTANCE_CRITIQUE*2 && sensR > DISTANCE_CRITIQUE*2;
          if (demi_tour){
            speedL = -max_speed/4;
            speedR = max_speed/4;
            cnt_DT --;
            if (cnt_DT <= 0){
              demi_tour = false;
            }
          }else{ // !demi_tour
            if (distance_critique){
              speedL = 0;
              speedR = 0;
              demi_tour = true;
              cnt_DT = DELAY_WARNING /SAMPLING_PERIOD_ms;
            }else{ // !distance_critique
              bool left_limit = sensL > DISTANCE_CRITIQUE;
              bool right_limit = sensR > DISTANCE_CRITIQUE;
              int env = left_limit | right_limit <<1;
              switch (env){
              case 0b01:// left_limit reached
            	speedL= max_speed/10;
                speedR= max_speed/2;
                break;
              case 0b10:// right_limit reached
            	speedR= max_speed/10;
                speedL= max_speed/2;
                break;
              case 0b11: // both right and left limit reached
                demi_tour = true;
                cnt_DT = DELAY_DT / SAMPLING_PERIOD_ms;
              case 0: // none of the limits reached
                speedL = max_speed;
                speedR = max_speed;
              }
            }
          }
          break;
        case REMOTE:
          speedL = speed_remote - direction_remote;
          speedR = speed_remote + direction_remote;
          break;
        case COLOR:
          speedL = TRACKING_CST_Y * errorY - TRACKING_CST_X * errorX;
          speedR = TRACKING_CST_Y * errorY + TRACKING_CST_X * errorX;
          break;
        }
        // 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);
        }
        vTaskDelay(SAMPLING_PERIOD_ms);
    }
}

static void task_MotorR(void *pvParameters)
{
    struct AMessage pxRxedMessage;
   	int speed = 0;
    int consigne = 1500;                  // Vitesse de consigne
    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;

		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;                  // Vitesse de consigne
    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;
  int speed;

  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);

  // Affichage via UART2 sur Terminal série $ minicom -D /dev/ttyACM0
  printf("hello\r\n"); // REM : ne pas oublier le \n

  VL53L0X_init();

  ret = VL53L0X_validateInterface();
  if(ret ==0)
  {
	  printf("VL53L0X OK\r\n");
  }
  else
  {
	  printf("!! PROBLEME VL53L0X !!\r\n");
  }
  VL53L0X_startContinuous(0);

  int a, b;
  groveLCD_begin(16,2,0); // !! cette fonction prend du temps
  HAL_Delay(100);
  groveLCD_display();
  a=5; b=2;
  groveLCD_term_printf("%d+%d=%d",a,b,a+b);
  groveLCD_setCursor(0,0);
  groveLCD_term_printf("hello");


  HAL_Delay(50);

#if FIND_MOTOR_INIT_POS

  //int16_t speed=0;
// RECHERCHE DE LA POSITION INITIALE ( 1er signal 'index' du capteur )
// Evite une erreur pour une mesure de vitesse 
	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)
{
  printf("STM received: %s\n\r", message);
  const std_msgs__msg__String * msg = (const std_msgs__msg__String *)msgin;
  char * message = msg->data.data;
  int value = 0;
  char *endptr;
  if (strcmp(message,  "Mode Manual") == 0){
    state=REMOTE;
    update_lcd = true;
    speed_remote=0;
    direction_remote = 0;
  }
  else if (strcmp(message, "Mode Random")==0){
    state = AUTO;
    update_lcd = true;
  }
  else if (strcmp(message, "Mode Tracking")==0){
    state = COLOR;
    update_lcd = true;
  }else if (*message == 'V'){
	char *ptr = &message[1];
    value= (int)strtol(ptr, &endptr, 10);
    speed_remote = value*max_speed/100;
  }else if (*message == 'S'){
		char *ptr = &message[1];
	    value= (int)strtol(ptr, &endptr, 10);
	    max_speed = value;
  }
  else if(*message == 'D'){
    char *ptr = &message[1];
    value= (int)strtol(ptr, &endptr, 10);
    direction_remote = value;
  }
  if (*message == 'X'){
	  char * ptr = &message[1];
	  errorX = (int)strtol(ptr, &endptr, 10);
	  endptr++;
	  errorY = (int)strtol(endptr, &endptr, 10);
  }
  // Process message
  //printf("Received from HOST: %s\n\r", msg->data);
  //modes messages:
  // 'Mode Manual"
  // 'Mode Random'
  // 'Mode Tracking'
  // 'D(int)direction' (for manual mode)
  // 'V(int)vitesse' (for manual mode)
  // 'S(int)max_speed' (define max speed)
}



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)\r\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);

  // create publisher
  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),"/sensor/dist_back");

  // create subscriber
  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),"/command/move");

    // Add subscription to the executor
  rclc_executor_t executor;
  rclc_executor_init(&executor, &support.context, 2, &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);
  //rclc_executor_add_subscription(&executor, &subscriber2, &str_msg2, &subscription_callback_raspberry, 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(;;)
  {
     /*sprintf(str_msg.data.data, "SpeedMotorR#%d", (int)Motor_speed_Right);

     str_msg.data.size = strlen(str_msg.data.data);
     rcl_ret_t ret = rcl_publish(&publisher, &str_msg, NULL);
     if (ret != RCL_RET_OK) { printf("Error publishing (line %d)\n\r", __LINE__); }
     */
     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 */
