/*
 * Assignment #C, CS154 Spring 2001
 * Name         :   SooYoung Jung
 * Date         :   April, 2, 2001
 * File         :   sy.cc
 * 
 */


/*
 * All numbers of motors and sensors
 */
int 	LEFT_MOTOR      = 0;   
int 	RIGHT_MOTOR     = 1;
int 	FAN_MOTOR       = 3;

int 	IR_SENSOR       = 0;
int 	LEFT_LIGHT      = 3;
int 	RIGHT_LIGHT     = 6;

int 	LEFT_BUMP       = 7;
int 	RIGHT_BUMP      = 10;
int 	BACK_BUMP       = 15;

/*
 * Direction of robot
 */
int 	LEFT_FOWARD     = 1;
int 	RIGHT_FOWARD    = 1;

int 	LEFT         	= 0;
int 	RIGHT        	= 1;

int 	STOP        	= 0;
int 	FOWARD      	= 1;
int 	BACKWARD    	= -1;


/*
 * Speed of motors.
 * According to handyboard manual, there are 7 degrees of power so I decided 
 * to increase/decrease motor power by 30.
 */
int 	MOTOR_DEGREE 	= 30;

int 	LEVEL_1 		= 1;
int 	LEVEL_2 		= 2;
int 	LEVEL_3 		= 3;

int 	FOWARD_1    	= 30;
int 	FOWARD_2    	= 60;
int 	FOWARD_3    	= 90;

int 	REVERSE_1   	= -30;
int 	REVERSE_2   	= -60;
int 	REVERSE_3   	= -90;

/*
 * The value of IR sensor when it sees flame
 */
int     NO_FIRE         = 245;
int     FOUND_FIRE      = 200;
int     NEAR_FIRE       = 25;

/*
 * The value of light sensors.
 * I have to use different valuse for light sensors
 * because two light sensors sense differenty with same situation.
 */
int  	L_LIGHT_FACING 	= 15;
int  	R_LIGHT_FACING 	= 30;

/*
 * When the flame is at the center of them, the values would be these.
 */
int  	L_LIGHT_GOOD 	= 18;
int  	R_LIGHT_GOOD 	= 45;

/*
 * This is time(second). But the motor will make 180 degree for 1 second turn.
 */
float   DEGREE_5  		= 0.05;
float   DEGREE_45  		= 0.25;
float   DEGREE_75  		= 0.2;
float   DEGREE_90  		= 0.5;
float   DEGREE_180 		= 1.0;

/*
 * When the fire is extinguished, the value will be set to 1
 */
int     extinguished 	= 0;

/*
 * Process id numbers
 */
int 	endPID;
int 	bumpPID;
int 	firePID;


/*
 * Main does not that much work. but it will produce processes to make the
 * program work.
 */
void main() 
{
    printf("\nYellowy\n");
    printf("Press Start\n");
    start_press();

    motor(LEFT_MOTOR, FOWARD_3);
    motor(RIGHT_MOTOR, FOWARD_3);

    endPID = start_process(endProcess());
    bumpPID = start_process(bumpCheck());
    firePID = start_process(fireCheck());

    while(!extinguished);

    kill_process(firePID);
    kill_process(endPID);

    printf("Yellowy, Good Job!!!\n");
    alloff();

}

/*
 * This process, fireCheck, will detect the fire.
 * If it detects, it will align with flame and extinguish it and return to 
 * main process. 
 */
void fireCheck()
{
    while(!extinguished)
    {
        /* Yes, it found the fire. Do the work */
        if (analog(IR_SENSOR) < FOUND_FIRE)
        {
	        /* 
	         * While aligning, the bump sensors can touch the candle holder or 
	         * wall. Then it will turn.  This isn't necessary anymore.
	         * So kill the bump process, and align to the fire
	         */
            kill_process(bumpPID);

	        /* 
	         * Shout, Fire!!!! and align, and extinguish it. 
	         */
            foundFire();
       		align();
            extinguish();
        }
    }
}


/*
 * Firstly, the robot will make left-right align, then move forward to be 
 * close enough to extinguish fire.  Then the robot will make left-right align 
 * again, because, the motors cannot make the robot move straight forward.  It 
 * keeps going to a little bit right side.
 *
 * To align, the robot will store the value of IR-sensor, and make a little 
 * left turn first, then check IR-sensor again. If the value shows the robot 
 * is closer(lower than before) than before, then the robot will make more 
 * left turn agian until it gets farther than before. 
 * Then the robot will make right turn and do the same thing as left 
 * alignment.
 *
 * And it will go forward a little bit continuously until the robot can blow 
 * out the flame.
 *
 * Then do left-right alignment again because of fault of motors(?)
 */
