/*************************************************************************
/*
 * Title: RC Car Controller
 * Authors: Robert Hood, Max Marcus, and Barry Peyton
 * Course: CMPE 450 (CmpE Nano Design Project)
 * Date: April 13, 2012
 *
**************************************************************************/

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

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

/* Definition of Task Priorities */
#define TASK1_PRIORITY      1
#define TASK2_PRIORITY      3
#define TASK3_PRIORITY      2

/* Base Addresses of LEDS and RS-232 */
#define LEDS (char * ) GREEN_LEDS_BASE
#define RS232 (char *) "/dev/RS_232_UART"


/*Car Steering Variables*/
#define STEERING_BASE (char*)STEERING_PWM_BASE
#define CAR_STEERING_HALF_DUTY_CYCLE 18

/* Motor controls*/
#define MOTOR_BASE (char*)MOTOR_PWM_BASE
#define MOTOR_HALF_DUTY_CYCLE 18

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

#define BUFFER_SIZE 3
FILE* RS232_fp;

/*
 * task1 simply blinks the 8 green LEDs once per second. This lets us know that the
 * application is running on the FPGA.
 */
void task1(void* pdata)
{
    char flip = 0;
    
  while (1)
  {
    if(flip){
        *LEDS = 0xff;
    } else {
        *LEDS = 0x00;
    }
    flip = ~flip;
    
    OSTimeDlyHMSM(0, 0, 1, 0);
  }
}


/*
 * task2 uses fgets() to collect chars as they arrive at the RS-232 UART core. It then checks
 * to make sure the three characters are valid command characters and if so, puts them into
 * the string rx_msg_str. A valid sequence of command characters must consist of:
 * 
 * 1) The '^' ASCII char
 * 2) One ASCII char between 'b' and 'l'
 * 3) One ASCII char between 'o' and 'y'
 * 
 * If rx_msg_char is not loaded with a valid sequence by fgets(), the sequence is discarded and 
 * the loop begins again. If it is valid, it is posted the the Message Queue.
 */
void task2(void* pdata)
{
  char* rx_msg_str = malloc(sizeof(char) * BUFFER_SIZE + 1);
  char* tx_msg_str = malloc(sizeof(char) * BUFFER_SIZE + 1);
  
  int i=0;
  int j=0;
  
  while (1){ 
    if(RS232_fp != NULL){  
        
        memset(rx_msg_str, '\0', BUFFER_SIZE + 1);
        memset(tx_msg_str, '\0', BUFFER_SIZE + 1);
        
        // Gets the command chars from the RS_232 file stream and stores them in rx_msg_str
        fgets(rx_msg_str, BUFFER_SIZE + 1, RS232_fp);
        
        // Ensures that all characters are of the correct type and then sorts the chars into
        // the correct order to be read by task3.
        if( (rx_msg_str[0] == '^' || rx_msg_str[1] == '^' || rx_msg_str[2] == '^') && 
          (((rx_msg_str[0] >= 'b') && (rx_msg_str[0] <= 'l')) || ((rx_msg_str[1] >= 'b') && (rx_msg_str[1] <= 'l')) || ((rx_msg_str[2] >= 'b') && (rx_msg_str[2] <= 'l'))) && 
          (((rx_msg_str[0] >= 'o') && (rx_msg_str[0] <= 'y')) || ((rx_msg_str[1] >= 'o') && (rx_msg_str[1] <= 'y')) || ((rx_msg_str[2] >= 'o') && (rx_msg_str[2] <= 'y')))) 
        {   

            tx_msg_str[0] = '^';
          
            for(i=0; i<3; i++)
            {
                 if( (rx_msg_str[i] >= 'b') && (rx_msg_str[i] <= 'l'))
                        tx_msg_str[1] = rx_msg_str[i];
            }
    
            for(j=0; j<3; j++)
            {
                 if( (rx_msg_str[j] >= 'o') && (rx_msg_str[j] <= 'y'))
                        tx_msg_str[2] = rx_msg_str[j];
            }
            
                // Posts the sorted command sequence to Cmd_Q
                OSQPost(Cmd_Q, tx_msg_str);
            } 
        }
        OSTimeDlyHMSM(0, 0, 0.05, 0);
     }
        
}


/*
 * task3 pends command sequences from the Message Queue (cmd_Q) and sends them to the PWM modules,
 * after converting them to the correct numeric constants for the appropriate duty cycles.
 */
 
//  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   
 
void task3(void* pdata)
{

    
    char* rx_msg_str = malloc(sizeof(char) * BUFFER_SIZE + 1);
    memset(rx_msg_str, '\0', BUFFER_SIZE + 1);
    INT8U err_code;
    
    char steer_check=CAR_STEERING_HALF_DUTY_CYCLE;
    char motor_check=MOTOR_HALF_DUTY_CYCLE;
    
    while(1)
    {
       // Posts the command sequence from Cmd_Q
       rx_msg_str = (char *) OSQPend(Cmd_Q, 0, &err_code);      
       
       // Conversion to appropriate numeric constants for PWM duty cycles
       steer_check = CAR_STEERING_HALF_DUTY_CYCLE + rx_msg_str[1] - 'g';
       motor_check = MOTOR_HALF_DUTY_CYCLE + rx_msg_str[2] - 't';       
       
       // Sends the duty cycles to the PWM_Steering and PWM_Motor modules.
       *STEERING_BASE = steer_check;
       *MOTOR_BASE = motor_check;
                    
       OSTimeDlyHMSM(0, 0, 0.05, 0);
    }
}

/* The main function creates two task and starts multi-tasking */
int main(void)
{
  
/* Instantiation of Message Queue */  
  Cmd_Q = OSQCreate(&StoarageBaseAddress[0], Q_SIZE);

/* Creates the three tasks */
  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);
  
    OSTaskCreateExt(task3,
                  NULL,
                  (void *)&task3_stk[TASK_STACKSIZE-1],
                  TASK3_PRIORITY,
                  TASK3_PRIORITY,
                  task3_stk,
                  TASK_STACKSIZE,
                  NULL,
                  0);
  
  RS232_fp = fopen(RS232, "r"); // The file pointer to the input stream from the RS-232 UART
  
  OSStart(); // Starts the tasks
  return 0;
}
