#include "stm32f4xx.h"
#include "stm32f4xx_gpio.h"
#include "stm32f4xx_usart.h"
#include "stm32f4xx_rcc.h"
#include "stm32f4xx_exti.h"
#include "stm32f4xx_syscfg.h"
#include "misc.h"
#include "scanner_lib.h"
#include "math.h"

#define BAUDRATE 115200
#define PI 3.14159265
#define MRWIDTH 100
#define MRFRONT 50
#define SHIELD 100



EXTI_InitTypeDef EXTI_InitStructure;
NVIC_InitTypeDef NVIC_InitStructure;

/*Funkcni prototypy*/
void ledInit(void);
void interuptInit(void);
uint8_t scannerSetup(void);
uint8_t *scanData(void);
unsigned short WallFollow(unsigned short int []);




uint8_t task[askmax];
uint8_t answer[ansmax];
uint8_t i = 1;
extern uint8_t k;
extern uint8_t *c, *p;
uint8_t *dataRaw;
unsigned short p_distances[541];
extern char *endPtr;
uint16_t st;



int main(void)
{
	unsigned short l, due;

	ledInit();
	usartInit(BAUDRATE);
	interuptInit();

	while(1)
    {

		if( i == 1)
		{
			dataRaw = scanData();
			st = dataprocessing(answer);
			l = WallFollow(p_distances);
			Delay(0x000FFFFF);
		//	due = WallFolowing(l);

		}
    }
}



unsigned short WallFollow(unsigned short int MeasuredValues[])
{
	GPIO_ResetBits(GPIOD,GPIO_Pin_13 | GPIO_Pin_12 | GPIO_Pin_14);
	unsigned short step,gamac;
	float p,l,x1,x2,a,b,v1,v2,z1,z2,c,y1,y2, y1limit,y2limit,r,s,turnlimit;
	float beta,gama, gama1,gama2,betadeg;
	step = 0;
	gama = (0.5*PI)/ 180;

	//shield setup

	c = MRWIDTH;
	z1 = SHIELD;
	z2 = MRFRONT;
	gama = atan((0.5*c)/(z1+z2));
	gamac = (unsigned short)(((180*gama)/PI)*2);
	y1limit = (0.5*c)/sin(gama);
	y2limit = y1limit;
	y1 = MeasuredValues[270-gamac];
	y2 = MeasuredValues[270+gamac];
	turnlimit = (z1+z2)/cos(gama);



	//right gama correction
	while(1)
	{
		p = (float)MeasuredValues[90];
		x1 = (float)MeasuredValues[90+step];
		gama = (0.5*PI*step)/ 180;
		v1 = x1*sin(gama);
		if(v1 < z2)
		{
			step++;
		}
		else
		{
			gama1 = gama;
			break;
		}
	}

	//left gama correction
	step = 0;
	while(1)
	{
		l = (float)MeasuredValues[450];
		x2 = (float)MeasuredValues[450-step];
		gama = (0.5*PI*step)/ 180;
		v2 = x2*sin(gama);
		if(v2 < z2)
		{
			step++;
		}
		else
		{
			gama2 = gama;
			break;
		}
	}

	if((y1<y1limit) || (y2<y2limit))
	{//shield breaker
		if(y1 < y2)
		{
			gama = ((270 - gamac - 90)*0.5*PI)/180;
			s = sqrt(pow(y1,2) + pow(p,2) - ((2*y1*p)*cos(gama)));
			beta = acos((pow(y1,2) - pow(s,2) - pow(p,2))/(-2*s*p));
			betadeg = ((180*beta)/PI);

			while(!(betadeg > 90 || y1 > turnlimit))
			{
				dataRaw = scanData();
				st = dataprocessing(answer);
				y1 = p_distances[270-gamac];
				p = (float)p_distances[90];
				GPIO_SetBits(GPIOD,GPIO_Pin_12);
				gama = ((270 - gamac - 90)*0.5*PI)/180;
				s = sqrt(pow(y1,2) + pow(p,2) - ((2*y1*p)*cos(gama)));
				beta = acos((pow(y1,2) - pow(s,2) - pow(p,2))/(-2*s*p));
				betadeg = ((180*beta)/PI);
			}

			if((betadeg > 90) && (y1 > turnlimit))
			{
				GPIO_ResetBits(GPIOD,GPIO_Pin_12);
				while(1)
				{
					dataRaw = scanData();
					st = dataprocessing(answer);
					y1 = p_distances[270-gamac];
					y2 = p_distances[270+gamac];

					if((y2 > y2limit) && (y1 > y1limit))
					{
						GPIO_SetBits(GPIOD,GPIO_Pin_13);
						Delay(0x000FFFFF);
					}
					else
						break;
				}
				return 1;
			}
		}

		if(y2 < y1)
		{
			gama = ((450 - (270 + gamac))*0.5*PI)/180;
			r = sqrt(pow(y2,2) + pow(l,2) - ((2*y2*l)*cos(gama)));
			beta = acos((pow(y2,2) - pow(r,2) - pow(l,2))/(-2*r*l));
			betadeg = ((180*beta)/PI);

			while(!(betadeg > 90 || y2 > turnlimit))
			{
				dataRaw = scanData();
				st = dataprocessing(answer);
				y2 = p_distances[270+gamac];
				l = (float)p_distances[450];
				GPIO_SetBits(GPIOD,GPIO_Pin_14);
				gama = ((450 - (270 + gamac))*0.5*PI)/180;
				r = sqrt(pow(y2,2) + pow(l,2) - ((2*y2*l)*cos(gama)));
				beta = acos((pow(y2,2) - pow(r,2) - pow(l,2))/(-2*r*l));
				betadeg = ((180*beta)/PI);
			}

			if((betadeg > 90) && (y2 > turnlimit))
			{
				GPIO_ResetBits(GPIOD,GPIO_Pin_14);
				while(1)
				{
					dataRaw = scanData();
					st = dataprocessing(answer);
					y1 = p_distances[270-gamac];
					y2 = p_distances[270+gamac];

					if((y1 > y1limit) && (y2 > y2limit))
					{
						GPIO_SetBits(GPIOD,GPIO_Pin_13);
						Delay(0x000FFFFF);
					}
					else
						break;
				}
				return 1;
			}
		}

	}
	else
	{

		if(x1 < x2)
		{
			a = sqrt(pow(p,2) + pow(x1,2) - ((2*p*x1)*cos(gama1)));
			beta = acos((pow(x1,2) - pow(a,2) - pow(p,2))/(-2*a*p));
			betadeg = ((180*beta)/PI);
			if(betadeg < 85)
			{
				GPIO_SetBits(GPIOD,GPIO_Pin_12);
			}
			else if(betadeg > 95)
			{
				GPIO_SetBits(GPIOD,GPIO_Pin_14);
			}
			else
			{
				GPIO_SetBits(GPIOD,GPIO_Pin_13);
			}
			return 1;

		}
		else
		{
			b = sqrt(pow(l,2) + pow(x2,2) - ((2*x2*l)*cos(gama2)));
			beta = acos((pow(x2,2) - pow(b,2) - pow(l,2))/(-2*b*l));
			betadeg = ((180*beta)/PI);
			if(betadeg < 85)
			{
				GPIO_SetBits(GPIOD,GPIO_Pin_14);
			}
			else if(betadeg > 95)
			{
				GPIO_SetBits(GPIOD,GPIO_Pin_12);
			}
			else
			{
				GPIO_SetBits(GPIOD,GPIO_Pin_13);
			}
			return 1;
		}
	}

	return 1;
}



