#include "stm32f30x_conf.h"
#include <stdlib.h>
#include <stdio.h>
#include "thetable.c"

#define PI 3.14159265358979323846
#define BUFFERSIZE 64
#define UNCERT 20
#define PERIOD 20

// IN DIESEM PROJEKT GILT:
// PROZESSORTAKT = 72 MHz
// AHB  Prescaler = 1
// APB1 Prescaler = 2
// APB2 Prescaler = 1

volatile char buffer[BUFFERSIZE];
volatile int bufferread = 0;

volatile int vertstate = 0;
volatile int horistate = 0;

volatile int pos = 0;
volatile char directions = 0;

volatile char photo [4];

volatile double rps = 0;

void vertstep(int direction);
void horistep(int direction);

int main(void){
// ---------------------------------------------------------------------
//	Enabling clocks
	RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOA, ENABLE);
	RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOB, ENABLE);
	RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOC, ENABLE);
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2,ENABLE);
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1,ENABLE);
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE);

	
// ---------------------------------------------------------------------
//	Init GPIO (A) for USART1 (Maya serial) and USART2 (PC serial)
	GPIO_InitTypeDef GPIO_Init_Struct;
	GPIO_Init_Struct.GPIO_Mode = GPIO_Mode_AF;
	GPIO_Init_Struct.GPIO_OType = GPIO_OType_PP;
	GPIO_Init_Struct.GPIO_Pin = GPIO_Pin_9 | GPIO_Pin_10 | GPIO_Pin_2 | GPIO_Pin_3;
	GPIO_Init_Struct.GPIO_PuPd = GPIO_PuPd_NOPULL;
	GPIO_Init_Struct.GPIO_Speed = GPIO_Speed_50MHz;
	
	GPIO_Init(GPIOA, &GPIO_Init_Struct);
	GPIO_PinAFConfig(GPIOA,GPIO_PinSource9,GPIO_AF_7);
	GPIO_PinAFConfig(GPIOA,GPIO_PinSource10,GPIO_AF_7);
	GPIO_PinAFConfig(GPIOA,GPIO_PinSource2,GPIO_AF_7);
	GPIO_PinAFConfig(GPIOA,GPIO_PinSource3,GPIO_AF_7);
// ---------------------------------------------------------------------
// Init GPIO (A) for debugging with LED2 and extra PA6 for osci
	
//	GPIO_Init_Struct.GPIO_Mode = GPIO_Mode_OUT;
//	GPIO_Init_Struct.GPIO_OType = GPIO_OType_PP;
//	GPIO_Init_Struct.GPIO_Pin = GPIO_Pin_5 | GPIO_Pin_6;
//	GPIO_Init_Struct.GPIO_PuPd = GPIO_PuPd_NOPULL;
//	GPIO_Init_Struct.GPIO_Speed = GPIO_Speed_50MHz;
//	
//	GPIO_Init(GPIOA,&GPIO_Init_Struct);
	
// ---------------------------------------------------------------------
//	Init GPIO (B) for Vertical Stepper
	GPIO_Init_Struct.GPIO_Mode = GPIO_Mode_OUT;
	GPIO_Init_Struct.GPIO_OType = GPIO_OType_PP;
	GPIO_Init_Struct.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7 | GPIO_Pin_8  | GPIO_Pin_9;
	GPIO_Init_Struct.GPIO_PuPd = GPIO_PuPd_NOPULL;
	GPIO_Init_Struct.GPIO_Speed = GPIO_Speed_50MHz;
	
	GPIO_Init(GPIOB, &GPIO_Init_Struct);
	GPIOB->ODR &= 0xF<<6;
	
// ---------------------------------------------------------------------
//	Init GPIO (B) for Lichtschranken
	GPIO_Init_Struct.GPIO_Mode = GPIO_Mode_IN;
	GPIO_Init_Struct.GPIO_Pin =  GPIO_Pin_3  | GPIO_Pin_4;
	GPIO_Init_Struct.GPIO_PuPd = GPIO_PuPd_DOWN;
	
	GPIO_Init(GPIOB, &GPIO_Init_Struct);
	
