#include <interrupt/interrupt.h>
#include <os/thread.h>
#include <os/time.h>
#include <can/can.h>
#include <usart/usart.h>
#include <gpio/pin.h>
#include <timer/timer.h>
#include <adc/adc.h>

static Pin& led_ready = PB3;
static Pin& led_error = PB4;

static Pin& foo = PA9;

static Pin& a_l = PA7;
static Pin& b_l = PB0;
static Pin& c_l = PB1;

static Pin& a_h = PB6;
static Pin& b_h = PB7;
static Pin& c_h = PB8;

static Pin& sense_a = PA1; // ADC IN1
static Pin& sense_b = PA2; // ADC IN2
static Pin& sense_c = PA3; // ADC IN3

volatile bool sense_rising = 0;

class Commutator {
	private:
		int state;
		volatile uint32_t* active_pwm;
	
	public:
		Commutator() : state(0), active_pwm(&TIM4.CCR1) {}
		
		void set_pwm(uint32_t pwm) {
			*active_pwm = pwm;
		}
		
		void next() {
			uint32_t temp;
			
			switch(state++) {
				case 0:
					b_l.off();
					c_l.on();
					ADC1.JSQR = 2 << 15;
					sense_rising = true;
					break;
				
				case 1:
					temp = *active_pwm;
					*active_pwm = 0;
					active_pwm = &TIM4.CCR2;
					*active_pwm = temp;
					ADC1.JSQR = 1 << 15;
					sense_rising = false;
					break;
				
				case 2:
					c_l.off();
					a_l.on();
					ADC1.JSQR = 3 << 15;
					sense_rising = true;
					break;
				
				case 3:
					temp = *active_pwm;
					*active_pwm = 0;
					active_pwm = &TIM4.CCR3;
					*active_pwm = temp;
					ADC1.JSQR = 2 << 15;
					sense_rising = false;
					break;
				
				case 4:
					a_l.off();
					b_l.on();
					ADC1.JSQR = 1 << 15;
					sense_rising = true;
					break;
				
				case 5:
					temp = *active_pwm;
					*active_pwm = 0;
					active_pwm = &TIM4.CCR1;
					*active_pwm = temp;
					ADC1.JSQR = 3 << 15;
					sense_rising = false;
					
					state = 0;
					break;
			}
		}
};

Commutator commutator;

volatile uint32_t ctv = 170;

volatile uint32_t ct = 0;
volatile uint32_t cc = 200;

volatile bool do_cc_est = 0;
volatile bool done_cc_est = 0;

template <>
void interrupt<Interrupt::ADC1_2>() {
	if(ADC1.SR & 0x04) {
		USART1.reg.DR = ADC1.JDR1;
		
		if(sense_rising ? ADC1.JDR1 > ctv : ADC1.JDR1 < ctv) {
			//foo.on();
			if(do_cc_est && !done_cc_est && ct > 10) {
				cc = cc * 0.95 + (ct * 2) * 0.05;
				done_cc_est = true;
			}
		} else {
			//foo.off();
		}
	}
	
	ADC1.SR = 0;
}

int main() {
	// Initialize system timer.
	STK.LOAD = 72000000 / 8 / 1000; // 1000 Hz.
	STK.CTRL = 0x03;
	
	RCC.enable(RCC.AFIO);
	RCC.enable(RCC.GPIOA);
	RCC.enable(RCC.GPIOB);
	
	//AFIO.MAPR = 0x02000000; // Free JTAG pins used for LEDs.
	
	//led_ready.set_mode(Pin::Output);
	//led_error.set_mode(Pin::Output);
	
	RCC.enable(RCC.CAN);
	CAN1.enable();
	
	PA12.set_mode(Pin::AF);
	
	foo.set_mode(Pin::AF);
	RCC.enable(RCC.USART1);
	USART1.set_baudrate(1000000);
	USART1.enable();
	USART1.reg.CR1 |= 1 << 12;
	
	// ADC.
	RCC.enable(RCC.ADC1);
	
	// Calibrate ADC.
	ADC1.CR2 = 0x9;
	while(ADC1.CR2 & 0x8);
	ADC1.CR2 = 0x5;
	while(ADC1.CR2 & 0x4);
	
	sense_a.set_mode(Pin::Analog);
	sense_b.set_mode(Pin::Analog);
	sense_c.set_mode(Pin::Analog);
	
	// Motor outputs.
	a_l.off();
	a_l.set_mode(Pin::Output);
	//b_l.off();
	b_l.on();
	b_l.set_mode(Pin::Output);
	c_l.off();
	c_l.set_mode(Pin::Output);
	
	a_h.set_mode(Pin::AF);
	b_h.set_mode(Pin::AF);
	c_h.set_mode(Pin::AF);
	
	RCC.enable(RCC.TIM4);
	TIM4.PSC = 0;
	TIM4.ARR = 1000;
	TIM4.CCER = 0x1111;
	TIM4.CCMR1 = 0x6868;
	TIM4.CCMR2 = 0x6868;
	
	TIM4.CR2 = 0x70;
	
	TIM4.CR1 = 0x40;
	TIM4.CR1 = 0x41;
	
	TIM4.CCR1 = 0;
	TIM4.CCR2 = 0;
	TIM4.CCR3 = 0;
	TIM4.CCR4 = 1;
	
	Interrupt::enable(Interrupt::ADC1_2);
	
	ADC1.CR1 |= 0x80;
	ADC1.CR2 |= 0xd001;
	
	ADC1.JSQR = 3 << 15;
	
	// Give all hardware enough time to initialize.
	Time::sleep(200);
	
	led_ready.on();
	
	// Configure CAN.
	CAN1.f_reg.FR[0][0] = 0;
	CAN1.f_reg.FR[0][1] = 0;
	CAN1.f_reg.FS1R[0] = 0x1;
	CAN1.f_reg.FA1R[0] = 0x1;
	CAN1.f_reg.FMR = 0;
	
	uint32_t pwm = 0;
	
	while(1) {
		// Check for CAN message.
		if(CAN1.reg.RF0R & 0x3) {
			switch(CAN1.mb_reg.RDL0R) {
				case '1':
					led_ready.toggle();
					break;
				
				case '2':
					led_error.toggle();
					break;
				
				case '+':
					pwm += 10;
					commutator.set_pwm(pwm);
					break;
				
				case '-':
					if(pwm >= 10) {
						pwm -= 10;
						commutator.set_pwm(pwm);
					}
					break;
				
				case 'e':
					pwm += 1;
					commutator.set_pwm(pwm);
					break;
				
				case 'd':
					if(pwm) {
						pwm -= 1;
						commutator.set_pwm(pwm);
					}
					break;
				
				case 'q':
					cc += 10;
					break;
				
				case 'a':
					cc -= 10;
					break;
				
				case 'w':
					cc++;
					break;
				
				case 's':
					cc--;
					break;
				
				case 'r':
					ctv += 1;
					break;
				
				case 'f':
					ctv -= 1;
					break;
				
				case 'y':
					do_cc_est = !do_cc_est;
					break;
				
				case '0':
					commutator.set_pwm(pwm = 0);
					cc = 200;
					do_cc_est = false;
					break;
			}
			
			// Release mailbox.
			CAN1.reg.RF0R = 0x20;
		}
		
		// Check for pwm update.
		if(TIM4.SR & 0x01) {
			TIM4.SR &= ~0x01;
			if(++ct >= cc) {
				commutator.next();
				done_cc_est = false;
				ct = 0;
			}
		}
		
		// Check for pulse center.
		if(TIM4.SR & 0x10) {
			TIM4.SR &= ~0x10;
		}
	}
}