int align()
{
    int last_IR;						/* previous IR-sensor value */
    int current_IR 	= analog(IR_SENSOR);
    
    /*
     * boolean value for left-right alignment and forwarding to candle.
     */
    int leftRight 	= 0;
    int closeEnough = 0;

	int left 		= 1;				/* or forward */
	int right 		= 1;				/* or backward */
	
	/*
	 * Before we check IR-sensor, need to make turn or move forward so we can 
	 * compare previous value and current value.
	 */
	int firstTime = 1;

	printf("align start\n");			/* Info that I need */

	/*
	 * left right alignment
	 */
	while (!leftRight)
    {  
    	/*
    	 * The robot is going to closer, or it is first time, 
    	 * so make more turn!
    	 */ 
		if ((current_IR < last_IR) || firstTime)  /* goint to closer */
		{
			last_IR = analog(IR_SENSOR);

			/* 
			 * left alignment first, and right alignment
			 */
			if (left || firstTime)
			{
				angleTurn(LEFT, LEVEL_1, DEGREE_5);
				firstTime = 0;
			}
			else  						/* right alignment */
			{
				angleTurn(RIGHT, LEVEL_1, DEGREE_5);
			}

		}
	
		/*
		 * Going to father, so finish left-alignment and do right alignment
		 * If done both, then wer are done with left-right alignment
		 */
		else							
		{
			last_IR = analog(IR_SENSOR);

			/*
			 * Finish left alignment
			 */
			if (left && right)
			{
				left = 0;				/* no more left */
				angleTurn(RIGHT, LEVEL_1, DEGREE_5);
			}
			else if (right)
			{
				right = 0;
				angleTurn(LEFT, LEVEL_1, DEGREE_5);
			}
		}
        ao();

		/*
		 * Gotta sleep, so the value of IR-sensor can differ, 
		 * otherwise, the robot can read the value while turing.
		 */
		sleep(0.5);
   		current_IR = analog(IR_SENSOR);

		/*
		 * check if we are really done.
		 */
		if (!left && !right)
		{
			leftRight = 1;
		}

		/* Info */
        printf ("L:%d,R:%d,I:%d\n", analog(LEFT_LIGHT), analog(RIGHT_LIGHT),
        							analog(IR_SENSOR));
	}

	firstTime = 1;

	/*
	 * Move forward to make the robot close enough to blow out the candle
	 * The same strategy as left-right alignment but moving forward.
	 */
	while (!closeEnough)
    {   
    	/*
    	 * The robot is going to closer, or it is first time, 
    	 * so make more forward movement!
    	 */ 
		if ((current_IR < last_IR) || firstTime)
		{
			last_IR = analog(IR_SENSOR);

			move(FOWARD, LEVEL_1, DEGREE_5);
			firstTime = 0;
		}
        ao();
		sleep(0.5);
   		current_IR = analog(IR_SENSOR);

		/*
		 * If the robot is close enough, then finish the forward-alignment.
		 */
		if (current_IR < NEAR_FIRE)
		{
			closeEnough = 1;
		}
		
		/*
		 * If the robot sense the bump(s), then do not go more.
		 * Let's assume it is candle holder. 
		 */
		if (digital(LEFT_BUMP) || digital(LEFT_BUMP))
		{
			move(BACKWARD, LEVEL_1, DEGREE_5);
		}
        
        printf ("L:%d,R:%d,I:%d\n", analog(LEFT_LIGHT), analog(RIGHT_LIGHT), 
        							analog(IR_SENSOR));
	}

	left 		= 1;
	right 		= 1;
	firstTime 	= 1;
	leftRight 	= 0;

	/*
	 * left-right alignment again
	 */
	while (!leftRight)
    {  
    	/*
    	 * The robot is going to closer, or it is first time, 
    	 * so make more turn!
    	 */ 
		if ((current_IR < last_IR) || firstTime)  /* goint to closer */
		{
			last_IR = analog(IR_SENSOR);

			/* 
			 * left alignment first, and right alignment
			 */
			if (left || firstTime)
			{
				angleTurn(LEFT, LEVEL_1, DEGREE_5);
				firstTime = 0;
			}
			else  						/* right alignment */
			{
				angleTurn(RIGHT, LEVEL_1, DEGREE_5);
			}

		}
	
		/*
		 * Going to father, so finish left-alignment and do right alignment
		 * If done both, then wer are done with left-right alignment
		 */
		else							
		{
			last_IR = analog(IR_SENSOR);

			/*
			 * Finish left alignment
			 */
			if (left && right)
			{
				left = 0;				/* no more left */
				angleTurn(RIGHT, LEVEL_1, DEGREE_5);
			}
			else if (right)
			{
				right = 0;
				angleTurn(LEFT, LEVEL_1, DEGREE_5);
			}
		}
        ao();

		/*
		 * Gotta sleep, so the value of IR-sensor can differ, 
		 * otherwise, the robot can read the value while turing.
		 */
		sleep(0.5);
   		current_IR = analog(IR_SENSOR);

		/*
		 * check if we are really done.
		 */
		if (!left && !right)
		{
			leftRight = 1;
		}

		/* Info */
        printf ("L:%d,R:%d,I:%d\n", analog(LEFT_LIGHT), analog(RIGHT_LIGHT),
        							analog(IR_SENSOR));
	}

	/*
	 * tell the caller we are successfully done 
	 */
	return closeEnough;					
}