// ---------------------------------------------------------------------
//	Init GPIO (C) for Horizontal Stepper
	GPIO_Init_Struct.GPIO_Mode = GPIO_Mode_OUT;
	GPIO_Init_Struct.GPIO_OType = GPIO_OType_PP;
	GPIO_Init_Struct.GPIO_Pin = GPIO_Pin_9 | GPIO_Pin_10 | GPIO_Pin_11  | GPIO_Pin_12;
	GPIO_Init_Struct.GPIO_PuPd = GPIO_PuPd_NOPULL;
	GPIO_Init_Struct.GPIO_Speed = GPIO_Speed_50MHz;
	
	GPIO_Init(GPIOC, &GPIO_Init_Struct);
	GPIOC->ODR = 0;
	
// ---------------------------------------------------------------------
// Init TIM2 for Stepping every PERIOD ms
	TIM_TimeBaseInitTypeDef TIM_Init_Struct;
	TIM_Init_Struct.TIM_Prescaler = 7200 - 1; // 100 mcs per tick (0.1ms)
	TIM_Init_Struct.TIM_Period = PERIOD - 1;
	
	TIM_TimeBaseInit(TIM2, &TIM_Init_Struct);
	TIM_ITConfig(TIM2, TIM_IT_Update, ENABLE); //IT enable
	TIM_Cmd(TIM2,ENABLE);
// ---------------------------------------------------------------------
// Init TIM3 for rps calculation
	TIM_Init_Struct.TIM_Prescaler = 7200 - 1; // 100 mcs per tick (0.1ms)
	TIM_Init_Struct.TIM_Period = 0xFFFF;
	
	TIM_TimeBaseInit(TIM3, &TIM_Init_Struct);
	TIM_ITConfig(TIM3, TIM_IT_Update, ENABLE);
	TIM_Cmd(TIM3,ENABLE);	
// ---------------------------------------------------------------------
//	Init USART1 (Maya serial)
	USART_InitTypeDef USART_Init_Struct;
	USART_Init_Struct.USART_BaudRate = 115200;
	USART_Init_Struct.USART_Parity = USART_Parity_No;
	USART_Init_Struct.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
	USART_Init_Struct.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
	USART_Init_Struct.USART_StopBits = USART_StopBits_1;
	USART_Init_Struct.USART_WordLength = USART_WordLength_8b;
	
	USART_Init(USART1,&USART_Init_Struct);
	USART_ITConfig(USART1, USART_IT_RXNE, ENABLE); // RXNE = ReceiveDataRegister Not Empty
	USART_ITConfig(USART1, USART_IT_ORE, DISABLE);
	USART_Cmd(USART1,ENABLE);
// ---------------------------------------------------------------------
//	Init USART2 (PC serial)
	USART_Init_Struct.USART_BaudRate = 115200;
	USART_Init_Struct.USART_Parity = USART_Parity_No;
	USART_Init_Struct.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
	USART_Init_Struct.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
	USART_Init_Struct.USART_StopBits = USART_StopBits_1;
	USART_Init_Struct.USART_WordLength = USART_WordLength_8b;
	
	USART_Init(USART2,&USART_Init_Struct);
	USART_ITConfig(USART2, USART_IT_TC, DISABLE);
	USART_Cmd(USART2,ENABLE);
	
// ---------------------------------------------------------------------
//	Init EXTI for Lichtschranke MAYA 
	SYSCFG_EXTILineConfig(EXTI_PortSourceGPIOB, EXTI_PinSource4);
	
	EXTI_InitTypeDef EXTI_Init_Struct;
	EXTI_Init_Struct.EXTI_Line = EXTI_Line4;
	EXTI_Init_Struct.EXTI_LineCmd = ENABLE;
	EXTI_Init_Struct.EXTI_Mode = EXTI_Mode_Interrupt;
	EXTI_Init_Struct.EXTI_Trigger = EXTI_Trigger_Rising_Falling;
	
	EXTI_Init(&EXTI_Init_Struct);
	
// ---------------------------------------------------------------------
//	NVIC
	NVIC_InitTypeDef NVIC_Init_Struct;
