/*************************************************************************
* Copyright (c) 2004 Altera Corporation, San Jose, California, USA.      *
* All rights reserved. All use of this software and documentation is     *
* subject to the License Agreement located at the end of this file below.*
**************************************************************************
* Description:                                                           *
* The following is a simple hello world program running MicroC/OS-II.The * 
* purpose of the design is to be a very simple application that just     *
* demonstrates MicroC/OS-II running on NIOS II.The design doesn't account*
* for issues such as checking system call return codes. etc.             *
*                                                                        *
* Requirements:                                                          *
*   -Supported Example Hardware Platforms                                *
*     Standard                                                           *
*     Full Featured                                                      *
*     Low Cost                                                           *
*   -Supported Development Boards                                        *
*     Nios II Development Board, Stratix II Edition                      *
*     Nios Development Board, Stratix Professional Edition               *
*     Nios Development Board, Stratix Edition                            *
*     Nios Development Board, Cyclone Edition                            *
*   -System Library Settings                                             *
*     RTOS Type - MicroC/OS-II                                           *
*     Periodic System Timer                                              *
*   -Know Issues                                                         *
*     If this design is run on the ISS, terminal output will take several*
*     minutes per iteration.                                             *
**************************************************************************/


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

/* Function Declarations */
int dampen_esc(char* input, char* output);

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

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


void task1(void* pdata)
{
  char flip = 0;
    
  while (1)
  {
    if(flip)
        *LEDS = 0xff;

    else 
        *LEDS = 0x00;

    flip = ~flip;   
    
    OSTimeDlyHMSM(0, 0, 1, 0);
  }
}


/**/
void task2(void* pdata)
{
  char* rx_msg_str = malloc(sizeof(char) * BUFFER_SIZE + 1);
  char* tx_msg_str = malloc(sizeof(char) * BUFFER_SIZE + 1);
  
  char current_msg_str[] = "^gt";
  char bad_data = 0;
  char damp_err = 0;

  INT8U err_code;
  
  while (1){ 
    if(RS232_fp != NULL)
    {         
        memset(rx_msg_str, '\0', BUFFER_SIZE + 1);
        memset(tx_msg_str, '\0', BUFFER_SIZE + 1);
        
        fgets(rx_msg_str, BUFFER_SIZE + 1, RS232_fp);
        
        printf("Received String is: %s\n", rx_msg_str);
        
        if(rx_msg_str[0] == '^' || rx_msg_str[1] == '^' || rx_msg_str[2] == '^')
        {   

            tx_msg_str[0] = '^';
            
            if(rx_msg_str[0] != '^')
            {
                if((rx_msg_str[0] >= 'b') && (rx_msg_str[0] <= 'l'))
                {  
                    tx_msg_str[1] = rx_msg_str[0];
                } 
                else if ((rx_msg_str[0] >= 'o') && (rx_msg_str[0] <= 'y'))
                { 
                    tx_msg_str[2] = rx_msg_str[0];
                }
                else
                    bad_data = 1;
            }
            
            if(rx_msg_str[1] != '^')
            {   
                if((rx_msg_str[1] >= 'b') && (rx_msg_str[1] <= 'l'))
                {  
                    tx_msg_str[1] = rx_msg_str[1];
                } 
                else if ((rx_msg_str[0] >= 'o') && (rx_msg_str[1] <= 'y'))
                { 
                    tx_msg_str[2] = rx_msg_str[1];
                }
                else
                    bad_data = 1;
            }
            
            if(rx_msg_str[2] != '^')
            {   
                if((rx_msg_str[2] >= 'b') && (rx_msg_str[2] <= 'l'))
                {  
                    tx_msg_str[1] = rx_msg_str[2];
                } 
                else if ((rx_msg_str[2] >= 'o') && (rx_msg_str[2] <= 'y'))
                { 
                    tx_msg_str[2] = rx_msg_str[2];
                }
                else
                    bad_data = 1;
            } 
        }
        
        else
            bad_data = 1;
        
        // if 1st char is steering, 2nd char is motor
        if((tx_msg_str[1] >= 'b') && (tx_msg_str[1] <= 'l') && (tx_msg_str[2] >= 'o') && (tx_msg_str[2] <= 'y'))
        {
            // Do Nothing            
        }
        
        // if 1st char is motor, 2nd char is steering
        else if((tx_msg_str[1] >= 'o') && (tx_msg_str[1] <= 'y') && (tx_msg_str[2] >= 'b') && (tx_msg_str[2] <= 'l'))
        {
            char temp = tx_msg_str[1];
            tx_msg_str[1] = tx_msg_str[2];
            tx_msg_str[2] = temp;
        }
        
        else
            bad_data = 1;
        
        if(bad_data)
            damp_err = dampen_esc(current_msg_str, tx_msg_str);
        
        if(damp_err < 0)
        {
            tx_msg_str[0] = '^';
            tx_msg_str[1] = 'g';
            tx_msg_str[2] = 't';
            //tx_msg_str[3] = '\0';
        }
        
        bad_data = 0;
        strncpy(current_msg_str, tx_msg_str, BUFFER_SIZE + 1);
        err_code = OSQPost(Cmd_Q, tx_msg_str);
        
        if(err_code != OS_NO_ERR)
            printf("Error posting to queue Cmd_Q.\n");
     }
     
     OSTimeDlyHMSM(0, 0, 0.05, 0);
   }    
}
/**/

