#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 PI 3.14159265


GPIO_InitTypeDef GPIO_InitStructure;
USART_InitTypeDef USART_InitStructure;
EXTI_InitTypeDef EXTI_InitStructure;
NVIC_InitTypeDef NVIC_InitStructure;

/*Funkcni prototypy*/
void ledInit(void);
void usartInit(void);
void interuptInit(void);
uint8_t scannerSetup(void);
void answerInit(void);
unsigned char *sendTask_getAns(unsigned char*);
uint8_t *scanData(void);
unsigned short LineSegmentation(unsigned short int []);
unsigned short PathFinder(unsigned short);
void Delay(__IO uint32_t nCount);


uint8_t task[askmax];
uint8_t answer[ansmax];
uint8_t i = 1;
uint8_t k = 0;
uint8_t *c, *p, *dataRaw;
unsigned short p_distances[541];
LINE AllLines[540];
CORNER Corners[541];
WAY ways[270];
extern char *endPtr;
uint16_t st;



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

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

	while(1)
    {

		if( i == 1)
		{
			dataRaw = scanData();
			st = fast(answer);
			l = LineSegmentation(p_distances);
			due = PathFinder(l);

		}
    }
}

unsigned short PathFinder(unsigned short CornCount)
{
	float MaxDistance, MaxWeight;
	unsigned short WeightTable[CornCount];
	unsigned short index,MaxWindex, MaxDindex, index2;

	index2 = 0;
	MaxWeight = 0;
	MaxWindex = 0;

	for(index = 0; index < CornCount; index++)
	{
		Corners[index].Banned = 0;
	}


	for(index2 = 0; index2 < CornCount; index2++)
	{
		for(index = 0; index < CornCount; index++)
		{
			if(Corners[index].Banned != 1 )
			{
				if( Corners[index].Weight > MaxWeight)
				{
					MaxWeight = Corners[index].Weight;
					WeightTable[index2] = index;
					MaxWindex = index;
				}
			}
		}
		MaxWeight = 0;
		Corners[MaxWindex].Banned = 1;
	}

	for(index = 0; index < CornCount; index++)
	{
		Corners[index].Banned = 0;
	}

	float a,b,s,gama;
	unsigned short waycount = 0;

	index = 0;
	index2 = 0;

	for(index = 0; index < CornCount-1; index++)
	{
		a = Corners[WeightTable[index]].Distance;
		b = Corners[WeightTable[index+1]].Distance;
		if (Corners[WeightTable[index]].AbsAngle > Corners[WeightTable[index+1]].AbsAngle)
			gama = (0.5*(Corners[WeightTable[index]].AbsAngle - Corners[WeightTable[index+1]].AbsAngle)*PI)/180;
		else
			gama = (0.5*(Corners[WeightTable[index+1]].AbsAngle - Corners[WeightTable[index]].AbsAngle)*PI)/180;

		if (gama > PI)
			gama = (2*PI) - gama;

		if(b > a)
		{
			s = sqrt(pow(a,2) + pow(a,2) - ((2*a*a)*cos(gama)));
		}
		else
		{
			s = sqrt(pow(b,2) + pow(b,2) - ((2*b*b)*cos(gama)));
		}

		if(s >400)
		{
			ways[index2].space = s;
			if (Corners[WeightTable[index]].AbsAngle > Corners[WeightTable[index+1]].AbsAngle)
			{
				ways[index2].angle =(unsigned short)((Corners[WeightTable[index]].AbsAngle - Corners[WeightTable[index+1]].AbsAngle)/2)+Corners[WeightTable[index+1]].AbsAngle;
				ways[index2].midlength = p_distances[ways[index2].angle];
			}
			else
			{
				ways[index2].angle =(unsigned short)((Corners[WeightTable[index+1]].AbsAngle - Corners[WeightTable[index]].AbsAngle)/2)+Corners[WeightTable[index]].AbsAngle;
				ways[index2].midlength = p_distances[ways[index2].angle];
			}
			index2++;
			waycount++;
		}
	}

	MaxDistance = 0;

	for(index2 = 0; index2 < waycount; index2++)
	{
		if( ways[index2].midlength > MaxDistance)
		{
			MaxDistance = ways[index2].midlength;
			MaxDindex = index2;
		}
	}


	GPIO_ResetBits(GPIOD, GPIO_Pin_12 | GPIO_Pin_13 | GPIO_Pin_14 | GPIO_Pin_15);
	if(ways[MaxDindex].angle <= 45)
	{
		GPIO_SetBits(GPIOD, GPIO_Pin_14 | GPIO_Pin_15);
	}
	else if ((ways[MaxDindex].angle > 45) && (ways[MaxDindex].angle <= 135))
	{
		GPIO_SetBits(GPIOD, GPIO_Pin_14);
	}
	else if ((ways[MaxDindex].angle > 135) && (ways[MaxDindex].angle <= 225))
	{
		GPIO_SetBits(GPIOD, GPIO_Pin_14 | GPIO_Pin_13);
	}
	else if ((ways[MaxDindex].angle > 225) && (ways[MaxDindex].angle <= 315))
	{
		GPIO_SetBits(GPIOD, GPIO_Pin_13);
	}
	else if ((ways[MaxDindex].angle > 315) && (ways[MaxDindex].angle <= 405))
	{
		GPIO_SetBits(GPIOD, GPIO_Pin_13 | GPIO_Pin_12);
	}
	else if ((ways[MaxDindex].angle > 405) && (ways[MaxDindex].angle <= 495))
	{
		GPIO_SetBits(GPIOD, GPIO_Pin_12);
	}
	else if (ways[MaxDindex].angle > 495)
	{
		GPIO_SetBits(GPIOD, GPIO_Pin_12 | GPIO_Pin_15);
	}

	return ways[MaxDindex].angle;
}