// USART2 (PC serial)
	NVIC_Init_Struct.NVIC_IRQChannel = USART2_IRQn;
	NVIC_Init_Struct.NVIC_IRQChannelCmd = ENABLE;
	NVIC_Init_Struct.NVIC_IRQChannelPreemptionPriority = 4;
	NVIC_Init_Struct.NVIC_IRQChannelSubPriority = 4;
	NVIC_Init(&NVIC_Init_Struct);
// EXTI LICHTSCHRANKE
	NVIC_Init_Struct.NVIC_IRQChannel = EXTI4_IRQn;
	NVIC_Init_Struct.NVIC_IRQChannelCmd = ENABLE;
	NVIC_Init_Struct.NVIC_IRQChannelPreemptionPriority = 3;
	NVIC_Init_Struct.NVIC_IRQChannelSubPriority = 3;
	NVIC_Init(&NVIC_Init_Struct);
// TIM3 (rps measurement)
	NVIC_Init_Struct.NVIC_IRQChannel = TIM3_IRQn;
	NVIC_Init_Struct.NVIC_IRQChannelCmd = ENABLE;
	NVIC_Init_Struct.NVIC_IRQChannelPreemptionPriority = 2;
	NVIC_Init_Struct.NVIC_IRQChannelSubPriority = 2;
	NVIC_Init(&NVIC_Init_Struct);
// TIM2 (Stepping every PERIOD ms)
	NVIC_Init_Struct.NVIC_IRQChannel = TIM2_IRQn;
	NVIC_Init_Struct.NVIC_IRQChannelCmd = ENABLE;
	NVIC_Init_Struct.NVIC_IRQChannelPreemptionPriority = 1;
	NVIC_Init_Struct.NVIC_IRQChannelSubPriority = 1;
	NVIC_Init(&NVIC_Init_Struct);
// USART1 (Maya serial)
	NVIC_Init_Struct.NVIC_IRQChannel = USART1_IRQn;
	NVIC_Init_Struct.NVIC_IRQChannelCmd = ENABLE;
	NVIC_Init_Struct.NVIC_IRQChannelPreemptionPriority = 0;
	NVIC_Init_Struct.NVIC_IRQChannelSubPriority = 0;
	NVIC_Init(&NVIC_Init_Struct);
	
// ---------------------------------------------------------------------
// Wrapup
//	GPIO_ResetBits(GPIOA, GPIO_Pin_5);
//	GPIO_ResetBits(GPIOA, GPIO_Pin_6);

	
// ---------------------------------------------------------------------
  /* Infinite loop */
  while (1);
}

void USART1_IRQHandler(void) {
//	GPIO_SetBits(GPIOA, GPIO_Pin_6);
	char recvd1 = USART_ReceiveData(USART1);
	photo[(int)recvd1>>0x6] = recvd1 & 0x3F; // recvd>>0x6 = 0b 0000 00xx // 0x3F = 0b 0011 1111
	
	USART_ClearFlag(USART1, USART_FLAG_ORE);
//	GPIO_ResetBits(GPIOA, GPIO_Pin_6);
}

void USART2_IRQHandler(void) {
	if (buffer[bufferread] == '\r') {
		bufferread = 0;
		USART_SendData(USART2, '\r');
		USART_ITConfig(USART2, USART_IT_TC, DISABLE);
	}else {
		USART_SendData(USART2, buffer[bufferread++]);
	}
}

void EXTI4_IRQHandler(void){
	//GPIO_SetBits(GPIOA, GPIO_Pin_5);
	int ls1 = GPIO_ReadInputDataBit(GPIOB,GPIO_Pin_3);
	int ls2 = GPIO_ReadInputDataBit(GPIOB,GPIO_Pin_4);
	
	directions = directions<<1;
	
	if (ls1 == ls2) {
		pos++;
		directions |= 1;
	}
	else
		pos--;
	
	char last3 = directions & 7; // 7 = 0b0111
	if ((last3 == 5) || (last3 == 2)) { // 5 = 0b0101 // 2 = 0b0010 // if NULLPOSITION
		
		long temp = TIM_GetCounter(TIM3);
		if (temp != 0){
			rps = (10000) / (double)temp;
			TIM_SetCounter(TIM3, 0);
			sprintf(buffer, "%lf rps          \r", rps);
			USART_ITConfig(USART2, USART_IT_TC, ENABLE);
		}else {
			TIM_Cmd(TIM3, ENABLE);
			rps = 0;
			sprintf(buffer, "%lf rps          \r", rps);
			USART_ITConfig(USART2, USART_IT_TC, ENABLE);
		}
		
		if (pos >= 0)
			pos = 0;
		else
			pos = 229;
	}
	
	//GPIO_ResetBits(GPIOA, GPIO_Pin_5);
	EXTI_ClearITPendingBit(EXTI_Line4);
}