/*
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);
          
        fgets(rx_msg_str, BUFFER_SIZE + 1, RS232_fp);
        
        //printf("Received String is: %s\n", rx_msg_str);
        
        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];
            }
            
            OSQPost(Cmd_Q, tx_msg_str);
            //printf("Transmitted String is: %s\n", tx_msg_str);
        }
    }
    
    OSTimeDlyHMSM(0, 0, 0.05, 0);
  }   
}
*/


void task3(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);
    memset(rx_msg_str, '\0', BUFFER_SIZE + 1);
    rx_msg_str[0] = '^';
    rx_msg_str[1] = 'g';
    rx_msg_str[2] = 't';
    rx_msg_str[3] = '\0';
    
    //char rx_msg_str[] = "^gt";
    char current_msg_str[] = "^gt";
    
    INT8U err_code;
    
    char steer_check = CAR_STEERING_HALF_DUTY_CYCLE;
    //char steer_check_2=CAR_STEERING_HALF_DUTY_CYCLE;
    char motor_check = MOTOR_HALF_DUTY_CYCLE;
    //char motor_check_2=MOTOR_HALF_DUTY_CYCLE;
    char damp_err = 0;
    
    while(1)
    {
        rx_msg_str = (char *) OSQPend(Cmd_Q, 1 * alt_ticks_per_second(), &err_code);
    
        printf("Pended String is: %s\n", rx_msg_str);

        if(err_code == OS_TIMEOUT)
        {
            damp_err = dampen_esc(current_msg_str, rx_msg_str);
            printf("Slowing down due to lost connection.\n");
        }
    
        else if(err_code != OS_NO_ERR)
        {
            damp_err = dampen_esc(current_msg_str, rx_msg_str);
            printf("Unexpected error pending from queue Cmd_Q.\n");
        }
        
        if(damp_err < 0)
        {
            rx_msg_str[0] = '^';
            rx_msg_str[1] = 'g';
            rx_msg_str[2] = 't';
            rx_msg_str[3] = '\0';
        }
        
        strncpy(current_msg_str, rx_msg_str, BUFFER_SIZE + 1);
        printf("damp_err: %d  --  Sending: %s\n", damp_err, rx_msg_str);
        
        damp_err = 0;
        
        steer_check = CAR_STEERING_HALF_DUTY_CYCLE + rx_msg_str[1] - 'g';
        motor_check = MOTOR_HALF_DUTY_CYCLE + rx_msg_str[2] - 't';
       
        //steer_check = CAR_STEERING_HALF_DUTY_CYCLE;
        //motor_check = MOTOR_HALF_DUTY_CYCLE;
        //int k=fabs(steer_check - steer_check_2);
        //int r=fabs (motor_check - motor_check_2);
       
        //printf("The difference in steering is: %d\n", k);
        //printf("The difference in motor is: %d\n", r);
        //if( k < 2 ) && r < 2))
        //{
        //steer_check_2=steer_check;
        //motor_check_2=motor_check;
        
        
        
        *STEERING_BASE = steer_check;
        *MOTOR_BASE = motor_check;
        //}
                    
        OSTimeDlyHMSM(0, 0, 0.05, 0);
    }
}