unsigned short LineSegmentation(unsigned short int MeasuredValues[])
{
	unsigned short i,j,k,l;
	unsigned short step;
	float n_j, n_k, v, v_p, c, linelength, linepoint, isonline;
	float alfa, beta, gama, alfax, betax, gaman, delta;

	l = 0;
	i = 540;
	delta = 50;
	j = 0;
	k = 1;
	step = 1;
	linelength = 0;
	linepoint = 1;
	isonline = 0;


	while(1)
	{
		if(isonline == 1)
		{
			n_j = (float)MeasuredValues[(j*step)];
			gaman = (0.5*PI*linepoint*step) / 180;
			/* výpočet délky strany c */
			c = sqrt(pow(n_j,2) + pow(v_p,2) - ((2*n_j*v_p)*cos(gaman)));
			/* výpočet úhlu beta */
			beta = acos((pow(v_p,2) - pow(c,2) - pow(n_j,2))/(-2*n_j*c));
			/* výpočet úhlu alfa */
			alfa = PI - gaman - beta;
			betax = PI - alfa;
			alfax = PI - gama - betax;
			v = (v_p*sin(betax))/(sin(alfax));
			v_p = v;
		}
		else
		{
			/* načti hodnoty z vektoru naměřených hodnot */
			n_j = (float)MeasuredValues[(j*step)];
			n_k = (float)MeasuredValues[(k*step)];
			/* převod úhlu gama, stupně na radiány */
			gama = (0.5*PI*step) / 180;
			/* výpočet délky strany c */
			c = sqrt(pow(n_j,2) + pow(n_k,2) - ((2*n_j*n_k)*cos(gama)));
			/* výpočet úhlu beta */
			beta = acos((pow(n_k,2) - pow(c,2) - pow(n_j,2))/(-2*n_j*c));
			/* výpočet úhlu alfa */
			alfa = PI - gama - beta;
			betax = PI - alfa;
			alfax = PI - gama - betax;
			v = (n_k*sin(betax))/(sin(alfax));
			v_p = v;
		}

		if(abs((v - MeasuredValues[((k+1)*step)])) <= delta )
		{
			isonline = 1;
			linepoint++;
			linelength += c;
			k++;
		}
		else
		{
			Corners[l].Distance = n_j;
			Corners[l].Weight = 1 / Corners[l].Distance;
			Corners[l].AbsAngle = (j*step);

			linepoint = 1;
			linelength = 0;
			isonline = 0;

			j = k;
			k++;
			l++;
		}

		if (k*step == 540)
		{
			n_k = (float)MeasuredValues[(k*step)];
			if(isonline == 1)
				Corners[l].Distance = v_p;
			else
				Corners[l].Distance = n_k;

			Corners[l].AbsAngle = (k*step);
			Corners[l].Weight = 1 / Corners[l].Distance;
			l++;
			break;
		}

	}
	return l;
}


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;
}