/*
 * This etinguish function will extingush the fire.
 * Check every 5 second they are really gone.
 */
void extinguish()
{
    int done = 0;
    int last_IR;
    
    while(!done)
    {
        last_IR = analog(IR_SENSOR);
        motor(FAN_MOTOR,  FOWARD_3);
        sleep(5.0);
        
        /*
         * both of current and previouse value should not detect fire
         */
        if (analog(IR_SENSOR) > NO_FIRE && last_IR > NO_FIRE)
        {
            ao();
            done 		 = 1;
            extinguished = 1;
        }
    }
}

/*
 * Just say to everybody.
 */
void foundFire()
{
    ao();
    printf("FIRE!!!\n");
    tone(500.0, 1.0);
}   


/*
 * This is another process(thread) that main will check bumper status all the 
 * time.  If there is obstacle on the left, then it will make a right angle 
 * turn to right or vice versus. If the back sensor is detecting something, 
 * then it should happen when they the robot is making left or right turn. so 
 * just stop and make turn.
 *
 * I just realize that this back bumper part whouldn't really work so it 
 * should be separate as another process so it can probably detect when they 
 * are making turn.
 */
void bumpCheck()
{
    while(1)
    {
        if (digital(RIGHT_BUMP) && digital(LEFT_BUMP) || digital(RIGHT_BUMP))
        {
            printf("LEFT && RIGHT Bumps\n");
            move(BACKWARD, LEVEL_3, DEGREE_75);
            angleTurn(LEFT, LEVEL_3, DEGREE_90);
            move(FOWARD, LEVEL_3, DEGREE_75);
        }
        else if (digital(LEFT_BUMP))
        {
            printf("LEFT Bump\n");
            move(BACKWARD, LEVEL_3, DEGREE_75);
            angleTurn(RIGHT, LEVEL_3, DEGREE_90);
            move(FOWARD, LEVEL_3, DEGREE_75);
        }
        else if (digital(BACK_BUMP))
        {
            printf("BACK Bump %d\n", LEVEL_1);
            ao();
            
	        if (digital(RIGHT_BUMP) && digital(LEFT_BUMP) || 
	        	digital(RIGHT_BUMP))
	        {
	            printf("LEFT && RIGHT Bumps\n");
	            angleTurn(LEFT, LEVEL_3, DEGREE_90);
	            move(FOWARD, LEVEL_3, DEGREE_75);
	        }
	        else
	        {
	            printf("LEFT Bump\n");
	            angleTurn(RIGHT, LEVEL_3, DEGREE_90);
	            move(FOWARD, LEVEL_3, DEGREE_75);
	        }
        }
    }
}

/*
 * This angleTurn will take 3 arguments and make the robot turn.
 * Three arguments are 
 *	1. which way the robot should go(left or right)
 *	2. how fast it should go
 *  3. turing angle.
 */
void angleTurn(int whichWay, int powerLevel, float angle)
{
    int motorPower = MOTOR_DEGREE * powerLevel;
    
    if (whichWay == LEFT)
    {
    	/*
    	 * Move both motors so it does faster turn compare to one motor moving
    	 * turn.
    	 */
        motor(RIGHT_MOTOR, motorPower);
        motor(LEFT_MOTOR, motorPower * BACKWARD);
    }
    else if (whichWay == RIGHT)
    {
        motor(LEFT_MOTOR, motorPower);
        motor(RIGHT_MOTOR, motorPower * BACKWARD);
    }
    
    sleep(angle);
}
    
/*
 * This move will take 3 arguments and make the robot move.
 * Three arguments are 
 *	1. which way the robot should go(forward or backward)
 *	2. how fast it should go
 *  3. how long the robot should move.
 * The function will make the robot move at leat third input argument of 
 * seconds.
 * Stoping the robot after movement is caller's responsibility.
 */
void move(int whichWay, int powerLevel, float howlong)
{
    int motorPower = MOTOR_DEGREE * powerLevel * whichWay;
    
    motor(LEFT_MOTOR,  motorPower);
    motor(RIGHT_MOTOR, motorPower);
    sleep(howlong);
}

/*
 * The other process(thread).
 * Whenever stop button is pressed, the robot will stop all motors.
 */
void endProcess() 
{
    while(1) 
    {
        stop_press();
        alloff();
        printf("Stop pressed\n");
    }
}