/*
 *   ROBOTAR.C -- A simple RoboTar program with uCOS-II
 *   Written by: Rosbi Mamat 6/5/2014
 */

#include "..\ucosobj\kernel.h"              /* Always include these to use uCOS-II      */
#include "..\hal\hal_robo.h"                /*   and RoboTar HAL                        */

#define TASK_STK_SZ            128          /* Size of each task's stacks (# of bytes)  */
#define TASK_START_PRIO          1          /* Highest priority                         */
#define TASK_CHKCOLLIDE_PRIO     3
#define TASK_CTRLMOTOR_PRIO      4
#define TASK_CRUISE_PRIO         5          /* Lowest priority                          */
#define TASK_LIGHT_PHOBIA        2

OS_STK CtrlmotorStk[TASK_STK_SZ];           /* Task CtrlMotors stack                    */
OS_STK CruiseStk[TASK_STK_SZ];              /* Task NavigRobot stack                    */
OS_STK TaskStartStk[TASK_STK_SZ];           /* TaskStartTask stack                      */
OS_STK ChkCollideStk[TASK_STK_SZ];          /* Task StopOnCollide stack                 */
OS_STK LightPhobiaStk[TASK_STK_SZ];

/* ------ Global shared variable -------*/
/* Ideally, this should be protected by a semaphore etc */
struct robostate
{
    int rspeed;                             /* right motor speed  (-100 -- +100)        */
    int lspeed;                             /* leftt motor speed  (-100 -- +100)        */
    char collided;                          /* collision? 1 = yes, 0 = no               */
	char light_side;
} myrobot;

/*------High pririority task----------*/
void CheckCollision (void *data)
{
    for(;;)
    {
        if ( (robo_bumpSensorR() == HIT) ||         /* collide ?                         */
             (robo_bumpSensorL() == HIT)    )
        {
            myrobot.collided = 1;                   /* signal that collisioin            */
         }
        else
        {
            myrobot.collided = 0;                   /* signal no collisioin              */
        }
        OSTimeDlyHMSM(0, 0, 0, 100);                /* Task period ~ 100 ms              */
    }
}

/* Lowest Priority Task - Light Chase Task*/
void LightPhobia (void *data)
{
    int r_light;
	int l_light;
	int light_diff;

    for(;;)
    {
		r_light = robo_lightSensorR();
		l_light = robo_lightSensorL();
		light_diff = r_light - l_light;

		//TURN RIGHT
		if (r_light > 400 && l_light > 400 && light_diff > 30)
		{
			myrobot.light_side = 2;
		}

		//TURN LEFT
		else if (r_light > 400 && l_light > 400 && light_diff < -30)
		{
			myrobot.light_side = 1;
		}

		else //if (light_diff >= -30 && light_diff <= 30)
		{
			myrobot.light_side = 0;
		}

		OSTimeDlyHMSM(0, 0, 0, 200);                /* Task period ~ 200 ms                  */
    }
}



/* Control robot Motors TASK */
void CntrlMotors (void *data)
{
    int speed_r, speed_l;

    for(;;)
    {
        speed_r = myrobot.rspeed;
        speed_l = myrobot.lspeed;
        robo_motorSpeed(speed_l, speed_r);
        OSTimeDlyHMSM(0, 0, 0, 200);                /* Task period ~ 200 ms              */
    }
}

/* -----------------------------------------------
 * Task for navigating robot
 * Write you own navigation task here
 */

//void Cruise (void *data)
//{
//    int dist;
//
//    for (;;)
//    {
//        if (myrobot.collided == 1)                  /* If collided then reverse              */
//        {
//            myrobot.rspeed   = -LOW_SPEED;          /* REVERSE */
//            myrobot.lspeed   = -LOW_SPEED;
//        }
//        else                                        /* obstacle is far away & no collision   */
//        {
//            myrobot.rspeed   = LOW_SPEED;           /* move forward at straight              */
//           myrobot.lspeed   = LOW_SPEED - 8;
//        }
//        OSTimeDlyHMSM(0, 0, 0, 500);                /* Task period ~ 500 ms                  */
//    }
//}