uint8_t scannerSetup(void)
{
	/*vynuluje answer*/
	answerInit();
	/*login do scanneru*/
	c = (taskInit(login));
	p = sendTask_getAns(c);
	k = ansCheck(p);
	if ( k == 1)
	{
		GPIO_ToggleBits(GPIOD, GPIO_Pin_12);
		Delay(0x0000FFFF);
		GPIO_ToggleBits(GPIOD, GPIO_Pin_12);
	}
	else
	{
		GPIO_ToggleBits(GPIOD, GPIO_Pin_14);
		Delay(0x0000FFFF);
		GPIO_ToggleBits(GPIOD, GPIO_Pin_14);
		return 0;
	}

	/*nastaveni scanneru*/
	answerInit();
	k = 0;
	c = (taskInit(set_freq_and_res));
	p = sendTask_getAns(c);
	k = ansCheck(p);
	if ( k == 1)
	{
		GPIO_ToggleBits(GPIOD, GPIO_Pin_15);
	}
	else
	{
		GPIO_ToggleBits(GPIOD, GPIO_Pin_14);
		Delay(0x0000FFFF);
		GPIO_ToggleBits(GPIOD, GPIO_Pin_14);
		return 0;
	}

	answerInit();
	k = 0;
	c = (taskInit(set_angle));
	p = sendTask_getAns(c);
	k = ansCheck(p);
	if ( k == 1)
	{
		GPIO_ToggleBits(GPIOD, GPIO_Pin_12);
	}
	else
	{
		GPIO_ToggleBits(GPIOD, GPIO_Pin_14);
		Delay(0x0000FFFF);
		GPIO_ToggleBits(GPIOD, GPIO_Pin_14);
		return 0;
	}


	/*nastaveni vystupnich dat scannerru*/
	answerInit();
	k = 0;
	c = (taskInit(output_conf));
	p = sendTask_getAns(c);
	k = ansCheck(p);
	if ( k == 1)
	{
		GPIO_ToggleBits(GPIOD, GPIO_Pin_13);
	}
	else
	{
		GPIO_ToggleBits(GPIOD, GPIO_Pin_14);
		Delay(0x0000FFFF);
		GPIO_ToggleBits(GPIOD, GPIO_Pin_14);
		return 0;
	}


	/*logout*/
	answerInit();
	k = 0;
	c = (taskInit(logout));
	p = sendTask_getAns(c);
	k = ansCheck(p);
	if ( k == 1)
	{
		GPIO_ToggleBits(GPIOD, GPIO_Pin_15);
		GPIO_ToggleBits(GPIOD, GPIO_Pin_13);
		GPIO_ToggleBits(GPIOD, GPIO_Pin_12);
	}
	else
	{
		GPIO_ToggleBits(GPIOD, GPIO_Pin_14);
		Delay(0x0000FFFF);
		GPIO_ToggleBits(GPIOD, GPIO_Pin_14);
		return 0;
	}

	return 1;
}


void answerInit()
{
	uint32_t a;
	for(a = 0; a<ansmax ; a++)
	{
		answer[a] = 0;
	}
}

unsigned char *sendTask_getAns(unsigned char *r)
{
	while(*r != etx)
	{
		USART_SendData(USART1, (*r));
		Delay(0x000000FF);
		r++;
	}
	USART_SendData(USART1, (*r));

	r = &answer[0];
	while(1)
	{
		while (USART_GetFlagStatus(USART1, USART_FLAG_RXNE) != SET)
		{;}
		*r = USART_ReceiveData(USART1);
		if(*r == etx)
		{
			USART_ClearFlag(USART1, USART_FLAG_RXNE);
			break;
		}

		if(USART_GetFlagStatus(USART1, USART_FLAG_RXNE) == SET)
		{}
		else {r++;}
	}
	return &answer[0];
}



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);
}

void usartInit(void)
{
	/*zapnout hodiny pro usart1°*/
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1, ENABLE);
	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE);
	/*nastavit piny portuB pro USART1*/
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
	GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
	GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
	GPIO_Init(GPIOB, &GPIO_InitStructure);
	/*namapovat piny na usart1*/
	GPIO_PinAFConfig(GPIOB, GPIO_PinSource6, GPIO_AF_USART1); /*Tx*/
	GPIO_PinAFConfig(GPIOB, GPIO_PinSource7, GPIO_AF_USART1); /*Rx*/
	/*nastaveni usart1*/
	USART_InitStructure.USART_BaudRate = 115200;
	USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
	USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
	USART_InitStructure.USART_Parity = USART_Parity_No;
	USART_InitStructure.USART_StopBits = USART_StopBits_1;
	USART_InitStructure.USART_WordLength = USART_WordLength_8b;
	USART_OverSampling8Cmd(USART1,ENABLE);
	USART_Init(USART1, &USART_InitStructure);
	/*zapnout usart1*/
	USART_Cmd(USART1, ENABLE);
}

void Delay(__IO uint32_t nCount) {
  while(nCount--) {
  }
}
