#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;
    //INT8U err_code;
    
  while (1)
  { 
    if(RS232_fp != NULL)
    {    
        //rx_msg = fgetc(RS232_fp);
        memset(rx_msg_str, 0, BUFFER_SIZE + 1);
        
        fgets(rx_msg_str, BUFFER_SIZE + 1, RS232_fp);
        
        /**/
        if((rx_msg_str[0] == '^') || (rx_msg_str[1] == '^') || (rx_msg_str[2] == '^'))
        {
         roberts_msg_str[0] == '^';   
            if((rx_msg_str[1] >= 'a') && (rx_msg_str[1] <= 'z') || (rx_msg_str[1] >= 'a') && (rx_msg_str[1] <= 'z'))
            {   
                if((rx_msg_str[2] >= 'a') && (rx_msg_str[2] <= 'z'))
                {  
                    printf("the current string is: %s \n", rx_msg_str);
                
                    if(OSQPost(Cmd_Q, rx_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* rx_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 test1 = 2;
    char test2 = 2;
    
    char steering = 0;
    char motor = 0;
    
    while(1)
    {
        memset(rx_msg_str, 0, BUFFER_SIZE + 1);
        rx_msg_str = (char*) OSQPend(Cmd_Q, 0, &err_code);
        
        if(err_code == OS_NO_ERR)
        {
        
            test1 = (rx_msg_str[1] >= 'a') && (rx_msg_str[1] <= 'm');           
            test2 = (rx_msg_str[1] >= 'n') && (rx_msg_str[1] <= 'z');
            //printf("rx_msg_str: %s -- test1: %d -- test2: %d\n", rx_msg_str, test1, test2);
        
            if((rx_msg_str[1] >= 'a') && (rx_msg_str[1] <= 'm'))
            {
                if((rx_msg_str[2] >= 'n') && (rx_msg_str[2] <= 'z')) // 1st char is steering, 2nd char is motor
                {
                    steering = CAR_STEERING_HALF_DUTY_CYCLE + rx_msg_str[1] - 'g';
                    motor = MOTOR_HALF_DUTY_CYCLE + rx_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;
                }
        
                else
                    printf("\n%s: 2nd char invalid.\n", rx_msg_str);
            }
    
            else if((rx_msg_str[1] >= 'n') && (rx_msg_str[1] <= 'z'))
            {
                if((rx_msg_str[2] >= 'a') && (rx_msg_str[2] <= 'm')) // 1st char is motor, 2nd char is steering
                {
                    /*
                    printf("Motor: %c -- Steering: %c\n", rx_msg_str[0], rx_msg_str[1]);
            
                    *STEERING_BASE = CAR_STEERING_HALF_DUTY_CYCLE + rx_msg_str[1] - 'g';
                    *MOTOR_BASE = MOTOR_HALF_DUTY_CYCLE + rx_msg_str[0] - 't';
                    */
                    
                    steering = CAR_STEERING_HALF_DUTY_CYCLE + rx_msg_str[2] - 'g';
                    motor = MOTOR_HALF_DUTY_CYCLE + rx_msg_str[1] - 't';
                    
                    printf("Steering: %c -- Motor: %c\n", rx_msg_str[2], rx_msg_str[1]);
                    printf("Steering: %d -- Motor: %d\n\n", steering, motor);
                    
                    *STEERING_BASE = steering;
                    *MOTOR_BASE = motor;
                }
        
                else
                    printf("\n%s: 2nd char invalid.\n", rx_msg_str);
            }
    
            else
            {
                printf("\n%s: 1st char invalid.\n", rx_msg_str);
                
                if((rx_msg_str[2] < 'a') || (rx_msg_str[2] > 'z'))
                {
                    *STEERING_BASE = CAR_STEERING_HALF_DUTY_CYCLE;
                    *MOTOR_BASE = MOTOR_HALF_DUTY_CYCLE;
                }
            }
    
        }
    
            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;
}
