/*
 * function.cpp
 *
 * Created: 2.10.2013 17:06:29
 *  Author: Tomáš Feik
 */ 
#ifndef F_CPU
	#define F_CPU 16000000UL  // 16 MHz
#endif

#ifndef TF_FC
	#define HMC5883L_INTEGRATOR_VALUE (0xFF)
	
	#define Kp (0.7)
	#define Ki (0.3)
	#define Kd (0.1)
	
	#define sm_speed_const (0.25)
	
	#define Zero_Speed (0x8000)
	#define Normal_Speed (0xAAAA)
	
	#define TF_FC
#endif

#include <avr/io.h>
#include <util/delay.h>
#include "twi_lib.h"

char nibbleToHex(char nibble)
{
	nibble &= 0x0F;
	if(nibble < 10)
	{
		return nibble + 0x30;
	}
	
	return nibble + 0x37;
}

void byteToHex(char val, char* hn, char* ln)
{
	(*ln) = nibbleToHex(val);
	(*hn) = nibbleToHex(val >> 4);
}

int findMin(int value1, int value2, int value3)
{
	int array[3];
	int i, minValue;
	
	minValue = 0;
	array[0] = value1;
	array[1] = value2;
	array[2] = value3;
	
	for (i = 0; i < 3; i++)
	{	
		if (array[i] < minValue )
		{
			minValue = array[i];
		}
	}
	return minValue;
}

void sendStartSequenceToMotors()
{
	uart_putc('#');
}

void sendEndSequenceToMotors()
{
	uart_putc('\r');
	uart_putc('\n');
}

void sendSpeedToMotors(int left, int right)
{
	char buff[5];
	int bcc, i;

	byteToHex(left, &buff[0], &buff[1]);
	byteToHex(right, &buff[2], &buff[3]);
	
	sendStartSequenceToMotors();
	uart_putc('M');
	bcc = 'M';
	
	for (i = 0; i < 4; i++)
	{
		uart_putc(buff[i]);
		bcc ^= buff[i];
	}
	uart_putc(bcc);
	sendEndSequenceToMotors();
}

void sendBarrierDistanceToMotors(int barrierDistance)
{
	char buff[3];
	int bcc, i;
	
	byteToHex(barrierDistance, &buff[0], &buff[1]);
	
	sendStartSequenceToMotors();
	uart_putc('M');
	bcc = 'M';
	uart_putc('*');
	bcc ^= '*';
	uart_putc('B');
	bcc ^= 'B';
	
	for (i = 0; i < 2; i++)
	{
		uart_putc(buff[i]);
		bcc ^= buff[i];
	}
	uart_putc(bcc);
	sendEndSequenceToMotors();
}

void sendStartToMotors()
{
	int bcc;
	
	sendStartSequenceToMotors();
	uart_putc('M');
	bcc = 'M';
	uart_putc('*');
	bcc ^= '*';
	uart_putc('G');
	bcc ^= 'G';
	uart_putc(bcc);
	sendEndSequenceToMotors();
}

void sendEndToMotors()
{
	int bcc;
	
	sendStartSequenceToMotors();
	uart_putc('M');
	bcc = 'M';
	uart_putc('*');
	bcc ^= '*';
	uart_putc('E');
	bcc ^= 'E';
	uart_putc(bcc);
	sendEndSequenceToMotors();
}

float modifySpeedValue(int barrierDistance)
{
	if (barrierDistance >= 50)
		return 1.0;
	else if (barrierDistance <= 10)
		return 0.0;
	else
		return (barrierDistance - 10) / 40;
}

void SRF_init(unsigned int address)
{
	while(i2c_start(address+I2C_WRITE) == 1)
	{
		i2c_stop();
		_delay_us(5);
	}
	i2c_write(0x02);
	i2c_write(0x0B);
	i2c_stop();
}
	
void SRF_start(unsigned int address)
{
	while(i2c_start(address+I2C_WRITE) == 1)
	{
		i2c_stop();
		_delay_us(5);
	}
	i2c_write(0x00);
	i2c_write(0x51);
	i2c_stop();
}	

int SRF_read(unsigned int address)
{
	int distance_H, distance_L, end_of_read;
	distance_H = distance_L = end_of_read = 0;
		
	// KONTROLA DOKONČENÍ MĚŘENÍ
	do{
		while(i2c_start(address+I2C_WRITE) == 1)
		{
			i2c_stop();
			_delay_us(5);
		}
		i2c_write(0x00);
		
		while(i2c_rep_start(address+I2C_READ) == 1)
		{
			i2c_stop();
			_delay_us(5);
		}
		end_of_read = i2c_readNak();
		i2c_stop();
	}
	while(end_of_read == 0xFF);
	
	// ČTENÍ MSB HODNOTY PRVNÍHO ECHA
	while(i2c_start(address+I2C_WRITE) == 1)
	{
		i2c_stop();
		_delay_us(5);
	}
	i2c_write(0x02);

	while(i2c_rep_start(address+I2C_READ) == 1)
	{
		i2c_stop();
		_delay_us(5);
	}
	distance_H = i2c_readNak();
	i2c_stop();
	
	// ČTENÍ LSB HODNOTY PRVNÍHO ECHA
	while(i2c_start(address+I2C_WRITE) == 1)
	{
		i2c_stop();
		_delay_us(5);
	}
	i2c_write(0x03);

	while(i2c_rep_start(address+I2C_READ) == 1)
	{
		i2c_stop();
		_delay_us(5);
	}
	distance_L = i2c_readNak();
	i2c_stop();
	
	// VYHODNOCENÍ MĚŘENÍ
	if (distance_L == 0 && distance_H == 0)
		return 55; // pokud nenameri prekazku pak vraci 55 cm
		
	return (distance_L + 256*distance_H);
}

