// *********************************************************************************** //
//
//	Filename		:   Logic.c
//	Title        	:   TowerBuilder Logic File
//	Author			:   Fung Tin Ching
//	Created Date	:   2007.11.27
//	Revisited Date	:   2008.01.16
//	Description		:   The Logic Behaviour Should be place in here
//                      ** Note: If your want to use variable among the source file, add Declaration to the RoboTank.h file
//                               e.g. "u08 ABC;" in this file, and put "extern u08 ABC;" in the Global.h
//	Version         :   1.2
//
//  Copyright (c) 2008, All rights reserved.
// *********************************************************************************** //

#ifndef _LOGIC_H
#define _LOGIC_H

#include "Global.h"

// ================================================================================================================= //
// =========================                         Constant Define                       ========================= //
// ================================================================================================================= //

#define DEFAULT_MOTORSPEED_FORWARD_ML   20
#define DEFAULT_MOTORSPEED_FORWARD_MR   20

#define LOGIC_LINE_LINE_AT_CENTER_ML           DEFAULT_MOTORSPEED_FORWARD_ML
#define LOGIC_LINE_LINE_AT_CENTER_MR           DEFAULT_MOTORSPEED_FORWARD_MR

#define LOGIC_LINE_LINE_AT_LEFT_ML             LOGIC_LINE_LINE_AT_CENTER_ML-3 
#define LOGIC_LINE_LINE_AT_LEFT_MR             LOGIC_LINE_LINE_AT_CENTER_MR
#define LOGIC_LINE_LINE_AT_RIGHT_ML            LOGIC_LINE_LINE_AT_CENTER_ML
#define LOGIC_LINE_LINE_AT_RIGHT_MR            LOGIC_LINE_LINE_AT_CENTER_MR-3

#define LOGIC_LINE_LINE_AT_CLOSE_LEFT_ML       LOGIC_LINE_LINE_AT_CENTER_ML-2
#define LOGIC_LINE_LINE_AT_CLOSE_LEFT_MR       LOGIC_LINE_LINE_AT_CENTER_MR
#define LOGIC_LINE_LINE_AT_CLOSE_RIGHT_ML      LOGIC_LINE_LINE_AT_CENTER_ML
#define LOGIC_LINE_LINE_AT_CLOSE_RIGHT_MR      LOGIC_LINE_LINE_AT_CENTER_MR-2

#define LOGIC_LINE_LINE_AT_VCLOSE_LEFT_ML       LOGIC_LINE_LINE_AT_CENTER_ML-1
#define LOGIC_LINE_LINE_AT_VCLOSE_LEFT_MR       LOGIC_LINE_LINE_AT_CENTER_MR
#define LOGIC_LINE_LINE_AT_VCLOSE_RIGHT_ML      LOGIC_LINE_LINE_AT_CENTER_ML 
#define LOGIC_LINE_LINE_AT_VCLOSE_RIGHT_MR      LOGIC_LINE_LINE_AT_CENTER_MR-1


// ================================================================================================================= //
// =========================                         Function Prototype                    ========================= //
// ================================================================================================================= //


void logic_stop(void);
void line_tracking_forward(void);

// ================================================================================================================= //
// =========================                         Function Body                         ========================= //
// ================================================================================================================= //
	 
void mainLogic(void)
{
    line_tracking_forward();
}


void line_tracking_forward(void)
{

	idata unsigned char i=0;  // temporal counter
	idata unsigned char tempL,tempR,sensortemp;

	// Make the motor moving
	set_left_motor(MOTOR_MODE_FORWARD, DEFAULT_MOTORSPEED_FORWARD_ML);
	set_right_motor(MOTOR_MODE_FORWARD, DEFAULT_MOTORSPEED_FORWARD_MR);
	delay_ms(5);

	while (1)
	{

		sensortemp=sensor_front_ledon&0x7E; // get the front sensor data and masking
		switch (sensortemp)  
		{
		// Bit:
	    //  6 5 4  3 2 1 
		//  0 0 0  0 0 0 
	   			case 0x00:   
					// cannot find a line
					if (++i>150)
					{
						logic_stop();
						return;
					}
					break;
		//  0 1 1  1 1 0 
	   			case 0x2C:
		//  0 0 1  1 0 0 
	   			case 0x18:
		//  0 0 0  1 0 0 
	   			case 0x08:
		//  0 0 1  0 0 0 
	   			case 0x10:
					tempL=LOGIC_LINE_LINE_AT_CENTER_ML;
					tempR=LOGIC_LINE_LINE_AT_CENTER_MR;
					break;

		//  0 1 1  1 0 0 
	   			case 0x38:
					tempL=LOGIC_LINE_LINE_AT_VCLOSE_RIGHT_ML;
					tempR=LOGIC_LINE_LINE_AT_VCLOSE_RIGHT_MR;
					break;

		//  0 0 1  1 1 0 
	   			case 0x1C:
					tempL=LOGIC_LINE_LINE_AT_VCLOSE_LEFT_ML;
					tempR=LOGIC_LINE_LINE_AT_VCLOSE_LEFT_MR;
					break;




		//  0 0 0  1 1 0 
	   			case 0x0C:
					tempL=LOGIC_LINE_LINE_AT_CLOSE_RIGHT_ML;
					tempR=LOGIC_LINE_LINE_AT_CLOSE_RIGHT_MR;
					break;		 

		//  0 1 1  0 0 0 
	   			case 0x30:
					tempL=LOGIC_LINE_LINE_AT_CLOSE_LEFT_ML;
					tempR=LOGIC_LINE_LINE_AT_CLOSE_LEFT_MR;
					break; 

		//  0 0 0  1 1 1 
	   			case 0x0E:
		//  0 0 0  0 1 1 
	   			case 0x06:
		//  0 0 0  0 0 1 
	   			case 0x02: 
 					
					tempL= LOGIC_LINE_LINE_AT_RIGHT_ML;
					tempR= LOGIC_LINE_LINE_AT_RIGHT_MR;
					break;  

		//  1 1 1  0 0 0 
	   			case 0x70:
		//  1 1 0  0 0 0 
	   			case 0x60:
		//  1 0 0  0 0 0 
				case 0x40: 
					tempL=LOGIC_LINE_LINE_AT_LEFT_ML;
					tempR=LOGIC_LINE_LINE_AT_LEFT_MR;
					break; 																	

		//  1 1 1  1 1 1 
	   			case 0x7E:   
					// Hit Horizontal Line
//					logic_stop();
//					return;
					break;
		}
		set_left_motor(MOTOR_MODE_FORWARD,  tempL);
		set_right_motor(MOTOR_MODE_FORWARD, tempR);
  
		
		delay_100us(); // make a little delay  
	}
	

}	
  

void logic_stop(void)
{
	set_left_motor(MOTOR_MODE_BREAK, 0);
	set_right_motor(MOTOR_MODE_BREAK, 0);
}

#endif