#include "Driver_GPIO.h"
#include "MyTimer.h"
#include "MyADC.h"
#include "stm32f10x.h"
#include "bordage.h"

/********** PWM **********/
#define TIMER_PWM (TIM3)
#define CANAL_PWM (4)
#define GPIO_PWM (GPIOB)
#define GPIO_PIN_PWM (1)
/*************************/

/********** Codeur Incrémental **********/
#define TIMER_CI (TIM4)
#define GPIO_GIROUETTE (GPIOB)
#define GIROUETTE_PHA (6) //PB6
#define GIROUETTE_PHB (7) //PB7
#define GIROUETTE_INDEX (5) //PB5
/****************************************/

int tempo_chavirement = 0;

void IT_bordage_auto ( void) {
	int angle_girouette;

	if (tempo_chavirement > 0){
		tempo_chavirement--;
		return;
	}

	angle_girouette = TIMER_CI->CNT/4;
	bordage(calcul_angle_voile(angle_girouette));
}

void init_bordage ( void )
{
	MyGPIO_Struct_TypeDef GPIO_Struct;
	
	// Configuration du timer avec une période de 20ms
	MyTimer_Struct_TypeDef TIM;
	TIM.Timer = TIMER_PWM;
	TIM.ARR = 59999;
	TIM.PSC = 23; 
	MyTimer_Base_Init(&TIM);
	
	// Configuration du GPIO sur lequel sort la PWM
	GPIO_Struct.GPIO = GPIO_PWM;
	GPIO_Struct.GPIO_Pin = GPIO_PIN_PWM;
	GPIO_Struct.GPIO_Conf = AltOut_Ppull;
	MyGPIO_Init(&GPIO_Struct);
	
	MyTimer_PWM (TIMER_PWM, CANAL_PWM);
	
	// Mise en place d'une interruption gérant le bordage toute les 20ms  
	MyTimer_ActiveIT(TIMER_PWM, 1, IT_bordage_auto); 
}

void bordage ( float angle ) { 
	
	float angle_servo = 90.0 - angle;
	
	float duty_cycle = angle_servo/18.0 + 5.0; // convertit l'angle en rapport cyclique pour la commande du servo moteur

	// Génération de la PWM 
	Set_Duty_Cycle(TIMER_PWM, CANAL_PWM, duty_cycle);
}
 
void init_codeur_incr ( void ) {
	// Configuration GPIOS
	MyGPIO_Struct_TypeDef GPIO_Struct;
	GPIO_Struct.GPIO = GPIO_GIROUETTE;
	GPIO_Struct.GPIO_Pin = GIROUETTE_PHA;
	GPIO_Struct.GPIO_Conf = In_Floating;
	MyGPIO_Init(&GPIO_Struct);
	
	GPIO_Struct.GPIO_Pin = GIROUETTE_PHB;
	GPIO_Struct.GPIO_Conf = In_Floating;
	MyGPIO_Init(&GPIO_Struct);
	
	GPIO_Struct.GPIO_Pin = GIROUETTE_INDEX;
	GPIO_Struct.GPIO_Conf = In_Floating;
	MyGPIO_Init(&GPIO_Struct);
	
	init_exti_interrupt();
	MyTimer_EncoderMode_Conf(TIMER_CI);
	
}
void init_exti_interrupt ( void ) {
	RCC->APB2ENR |= RCC_APB2ENR_IOPBEN;
	RCC->APB2ENR |= RCC_APB2ENR_AFIOEN;
	AFIO->EXTICR[1] |= AFIO_EXTICR2_EXTI5_PB;
	EXTI->RTSR |= EXTI_RTSR_TR5; // Rising trigger enabled
	EXTI->IMR |= EXTI_IMR_MR5; // Interrupt request from Line 5 is not masked
	NVIC->IP[23] |= (1 << 4); // Fixe la priorité de l'interruption dans le NVIC
	NVIC->ISER[0] |= (1 << 23); // Autorise la prise en compte de l'interruption dans le NVIC
}

void EXTI9_5_IRQHandler ( void ) {
	EXTI->PR |= EXTI_PR_PR5; // Clear the pending bit
	TIMER_CI->CNT = 0;
}

void Roulis_Handler ( void )
{
	tempo_chavirement = 50; // 50 appels à 60ms = 3000 ms = 3 sec.
	bordage(0);
}

float calcul_angle_voile ( float angle ) {
	if (angle <= 45.0 || angle >= 360.0 - 45.0){
		return 0.0;
	}
	else if ( angle > 45.0 && angle <= 180.0) {
		return (angle - 45.0) * 90.0/135.0;
	}
	else {
		return (315.0 - angle) * 90.0/135.0;
	}
}