#include <stdio.h>
#include <stdlib.h>
#include <wiringPi.h>

#define NUMBYTES	1
#define BYTEINDEX	0
#define RSERVO		0x00
#define LSERVO		0x80
#define LREVERSE	0x00
#define LFORWARD	0x40
#define RREVERSE	LFORWARD
#define RFORWARD	LREVERSE
#define SPI_CH_0	0
#define SPI_CH_1	1

#define DUMMYBYTE1	0xFF
#define DUMMYBYTE2	0x7F

#define SPEED		10
#define OFF			0

unsigned char cmdBuff[NUMBYTES];

// This functions expects a value between -50 and 50 per side
void setMotors(signed char right, signed char left)
{
	unsigned char rdir, ldir, rspeed, lspeed;
	
	if (right < 0)
	{
		if (right < -50)
			right = -50;
		rdir = RREVERSE;
		rspeed = right * -1;
	}
	else // if (right >= 0)
	{
		if (right > 50)
			right = 50;
		rdir = RFORWARD;
		rspeed = right;
	}
	
	if (left < 0)
	{
		if (left < -50)
			left = -50;
		ldir = LREVERSE;
		lspeed = left * -1;
	}
	else // if (left >= 0)
	{
		if (left > 50)
			left = 50;
		ldir = LFORWARD;
		lspeed = left;
	}
	
	cmdBuff[BYTEINDEX] = RSERVO | rdir | rspeed;
	wiringPiSPIDataRW(SPI_CH_0, cmdBuff, NUMBYTES);
	
	cmdBuff[BYTEINDEX] = LSERVO | ldir | lspeed;
	wiringPiSPIDataRW(SPI_CH_0, cmdBuff, NUMBYTES);
}

void forward()
{
	setMotors(SPEED, SPEED);
}

void reverse()
{
	setMotors(-SPEED, -SPEED);
}

void stop()
{
	setMotors(OFF, OFF);
}

void turnLeftHard()
{
	setMotors(SPEED, -SPEED);
}

void turnRightHard()
{
	setMotors(-SPEED, SPEED);
}

void turnLeftSoft()
{
	setMotors(SPEED, OFF);
}

void turnRightSoft()
{
	setMotors(OFF, SPEED);
}

void msleep(int ms)
{
	usleep(ms * 1000); // convert to microseconds
}

unsigned char getSensorData()
{
	cmdBuff[BYTEINDEX] = DUMMYBYTE1;
	wiringPiSPIDataRW(SPI_CH_0, cmdBuff, NUMBYTES);
	msleep(1);
	cmdBuff[BYTEINDEX] = DUMMYBYTE2;
	wiringPiSPIDataRW(SPI_CH_0, cmdBuff, NUMBYTES);
	return (cmdBuff[BYTEINDEX] & 0x07); // whatever came back over SPI
}

int main(void)
{
	int i;
	unsigned char currentreflection, previousreflection;
	
	if (wiringPiSetup() == -1)
	{
		printf("wiringPi setup failed somehow!\n");
		exit(1);
	}

	if (wiringPiSPISetup(0, 1000000) < 0)
	{
		printf("SPI setup failed somehow!\n");
		exit(1);
	}

	stop();
	currentreflection = 0;
	previousreflection = 99;
	
	// Main program loop
	while (1)
	{
		// Resolution of change:
		msleep(20);
		currentreflection = getSensorData();
        // If something's changed:
		if (currentreflection != previousreflection)
        {
			switch (currentreflection)
			{
				// WHITE = 1, BLACK = 0!
				//         ...432
				//         ...RCL
				case 0: // ...000 - all black
					break;
				case 1: // ...001
					turnRightSoft();
					break;
				case 2: // ...010 - should not occur
					forward();
					break;
				case 3: // ...011
					turnRightHard();
					break;
				case 4: // ...100
					turnLeftSoft();
					break;
				case 5: // ...101 - ideal condition
					forward();
					break;
				case 6: // ...110
					turnLeftHard();
					break;
				case 7: // ...111 - all see white, line is lost
					break;
				default:
					forward();
			}
			previousreflection = currentreflection;
        }
	}
	return 0;
}