int HMC5883L_init(unsigned int address)
{
	// CRA
	i2c_start_wait(address+I2C_WRITE);
	i2c_write(0x00);
	i2c_write(0x78); // 8-average, 75 Hz, normal measurement 0x70
	i2c_stop();

	// CRB
	i2c_start_wait(address+I2C_WRITE);
	i2c_write(0x01);
	i2c_write(0xE0); // ± 8.1 Ga
	i2c_stop();
	
	// mod mereni
	i2c_start_wait(address+I2C_WRITE);
	i2c_write(0x02);
	i2c_write(0x00); // 0x00 - Continuous-Measurement Mode; 0x01 Single-Measurement Mode
	i2c_stop();

	return 0;
}

int HMC5883L_Read(unsigned int address, unsigned int address_MSB)
{
	int data_MSB, data_LSB;	
	data_MSB = 0;
	data_LSB = 0;

	// ČTENÍ HODNOTY MSB
	while(i2c_start(address + I2C_WRITE) == 1)
	{
		i2c_stop();
		_delay_us(10);
	}
	i2c_write(address_MSB);
	
	while(i2c_rep_start(address + I2C_READ) == 1)
	{
		i2c_stop();
		_delay_us(10);
	}
	data_MSB = i2c_readNak();
	i2c_stop();
	
	// ČTENÍ HODNOTY LSB
	i2c_start(address + I2C_WRITE);
	while(i2c_start(address + I2C_WRITE) == 1)
	{
		i2c_stop();
		_delay_us(10);
	}
	i2c_write(address_MSB - 1);
	
	while(i2c_rep_start(address + I2C_READ) == 1)
	{
		i2c_stop();
		_delay_us(10);
	}
	data_LSB = i2c_readNak();
	i2c_stop();
	
	return (data_LSB + 256 * data_MSB);
}	

void HMC5583L_UpdateValue(volatile int* updatedValue, int reallyValue)
{		
	if (reallyValue > updatedValue)
		updatedValue += HMC5883L_INTEGRATOR_VALUE;
	else if(reallyValue < updatedValue)
		updatedValue -= HMC5883L_INTEGRATOR_VALUE;
}

void Move_Stop()
{
	char buff[5];
	int bcc, i;

	byteToHex(Zero_Speed, &buff[0], &buff[1]);
	byteToHex(Zero_Speed, &buff[2], &buff[3]);
	
	sendStartSequenceToMotors();
	uart_putc('M');
	bcc = 'M';
	
	for (i = 0; i < 4; i++)
	{
		uart_putc(buff[i]);
		bcc ^= buff[i];
	}
	uart_putc(bcc);
	sendEndSequenceToMotors();
}

void sendHMC5883LValues(unsigned int hmc5883l_address, unsigned int x_msb_adress, unsigned int y_msb_adress, unsigned int z_msb_adress)
{
		int val;
		char h, l;
	
		val = HMC5883L_Read(hmc5883l_address, x_msb_adress);
		uart_putc('X');
		uart_putc(':');
		byteToHex(val >> 8 & 0xFF, &h, &l);
		uart_putc(h);
		uart_putc(l);		
		uart_putc(h);
		uart_putc(l);
		uart_putc(' ');
		
		val = HMC5883L_Read(hmc5883l_address, y_msb_adress);		
		uart_putc('Y');
		uart_putc(':');
		byteToHex(val >> 8 & 0xFF, &h, &l);
		uart_putc(h);
		uart_putc(l);		
		byteToHex(val & 0xFF, &h, &l);
		uart_putc(h);
		uart_putc(l);
		uart_putc(' ');
		
		val = HMC5883L_Read(hmc5883l_address, z_msb_adress);
		uart_putc('Z');
		uart_putc(':');
		byteToHex(val >> 8 & 0xFF, &h, &l);
		uart_putc(h);
		uart_putc(l);	
		byteToHex(val & 0xFF, &h, &l);
		uart_putc(h);
		uart_putc(l);		
		uart_putc('\r');
		uart_putc('\n');
}

void evaluateMeasurementSM(int x, int z, float modifySpeed)
{
	if (z > 0x1000) // robot neni na pasce
	{
		Move_Stop();
	}
	else
	{
		if (x > 0xA000)
			sendSpeedToMotors(round(modifySpeed * (Normal_Speed + Normal_Speed * sm_speed_const)), round(modifySpeed * (Normal_Speed - Normal_Speed * sm_speed_const)));
		else
			sendSpeedToMotors(round(modifySpeed * (Normal_Speed - Normal_Speed * sm_speed_const)), round(modifySpeed * (Normal_Speed + Normal_Speed * sm_speed_const)));
	}
}

void evaluateMeasurementPSD(int x, int z, int xPrev, int zPrev, float modifySpeed, int sumI)
{
	int xDifference;
	float speedChange;
	
	if (z > 0x1000) // robot neni na pasce
	{
		Move_Stop();
	}
	else
	{
		xDifference = x - xPrev;
		speedChange += sumI;

		speedChange = Kp * x + Ki * sumI + Kd * xDifference;

		if (x > 0xA000)
			sendSpeedToMotors(round(modifySpeed * (Normal_Speed + speedChange/2)), round(modifySpeed * (Normal_Speed - speedChange/2)));
		else
			sendSpeedToMotors(round(modifySpeed * (Normal_Speed - speedChange/2)), round(modifySpeed * (Normal_Speed + speedChange/2)));
	}
}