void TIM3_IRQHandler(void) {
	TIM_Cmd(TIM3, DISABLE);
	TIM_SetCounter(TIM3, 0);
	TIM_ClearITPendingBit(TIM3, TIM_IT_Update);
}

void TIM2_IRQHandler(void) {
//	GPIO_SetBits(GPIOA, GPIO_Pin_5);
	
//	int x = photo[0] + photo[1] - photo[2] - photo[3];
//	int y = photo[0] - photo[1] + photo[2] - photo[3];
//	
//	double abs = sqrt(x*x+y*y);
//	double angle = atan(y/x);
//	if (x < 0)
//		angle += PI;
//	double rot = -pos *  2 * PI / 230;
//	
//	x = (int)(abs * cos(angle + rot));
//	y = (int)(abs * sin(angle + rot));

	double x1 = photo[0] + photo[1] - photo[2] - photo[3];
	double y1 = photo[0] - photo[1] + photo[2] - photo[3];
	
	double x = x1 * cos_lookup[pos] + y1 * sin_lookup[pos];
	double y = y1 * cos_lookup[pos] - x1 * sin_lookup[pos];
	
	//INVERSION IS INSIDE HERE AS WELL
	if (x > UNCERT)
		horistep(1);
	else if (x < -UNCERT)
		horistep(-1);
		
	if (y > UNCERT)
			vertstep(1);
	else if (y < -UNCERT)
			vertstep(-1);
	
//	GPIO_ResetBits(GPIOA, GPIO_Pin_5);
	TIM_ClearITPendingBit(TIM2, TIM_IT_Update);
}



void vertstep(int direction){ // POSITIVE direction = DOWN
	vertstate += direction;
	
	if(vertstate > 7)
		vertstate = 0;
	if(vertstate < 0)
		vertstate = 7;
	
	switch(vertstate){
		 case 0:
			 GPIOB->ODR = 1<<9;
		 break; 
		 case 1:
			 GPIOB->ODR = 1<<9 | 1<<8;
		 break; 
		 case 2:
			 GPIOB->ODR = 1<<8;
		 break; 
		 case 3:
			 GPIOB->ODR = 1<<8 | 1<<7;
		 break; 
		 case 4:
			 GPIOB->ODR = 1<<7;
		 break; 
		 case 5:
			 GPIOB->ODR = 1<<7 | 1<<6;
		 break; 
			 case 6:
			 GPIOB->ODR = 1<<6;
		 break; 
		 case 7:
			 GPIOB->ODR = 1<<6 | 1<<9;
		 break; 
		 default:
			 GPIOB->ODR &= 0xF<<6;
		 break; 
	}
}


void horistep(int direction) {// POSITIVE direction = LEFT
	horistate += direction;
	
	if(horistate > 7)
		horistate = 0;
	if(horistate < 0)
		horistate = 7;
	
	switch(horistate){
		 case 0:
			 GPIOC->ODR = 1<<12;
		 break; 
		 case 1:
			 GPIOC->ODR = 1<<12 | 1<<11;
		 break; 
		 case 2:
			 GPIOC->ODR = 1<<11;
		 break; 
		 case 3:
			 GPIOC->ODR = 1<<11 | 1<<10;
		 break; 
		 case 4:
			 GPIOC->ODR = 1<<10;
		 break; 
		 case 5:
			 GPIOC->ODR = 1<<10 | 1<<9;
		 break; 
			 case 6:
			 GPIOC->ODR = 1<<9;
		 break; 
		 case 7:
			 GPIOC->ODR = 1<<9 | 1<<12;
		 break; 
		 default:
			 GPIOC->ODR &= 0xF<<9;
		 break; 
	}
}