void Cruise (void *data)
{
	int dist;

    for (;;)
    {
        dist = robo_distSensor();                   /* Read distance sensor                 */

		if (myrobot.collided == 1)             /* If collided then stop                */
        {
            myrobot.rspeed   = STOP_SPEED;          /* STOP */
            myrobot.lspeed   = STOP_SPEED;
			OSTimeDlyHMSM(0, 0, 0, 100);              /* Wait 1 sec for stop to complete      */

			myrobot.rspeed   = MEDIUM_SPEED;         /* REVERSE */
            myrobot.lspeed   = -MEDIUM_SPEED;
			OSTimeDlyHMSM(0, 0, 2, 0);              /* Wait 2 sec for turning right to complete    */
        }

		else if(dist < 35)
		{
			robo_Honk();
			if(myrobot.light_side == 0 ){
				myrobot.rspeed   =  LOW_SPEED;       /* turn right                            */
				myrobot.lspeed   =  -LOW_SPEED;
				OSTimeDlyHMSM(0, 0, 0, 200);          /* Wait 200 ms for right turn to complete */
			}

			else if(myrobot.light_side == 1 ){
				myrobot.rspeed   = -LOW_SPEED;       /* turn right                            */
				myrobot.lspeed   =  LOW_SPEED;
				OSTimeDlyHMSM(0, 0, 0, 200);          /* Wait 200 ms for right turn to complete */
			}

			else if(myrobot.light_side == 2 ){
				myrobot.rspeed   =  LOW_SPEED;       /* turn left                            */
				myrobot.lspeed   = -LOW_SPEED;
				OSTimeDlyHMSM(0, 0, 0, 200);          /* Wait 200 ms for right turn to complete */
			}
		}
		else
		{
			myrobot.rspeed   = LOW_SPEED;          /* move forward at straight              */
            myrobot.lspeed   = LOW_SPEED;
		}

        OSTimeDlyHMSM(0, 0, 0, 100);                /* Task period ~ 100 ms                  */
    }
}


/*------Highest pririority task----------*/
/* Create all other tasks here           */
void TaskStart( void *data )
{
    OS_ticks_init();                                        /*-enable RTOS timer tick        */

	OSTaskCreate(LightPhobia,                            /*-Task function                 */
                (void *)0,                                  /*-nothing passed to task        */
                (void *)&LightPhobiaStk[TASK_STK_SZ - 1],    /*-stack allocated to task       */
                TASK_LIGHT_PHOBIA);                      /*-priority of task              */

    OSTaskCreate(CheckCollision,                            /*-Task function                 */
                (void *)0,                                  /*-nothing passed to task        */
                (void *)&ChkCollideStk[TASK_STK_SZ - 1],    /*-stack allocated to task       */
                TASK_CHKCOLLIDE_PRIO);                      /*-priority of task              */

    OSTaskCreate(CntrlMotors,                               /*-Task function                 */
                (void *)0,                                  /*-nothing passed to task        */
                (void *)&CtrlmotorStk[TASK_STK_SZ - 1],     /*-stack allocated to task       */
                TASK_CTRLMOTOR_PRIO);                       /*-priority of task              */

    OSTaskCreate(Cruise,                                    /*-Task function                 */
                (void *)0,                                  /*-nothing passed to task        */
                (void *)&CruiseStk[TASK_STK_SZ - 1],      /*-stack allocated to task       */
                TASK_CRUISE_PRIO);                           /*-priority of task              */

    while(1)
    {
        OSTimeDlyHMSM(0, 0, 5, 0);                          /* Task period ~ 5 secs          */
        robo_LED_toggle();                                  /* Show that we are alive        */
    }

}

int main( void )
{
    robo_Setup();                                          /* initialize HAL for robot       */
    OSInit();                                              /* initialize UCOS-II kernel      */
	robo_wait4goPress();

    robo_motorSpeed(STOP_SPEED, STOP_SPEED);               /* Stop the robot                 */
    myrobot.rspeed   = STOP_SPEED;                         /* Initialize myrobot states      */
    myrobot.lspeed   = STOP_SPEED;
    myrobot.collided = 0;                                  /*  No collisioin                 */
	myrobot.light_side = 0;

    OSTaskCreate(TaskStart,                                /* create TaskStart Task          */
                (void *)0,
                (void *)&TaskStartStk[TASK_STK_SZ - 1],
                TASK_START_PRIO);
    OSStart();                                             /* Start multitasking             */
    while (1);                                             /* die here                       */
}