#include <stdint.h>
#include <stdbool.h>
sfr at 0xff portx;
bool lastTurn = false;	//used to find black line again. false = right of line, true = left of line

void put1(){
	uint8_t value = 112;
	portx = value;
	value = 32;
	portx = value;
}

void put0(){
	uint8_t value = 48;
	portx = value;
	value = 32;
	portx = value;
}

void applyToMotor(){
	uint8_t value = 160;
	portx = value;
}

void stop(){
	put0();
	put0();
	put0();
	put0();
	applyToMotor();
}

void left(){
        put0();
        put0();
        put1();
        put0();
        applyToMotor();
        lastTurn = false;
}


void sharpLeft(){
	put0();
	put1();
	put1();
	put0();
	applyToMotor();
	lastTurn = false;
}

void right(){
	put1();
	put0();
	put0();
	put0();
	applyToMotor();
	lastTurn = true;
}

void sharpRight(){
        put1();
        put0();
        put0();
        put1();
        applyToMotor();
        lastTurn = true;
}


void forward(){
	put1();
	put0();
	put1();
	put0();
	applyToMotor();
}

void backward(){
	put0();
	put1();
	put0();
	put1();
	applyToMotor();
}

//returns true, if the right sensor is above white ground. False if not.
bool RSensor(){
	uint8_t value = 5;
	portx = value;
	value = 4;
	portx = value;
	value = portx;
	return value > 0;
}


//returns true, if the left sensor is above white ground. False if not.
bool LSensor(){
	uint8_t value = 5;
	portx = value;
	value = 4;
	portx = value;
	value = 5;
	portx = value;
	value = 7;
	portx = value;
        value = portx;
        return value > 0;
}

void search()
{
	bool timeout = false;
	uint16_t i = 0;
	uint16_t retries = 1;
	while (RSensor() && LSensor() && !timeout)
	{
		i++;
		if (i > 2000 * (0.2 * retries))
		{
			timeout = true;
			i = 0;
			retries++;
		}
		if (lastTurn)
		{
			right();
		}
		else
		{
			left();
		}
	}
	while (RSensor() && LSensor() && timeout){
		i++;
		if (i > 6000 * (0.3 * retries))
		{
			timeout = false;
			i = 0;
			lastTurn = !lastTurn;
			retries++;
		}
		forward();
	}
}

void main()
{
	while(1)
	{
		//robot over dark ground. just follow the line
		if (!RSensor() && !LSensor()){
			forward();
		}
		//robot left of line. Turn right
		if (LSensor() && !RSensor())
		{
			right();
		}
		//robot right of line. turn left.
		if (RSensor() && !LSensor())
		{
			left();
		}
		//robot lost line. Searching for it @ the last known position.
		if (RSensor() && LSensor())
		{
			search();
		}
	}
}


unsigned char sdcc_external_startup() {
	return 0;
}