#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/signal.h>

int motor1_Stop = 63;
int motor1_Back = 64;
int motor1_Forward = 128;

int motor2_Stop = 252;
int motor2_Back = 1;
int motor2_Forward = 2;

int motor1First = 0;
int packetSignature = 108;

int pwmMax;
int pwm = 0;
int PWMFACTOR = 5;

int motor1Speed = 0;
int motor2Speed = 0;
int motor1Direction = 0;
int motor2Direction = 0;

//int packetsRecieved = 0;

int portBData = 0;
unsigned long lastPacketCounterTics;

unsigned char lastPacketData = 0;

void main(void)
{
	// Init USART driver
	USART_Init(200); // 8 MHz, 2400 BAUD (4800=100)

    //set PORT B and C for out
	DDRC = 0xff;
	DDRB = 0xff;
	
	// Set PWM setting
	pwmMax = 3 * PWMFACTOR;
	
	int data;
	
    while(1)
	{
		data = (int)USART_Recieve(); // Wait for serial data

		Styrning(data);
	}
    return 0;
}

// Lights Control
void Styrning(int data)
{
		// Car lights control
		int lightData = data & 3;
		switch(lightData)
		{
			// No lights
			case 0:
				PORTC = PORTC & 252;
				break;
			// Activate brakes
			case 1:
				PORTC = PORTC | 2;
				break;
			// Activate lights
			case 2:
				PORTC = PORTC | 1;
				break;
			// Activate both lights/brakes
			case 3:
				PORTC = PORTC | 3;
				break;
		}
		
		// Motor 1
		if ((data & 128) == 128) // Bit 7=1
			motor1Direction = 1;
		else
			motor1Direction = 0;

		if ((data & 96) == 0) motor1Speed = 0; // Bit 5,6= 0,0
		if ((data & 96) == 32) motor1Speed = 1; // Bit 5,6= 0,1
		if ((data & 96) == 64) motor1Speed = 2; // Bit 5,6= 1,0
		if ((data & 96) == 96) motor1Speed = 3; // Bit 5,6= 1,1


        // Motor 2
		if ((data & 16) == 16) // Bit 4=1
			motor2Direction = 1;
		else
			motor2Direction = 0;

		if ((data & 12) == 0) motor2Speed = 0; // Bit 2,3= 0,0
		if ((data & 12) == 4) motor2Speed = 1; // Bit 2,3= 0,1
		if ((data & 12) == 8) motor2Speed = 2; // Bit 2,3= 1,0
		if ((data & 12) == 12) motor2Speed = 3; // Bit 2,3= 1,1
		
		if (((motor1Speed > 0) || (motor2Speed > 0)) && ((motor1Direction == 0) || (motor2Direction == 0)))
		{
			if (motor1Speed != 3 && motor2Speed != 3) PORTC = PORTC | 128;
		}
		else
		{
			PORTC = PORTC & 127;
		}
		
		if (motor1Speed == 0 || motor2Speed == 0) PORTC = PORTC & 253;
}


