#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include "includes.h"

/* Definition of Task Stacks */
#define   TASK_STACKSIZE       2048
OS_STK    task1_stk[TASK_STACKSIZE];
OS_STK    task2_stk[TASK_STACKSIZE];

/* Definition of Task Priorities */

#define TASK1_PRIORITY      2
#define TASK2_PRIORITY      1

#define RS232 (char *) "/dev/RS_232_UART"
#define BUFFER_SIZE 3


/*Car Steering Variables*/
#define STEERING_BASE (char*)MAX_PWM_SERVO_BASE

#define CAR_STEERING_COUNTER_DUTY_CYCLE 13
#define CAR_STEERING_HALF_DUTY_CYCLE 18
#define CAR_STEERING_CLOCKWISE_DUTY_CYCLE 23

/* Motor controls*/
#define MOTOR_BASE (char*)MAX_PWM_MOTOR_BASE

#define MOTOR_COUNTER_DUTY_CYCLE 13
#define MOTOR_HALF_DUTY_CYCLE 18
#define MOTOR_CLOCKWISE_DUTY_CYCLE 24

/*Message Queue Instantiation*/
#define Q_SIZE 15
OS_EVENT* Cmd_Q;
void* StoarageBaseAddress[Q_SIZE];

FILE* RS232_fp;

void task1(void* pdata)
{
    char* roberts_msg_str = malloc(sizeof(char) * BUFFER_SIZE + 1);
    char* rx_msg_str = malloc(sizeof(char) * BUFFER_SIZE + 1);
    char dummy = 1;
    
  while (1)
  { 
    if(RS232_fp != NULL)
    {    

        memset(rx_msg_str, 0, BUFFER_SIZE + 1);
        memset(roberts_msg_str, 0, BUFFER_SIZE + 1);
        
        fgets(rx_msg_str, BUFFER_SIZE + 1, RS232_fp);
        printf("the input string is: %s \n", rx_msg_str);
        printf("the binary input string is: %d %d %d \n",rx_msg_str[0],rx_msg_str[1],rx_msg_str[2]);
        
        /**/
        if((rx_msg_str[0] == '^') || (rx_msg_str[1] == '^') || (rx_msg_str[2] == '^'))
        {
         roberts_msg_str[0] = '^';   
            
            if( ((rx_msg_str[0] >= 'a') && (rx_msg_str[0] <= 'm')) || ((rx_msg_str[1] >= 'a') && (rx_msg_str[1] <= 'm')) || ((rx_msg_str[2] >= 'a') && (rx_msg_str[2] <= 'z')))
            {   
                if((rx_msg_str[0] >= 'a') && (rx_msg_str[0] <= 'm'))
                roberts_msg_str[1] = rx_msg_str[0];
                
                else if ((rx_msg_str[1] >= 'a') && (rx_msg_str[1] <= 'm'))
                roberts_msg_str[1] = rx_msg_str[1];
                
                else if ((rx_msg_str[2] >= 'a') && (rx_msg_str[2] <= 'm'))
                roberts_msg_str[1] = rx_msg_str[2];
                
                    if(((rx_msg_str[0] >= 'n') && (rx_msg_str[0] <= 'z')) || ((rx_msg_str[1] >= 'n') && (rx_msg_str[1] <= 'z')) || ((rx_msg_str[2] >= 'n') && (rx_msg_str[2] <= 'z')))
                    {  
                            if((rx_msg_str[0] >= 'n') && (rx_msg_str[0] <= 'z'))
                            roberts_msg_str[2] = rx_msg_str[0];
                
                            else if((rx_msg_str[1] >= 'n') && (rx_msg_str[1] <= 'z'))
                            roberts_msg_str[2] = rx_msg_str[1];
                
                            else if((rx_msg_str[2] >= 'n') && (rx_msg_str[2] <= 'z'))
                            roberts_msg_str[2] = rx_msg_str[2];
                   
                            printf("the current string is: %s \n", roberts_msg_str);
                
                                if(OSQPost(Cmd_Q, roberts_msg_str) != OS_NO_ERR)
                                dummy += 0;
                                //printf("Post to Cmd_Q failed.\n");
                    }
            }
        }
    }
  }
}

void task2(void* pdata)
{
    //  a   b   c   d   e   f   g   h   i   j   k   l   m   n   o   p   q   r   s   t   u   v   w   x   y   z
    //  -6  -5  -4  -3  -2  -1  0   1   2   3   4   5   6   -6  -5  -4  -3  -2  -1  0   1   2   3   4   5   6   
    
    char* control_msg_str = malloc(sizeof(char) * BUFFER_SIZE + 1);
    INT8U err_code;
    
    *STEERING_BASE = CAR_STEERING_HALF_DUTY_CYCLE;
    *MOTOR_BASE = MOTOR_HALF_DUTY_CYCLE;
   
    char steering = 0;
    char motor = 0;
    
    while(1)
    {
        memset(control_msg_str, 0, BUFFER_SIZE + 1);
        control_msg_str = (char*) OSQPend(Cmd_Q, 0, &err_code);
        
        if(err_code == OS_NO_ERR)
        {
            if(control_msg_str[0] == '^')
            {
                    steering = CAR_STEERING_HALF_DUTY_CYCLE + control_msg_str[1] - 'g';
                    motor = MOTOR_HALF_DUTY_CYCLE + control_msg_str[2] - 't';
                    
                    //printf("Steering: %c -- Motor: %c\n", rx_msg_str[1], rx_msg_str[2]);
                    //printf("Steering: %d -- Motor: %d\n", steering, motor);
                    
                    *STEERING_BASE = steering;
                    *MOTOR_BASE = motor;
            }
        }
            
      OSTimeDlyHMSM(0, 0, 0, 10);
    }
}



/* The main function creates two task and starts multi-tasking */
int main(void)
{


  Cmd_Q = OSQCreate(&StoarageBaseAddress[0], Q_SIZE);
  
  OSTaskCreateExt(task1,
                  NULL,
                  (void *)&task1_stk[TASK_STACKSIZE-1],
                  TASK1_PRIORITY,
                  TASK1_PRIORITY,
                  task1_stk,
                  TASK_STACKSIZE,
                  NULL,
                  0);

  OSTaskCreateExt(task2,
                  NULL,
                  (void *)&task2_stk[TASK_STACKSIZE-1],
                  TASK2_PRIORITY,
                  TASK2_PRIORITY,
                  task2_stk,
                  TASK_STACKSIZE,
                  NULL,
                  0);
  
  RS232_fp = fopen(RS232, "r");
  
  OSStart();
  return 0;
}