int dampen_esc(char* input, char* output)
{
    char neutral[] = "^gt";
    memset(output, '\0', BUFFER_SIZE + 1);
    printf("input: %s\n", input);
    
    if(strlen(input) != 3)
    {
        printf("Error1 in dampen_esc. Invalid string length.\n");
        output[1] = 'g';
        output[2] = 't';
        output[3] = '\0';
             
        return -1;
    }

    else
    {
        char steer = input[1];
        char motor = input[2];
        /*
        output[0] = '^';
        output[1] = 'g';
        output[2] = 't';
        output[3] = '\0';
        */
        strncpy(output, neutral, BUFFER_SIZE + 1);
        
        printf("output: %s\n", output);
        
        if(steer < 'g')
            output[1] = steer + 1;
        else if(steer > 'g')  
            output[1] = steer - 1;
        else if(steer == 'g')  
            output[1] = 'g';
        else
        {
             printf("Error2 in dampen_esc. Invalid steering char.\n");
             output[1] = 'g';
             output[2] = 't';
             
             return -1;
        }
        
        if(motor < 't')
            output[2] = motor + 1;
        else if(motor > 't')  
            output[2] = motor - 1;
        else if(motor == 't')  
            output[2] = 't';
        else
        {
             printf("Error3 in dampen_esc. Invalid motor char.\n");
             output[1] = 'g';
             output[2] = 't';
             
             return -1;
        }
        
        printf("output: %s\n", output);
        return 0;
    }
}


/* 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);
  
    OSTaskCreateExt(task3,
                  NULL,
                  (void *)&task3_stk[TASK_STACKSIZE-1],
                  TASK3_PRIORITY,
                  TASK3_PRIORITY,
                  task3_stk,
                  TASK_STACKSIZE,
                  NULL,
                  0);
  
  RS232_fp = fopen(RS232, "r");
  
  OSStart();
  return 0;
}

/******************************************************************************
*                                                                             *
* License Agreement                                                           *
*                                                                             *
* Copyright (c) 2004 Altera Corporation, San Jose, California, USA.           *
* All rights reserved.                                                        *
*                                                                             *
* Permission is hereby granted, free of charge, to any person obtaining a     *
* copy of this software and associated documentation files (the "Software"),  *
* to deal in the Software without restriction, including without limitation   *
* the rights to use, copy, modify, merge, publish, distribute, sublicense,    *
* and/or sell copies of the Software, and to permit persons to whom the       *
* Software is furnished to do so, subject to the following conditions:        *
*                                                                             *
* The above copyright notice and this permission notice shall be included in  *
* all copies or substantial portions of the Software.                         *
*                                                                             *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR  *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,    *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER      *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING     *
* FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER         *
* DEALINGS IN THE SOFTWARE.                                                   *
*                                                                             *
* This agreement shall be governed in all respects by the laws of the State   *
* of California and by the laws of the United States of America.              *
* Altera does not recommend, suggest or require that this reference design    *
* file be used in conjunction or combination with any other product.          *
******************************************************************************/