// Poll for data on USART
unsigned char USART_Recieve (void)
{
	int dataPacketState = 0;
	
	unsigned char dataPacket;
	unsigned char packet1 = 0;
	unsigned char packet2 = 0;
	

	int loopDelay = 0;
	lastPacketCounterTics = 1000000; //Size?
		
	// Poll for data on USART
	while (1)
	{
		while (1)
		{
			// Shut down motor when no packets has been recieved
			//if ((loopDelay >= 8) && lastPacketCounterTics > 0)
			//{
			//	if (lastPacketCounterTics > 0) lastPacketCounterTics--;
			//	if (lastPacketCounterTics == 1) 
			//	{
			//		PORTB = 0;
			//		portBData = 0;
			//	}
			//	loopDelay = 0;
			//}
			//loopDelay++;
			
			if (motor1First == 1)
			{
				PWM_M2(pwm);
				PWM_M1(pwm);
				motor1First = 0;
			}
			else
			{
				PWM_M1(pwm);
				PWM_M2(pwm);
				motor1First = 1;
			}

			// Increase PWM ramp value. If the max value is reached, start over again
			pwm++; 
			if (pwm > pwmMax) pwm = 0;
			
			if ((UCSRA & ( 1<<RXC))) break;
		}
		
		// Recieve data from buffer
		dataPacket = UDR;
		
		// CRC check
		if (dataPacketState == 1)
		{
			dataPacketState = 0;
		
			int ones = 0;
			unsigned char crc = ~packet1;
			if ((crc & 1) == 1) ones++;
			if ((crc & 2) == 2) ones++;
			if ((crc & 4) == 4) ones++;
			if ((crc & 8) == 8) ones++;
			if ((crc & 16) == 16) ones++;
			if ((crc & 32) == 32) ones++;
			if ((crc & 64) == 64) ones++;
			if ((crc & 128) == 128) ones++;
		
			//if (dataPacket == ones) packetsRecieved++;
			if (dataPacket == ones) return packet1;
		}
		else
		{
			dataPacketState = 1;
			packet1 = dataPacket;
		}
	}
}

void Delay(int time)
{
	return;
	int dummy = 0;
	for (int i = 0; i <= time; i++)
	{
		dummy++;
	}
}

// Motor 1 PWM control
void PWM_M1(int pwmRamp)
{
	if (pwmRamp != 0)
	{
		switch (motor1Speed)
		{
			// No motor speed
			case 0:
				PORTB = PORTB & motor1_Stop;
				Delay(100);
				break;
		
			// Full speed
			case 3:
				if (motor1Direction == 1)
					PORTB = PORTB | motor1_Forward;
				else
					PORTB = PORTB | motor1_Back;
				break;
				
			// Speed 2
			case 2:
				if (pwmRamp == PWMFACTOR)
				{
					if (motor1Direction == 1)
						PORTB = PORTB | motor1_Forward;
					else
						PORTB = PORTB | motor1_Back;
				}
				break;
				
			// Speed 1
			case 1:
			if (pwmRamp == ((2 * PWMFACTOR ) - 1))
			{
				if (motor1Direction == 1)
					PORTB = PORTB | motor1_Forward;
				else
					PORTB = PORTB | motor1_Back;
			}
			break;
		}
	}
	else
		if (motor1Speed != 3) PORTB = PORTB & motor1_Stop;
}

// Motor 2 PWM control
void PWM_M2(int pwmRamp)
{

	if (pwmRamp != 0)
	{
		switch (motor2Speed)
		{
			// No motor speed
			case 0:
				PORTB = PORTB & motor2_Stop;
				Delay(100);
				break;
		
			// Full speed
			case 3:
				if (motor2Direction == 1)
					PORTB = PORTB | motor2_Forward;
				else
					PORTB = PORTB | motor2_Back;
				break;
				
			// Speed 2
			case 2:
				if (pwmRamp == PWMFACTOR)
				{
					if (motor2Direction == 1)
						PORTB = PORTB | motor2_Forward;
					else
						PORTB = PORTB | motor2_Back;
				}
				break;
				
			// Speed 1
			case 1:
			if (pwmRamp == ((2 * PWMFACTOR ) - 1))
			{
				if (motor2Direction == 1)
					PORTB = PORTB | motor2_Forward;
				else
					PORTB = PORTB | motor2_Back;
			}
			break;
		}
	}
	else
		if (motor2Speed != 3) PORTB = PORTB & motor2_Stop;
}

// Init USART com
void USART_Init(unsigned int baud)
{
	UBRRH = (unsigned char)(baud>>8);
	UBRRL = (unsigned char)baud;

	UCSRB = (1<<RXEN);
	UCSRC = (1<<URSEL)|(1<<USBS)|(3<<UCSZ0);
}