/**
 ******************************************************************************
 * @file           : main.c
 * @author         : Fernando Hermosillo Reynoso
 * @brief          : This program implements a basic vehicle injector control
 *									 simulation based on EXTI and timer encoder mode
 ******************************************************************************
 */

#include <stdint.h>
#include "stm32f103_hal.h"
#include "lcd.h"

/* Defines -----------------------------------------------------*/
#define ENGINE_NUM_TEETH	16
#define ADC_VREF	5.0F // 5.0 simulation / 3.3 real

/* Private Variables ------------------------------ */

/* Private constants -------------------------------*/


/* Prototype Function ------------------------------*/


/* User code -----------------------------------------------------*/
void delay_ms(uint16_t ms) {
	volatile unsigned long t = 0;
	for(uint16_t i = 0; i < ms; i++)
	{
		for(t = 0; t < 800; t++);
	}
}

void delay_us(uint16_t us)
{
	for (volatile unsigned int cycles = 0; cycles < us; cycles++);
}


volatile uint32_t tim1_overflow_count = 0;
volatile uint32_t ckp_count_prev = 0;
volatile uint32_t motor_Trps_us = 0;
volatile uint32_t motor_rps = 0;
volatile uint16_t motor_dt = 0;
uint32_t motor_temp = 0;


uint32_t compute_dt(uint32_t Tmotor_us, uint32_t TempMotor) {

  if (!Tmotor_us) return 0;
  uint32_t dt_us = Tmotor_us*(130 - TempMotor) / 120;
  if(dt_us < (Tmotor_us >> 2)) dt_us = Tmotor_us >> 2;
  if(dt_us > (Tmotor_us >> 1)) dt_us = Tmotor_us >> 1;

  return dt_us;
}

int main(void)
{
	// RCC configs
	rcc_clock_enable(RCC_GPIOA);
	rcc_clock_enable(RCC_GPIOC);
	rcc_clock_enable(RCC_TIM1);	// Encoder mode
	rcc_clock_enable(RCC_TIM2);	// Encoder sampling time
	rcc_clock_enable(RCC_TIM3);	// PWM
	rcc_clock_enable(RCC_TIM4);	// dt
	rcc_clock_enable(RCC_EXTI);	// TDC
	rcc_clock_enable(RCC_ADC1);	// LM35

	// GPIO configs
	gpio_set_output(GPIOC, GPIO_PIN_13, GPIO_OUTPUT_PP, GPIO_SPEED_2MHZ);
	gpio_set_input(GPIOA, GPIO_PIN_2, GPIO_INPUT_ANALOG); // Channel 2

	// LCD
  LCD_Init();
	LCD_Print("Vrps=");
	LCD_SetCursor(0,1);
	LCD_Print("Dt=");
	
	// ADC config
	adc_init(ADC1, ADC_MODE_SINGLE, ADC_TRIG_SOFTWARE, ADC_ALIGN_RIGHT);
	adc_set_channel(ADC1, ADC_CHANNEL_2, ADC_SMP_7_5);

	// Timer 1 configs: Encoder read
	timer_init(TIM1, TIMER_MODE_UP, 65535, 0);
	timer_set_ic_channel(TIM1,
        TIMER_CHANNEL_1,
        TIMER_IC_DIRECT,
        TIMER_IC_RISING,
        TIMER_IC_DIV_1,
        TIMER_IC_FILTER_CKDTS_NONE);
	timer_set_trgi(TIM1, TIMER_SLAVE_ENCODER_MODE1, TIMER_TRGI_TI1FP1);
	//timer_set_encoder_mode(TIM1, TIMER_ENCODER_MODE_TI1);
	timer_set_interrupt(TIM1, TIMER_IRQ_UPDATE, IRQ_ENABLE);
	nvic_enable_irq(TIM1_UP_IRQn);

	// Timer 2 configs: CKP Period
	timer_init(TIM2, TIMER_MODE_UP, 9999, 7); // TimeBase=1us, Period=10ms
	timer_set_interrupt(TIM2, TIMER_IRQ_UPDATE, IRQ_ENABLE);
	nvic_enable_irq(TIM2_IRQn);

	// Timer 4 configs: DeltaTime
	timer_init(TIM4, TIMER_MODE_UP, 65535, 7); // TimeBase=1us
	timer_set_interrupt(TIM4, TIMER_IRQ_UPDATE, IRQ_ENABLE);
	nvic_enable_irq(TIM4_IRQn);
	//timer_start(TIM4, TIMER_CONTINUOUS);

	// Timer 3 configs (Injector PWM)
	timer_init(TIM3, TIMER_MODE_UP, 9999, 7); // TimeBase=1us, Period=10ms/Freq=100Hz
	timer_set_oc_channel(TIM3,
											TIMER_CHANNEL_1,
											TIMER_OC_PWM1,
											TIMER_OC_POLARITY_LOW,
											1000,	//20% DC
											true);

	// NVIC configs
	exti_set_interrupt(PA1, EXTI_IRQ_RISING); // TDC


	timer_start(TIM2, TIMER_CONTINUOUS);
	timer_start(TIM1, TIMER_CONTINUOUS);
	nvic_enable_irq(EXTI1_IRQn);
	while (1)
	{
		// Leer canal IN2 (PA2)
		uint16_t adc_value=adc_read(ADC1);
		motor_temp = (adc_value*(uint32_t)(ADC_VREF*100)) >> 12;

		// Print to LCD
		LCD_SetCursor(5, 0);
		LCD_Print("        ");
		LCD_SetCursor(5, 0);
		LCD_Print_Int(motor_rps);
		LCD_SetCursor(4, 1);
		LCD_Print("        ");
		LCD_SetCursor(4, 1);
		LCD_Print_Int(motor_dt);

		// Screen delay
		delay_ms(100);
	}
}



void TIM1_UP_IRQHandler(void)
{
	// Clear pending flag
	TIM1->SR &= ~TIM_SR_UIF;

	GPIOC->ODR ^= 1 << 13;
	tim1_overflow_count++;
}

void TIM2_IRQHandler(void)
{
	// Clear pending flag
	TIM2->SR &= ~TIM_SR_UIF;

	// Capture current Timer counter value
	uint32_t  ckp_count = TIM1->CNT + (tim1_overflow_count << 16);

	// Vrps = DeltaQ / (Ts * ENGINE_NUM_TEETH) = DeltaQ*Fs/ENGINE_NUM_TEETH
	motor_rps = ((ckp_count - ckp_count_prev)*100) / ENGINE_NUM_TEETH;

	// Update tick
	ckp_count_prev = ckp_count;	
}

// TDC
void EXTI1_IRQHandler(void)
{
	// Clear pending flag
	EXTI->PR = (1 << 1);
	
	// Compute dt
	if(motor_rps)
	{
		// Compute dt
		uint32_t motor_Trps_us = 1e6 / motor_rps;
		uint32_t dt = compute_dt(motor_Trps_us, motor_temp);
		if(dt > 65535) {
			TIM4->PSC = 79; // 10us basetime
			dt /= 10;
		}
		else 
		{
			TIM4->PSC = 7;
		}

		// Start timer
		motor_dt = (uint16_t)dt;
		TIM4->ARR = (uint16_t)dt;
		timer_start(TIM4, TIMER_ONESHOT);
	} else{
		motor_dt = 0;
	}
}

void TIM4_IRQHandler(void)
{
	// Clear pending flag
	TIM4->SR &= ~TIM_SR_UIF;

	// Send PWM
	timer_start(TIM3, TIMER_ONESHOT);
}