void EXTI0_IRQHandler()
{
	Delay(0x000AFFFF);
	i = scannerSetup();
	EXTI_ClearITPendingBit(EXTI_Line0);

	GPIO_SetBits(GPIOD, GPIO_Pin_14);
	Delay(0x01FFFFFF);
	GPIO_ResetBits(GPIOD, GPIO_Pin_14);
}

uint8_t *scanData(void)
{
	uint8_t *m, *g;
	/*vynuluje answer*/
	answerInit();
	/*login do scanneru*/
	m = (taskInit(get_meas_data));
	g = sendTask_getAns(m);
	k = ansCheck(g);

	return g;
}


void interuptInit(void)
{
	/*zapnout hodiny pro portA*/
	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);
	/*nastaveni portuA*/
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN;
	GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
	GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
	GPIO_Init(GPIOA, &GPIO_InitStructure);
	/*vstupni pin pro preruseni*/
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE);
	SYSCFG_EXTILineConfig(EXTI_PortSourceGPIOA, GPIO_PinSource0);
	/*nastaveni externi line pro preruseni*/
	EXTI_InitStructure.EXTI_Line = EXTI_Line0;
	EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt;
	EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Rising;
	EXTI_InitStructure.EXTI_LineCmd = ENABLE;
	EXTI_Init(&EXTI_InitStructure);
	/*nastaveni NVIC*/
	NVIC_InitStructure.NVIC_IRQChannel = EXTI0_IRQn;
	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
	NVIC_Init(&NVIC_InitStructure);
}


void ledInit(void)
{
	/*zapnout hodiny pro ledky, resp pro portD*/
	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD, ENABLE);
	/*nastaveni portu pro ledky*/
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12 | GPIO_Pin_13 | GPIO_Pin_14 | GPIO_Pin_15;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
	GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
	GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
	GPIO_Init(GPIOD, &GPIO_InitStructure);
}




