#include <msp430.h> 

/*
 * main.c
 */

#define light (sensor!=BIT2)
#define speed 1

short sensor=1,state=0;

void sense()
{
	sensor=P2IN;
	sensor&=BIT2;
}


void delay(int i)
{
	for(;i>0;i--)
	{
		_delay_cycles(10000);
	}
}

void stop()
{
	P1OUT=0;
}

void step_back()
{

	int i;
	for(i=0;i<4;i++)
	{
		P1OUT=BIT1+BIT2;
		delay(15);
		stop();
		delay(10);
	}
}

void forward(short i)
{
	P1OUT=BIT0+BIT3;
	delay(i);
	stop();
	delay(i);
}

void rotate_left(short i)
{
	P1OUT=BIT1+BIT3;
	delay(i);
	stop();
	delay(i);
	state=0;
}


void rotate_right(short i)
{
	P1OUT=BIT0+BIT2;
	delay(i);
	stop();
	delay(i);
	state=1;
}

void initialize()
{
	WDTCTL = WDTPW | WDTHOLD;	// Stop watchdog timer

	P1DIR=0xff;
	P2DIR=0x00;

	P1OUT=0x00;

	P2REN |= BIT3; // P2 Enable Pullup/Pulldown
	 P2OUT |= BIT3; // P2 pullup
	 P2IE |= BIT3; // P2 interrupt enabled
	 P2IES |= BIT3; // P2 Hi/lo falling edge
	 P2IFG &= ~(BIT3); // P2 IFG cleared just in case
	 _EINT();

}

#pragma vector=PORT2_VECTOR
__interrupt void Port_2(void)
{
	sensor=0;
	P2IFG =0;
}

void toggle_state()
{
	if(state) state=0;
	else state=1;
}

void search_source()
{
	short flag=state;

	sense();
	while(!light)
	{
		if(flag)	rotate_left(speed*4);
		else		rotate_right(speed*4);
		sense();
	}


	stop();
	delay(20);

	sense();
	if(!light)   
	{
		while(!light)
		{
			if(flag)	rotate_right(speed);
			else		rotate_left(speed);
			sense();
		}
	}
}

void follow_light()
{
	while(1)
	{
		search_source();

		do
		{
			forward(speed);
			sense();
		}
		while(light);

		step_back();

		if(state) rotate_right(30);
		else rotate_left(30);
	}
}

int main(void)
{
	initialize();

	while(sensor);

	follow_light();

	return 0;
}