/*
* atkrouter.c
*
* Created: 6/17/2018 2:48:08 PM
* Author : Jake
*/

#include <avr/io.h>
#include <avr/interrupt.h>
#include "hardware.h"
#include "fastmath.h"
#include "sinelut.h"

// first setup all the pins
// want six step commutation, or sinpwm on encoder reading? 

void clock_init(void){
	OSC.XOSCCTRL = OSC_XOSCSEL_XTAL_256CLK_gc | OSC_FRQRANGE_12TO16_gc; // select external source
	OSC.CTRL = OSC_XOSCEN_bm; // enable external source
	while(!(OSC.STATUS & OSC_XOSCRDY_bm)); // wait for external
	OSC.PLLCTRL = OSC_PLLSRC_XOSC_gc | OSC_PLLFAC0_bm | OSC_PLLFAC1_bm; // select external osc for pll, do pll = source * 3
	//OSC.PLLCTRL = OSC_PLLSRC_XOSC_gc | OSC_PLLFAC1_bm; // pll = source * 2 for 32MHz std clock
	OSC.CTRL |= OSC_PLLEN_bm; // enable PLL
	while (!(OSC.STATUS & OSC_PLLRDY_bm)); // wait for PLL to be ready
	CCP = CCP_IOREG_gc; // enable protected register change
	CLK.CTRL = CLK_SCLKSEL_PLL_gc; // switch to PLL for main clock
}

void uarts_init(void){
	// UP0
	rb_init(&up0rbrx);
	rb_init(&up0rbtx);
	pin_init(&up0rxled, &PORTE, PIN4_bm, 4, 1);
	pin_init(&up0txled, &PORTE, PIN5_bm, 5, 1);
	uart_init(&up0, &USARTE1, &PORTE, PIN6_bm, PIN7_bm, &up0rbrx, &up0rbtx, &up0rxled, &up0txled);
	//PORTE.PIN7CTRL = PORT_SRLEN_bm;
	uart_start(&up0, SYSTEM_BAUDA, SYSTEM_BAUDB);
	
	ups[0] = &up0;
}

void atkps_init(void){
	atkport_init(&atkp0, 0, &up0);
}

void encoder_init(void){
	// startup SPI
	pin_init(&spiEncCsPin, &PORTD, PIN4_bm, 4, 1);
	spi_init(&spiEncoder, &USARTD1, &PORTD, PIN6_bm, PIN7_bm, PIN5_bm, &spiEncCsPin);
	spi_start(&spiEncoder, 1);
	// startup object 
	ams5047_init(&ams5047, &spiEncoder);
}

#define ENC_RESOLUTION 16835

// a handful of constants, probably need many less
// divisor is # of poles on motor / 2 (for each n/s pair, one set of 3 coils per full rotation)
const static uint16_t enc_resolution = ENC_RESOLUTION;
const static uint8_t enc_reverse = 1; // 1 or 0, for reverse op
const static uint16_t twoPi_enc = 2340; //ENC_RESOLUTION / 7; // divisor is # poles / 2
const static uint16_t pi_enc = 1170;
const static int16_t enc_offset = -600; // start at midpoint - adjust -ve or +ve from

static uint16_t enc_reading = 0;
static uint16_t phase_target = 0;
// 2^14 = 16,384

void pwm_periods(uint16_t peru, uint16_t perv, uint16_t perw){
	// check overrun
	(peru > 1024) ? peru = 1024 : (0);
	(perv > 1024) ? perv = 1024 : (0);
	(perw > 1024) ? perw = 1024 : (0);
	// hi is lo, per xmega, let's undo this
	peru = 1024 - peru;
	perv = 1024 - perv;
	perw = 1024 - perw;
	// A: 3, B: 2, C: 1 - channels to board
	TCC0.CCABUFL = (uint8_t) perw;
	TCC0.CCABUFH = (uint8_t) (perw >> 8);
	TCC0.CCBBUFL = (uint8_t) perv;
	TCC0.CCBBUFH = (uint8_t) (perv >> 8);
	TCC0.CCCBUFL = (uint8_t) peru;
	TCC0.CCCBUFH = (uint8_t) (peru >> 8);
}

void pwm_by_offset(int16_t ofu, int16_t ofv, int16_t ofw){
	// +ve offset to spend more time with hi-side off, signals are complimentary
	uint16_t peru = 512 + ofu;
	uint16_t perv = 512 + ofv;
	uint16_t perw = 512 + ofw;
	// now through business
	pwm_periods(peru, perv, perw);
}

void pwm_by_sin_duty(uint16_t phase, float duty){
	// phases respective of home
	int16_t pu = phase;
	(pu >= twoPi_enc) ? (pu -= twoPi_enc) : (0);
	int16_t pv = phase + 780;
	(pv >= twoPi_enc) ? (pv -= twoPi_enc) : (0);
	int16_t pw = phase + 1560;
	(pw >= twoPi_enc) ? (pw -= twoPi_enc) : (0);
	
	// zero-set offset
	int16_t peru = ((sinelut[pu] - 512) * duty) + 512;
	int16_t perv = ((sinelut[pv] - 512) * duty) + 512;
	int16_t perw = ((sinelut[pw] - 512) * duty) + 512;
	
	pwm_periods(peru, perv, perw);
}

void pwm_init(void){
	// setup awex etc
	
	pin_init(&lo1, &PORTC, PIN4_bm, 4, 1);
	pin_init(&hi1, &PORTC, PIN5_bm, 5, 1);
	pin_init(&lo2, &PORTC, PIN2_bm, 2, 1);
	pin_init(&hi2, &PORTC, PIN3_bm, 3, 1);
	pin_init(&lo3, &PORTC, PIN0_bm, 0, 1);
	pin_init(&hi3, &PORTC, PIN1_bm, 1, 1);
	
	// compare and capture at value
	uint16_t per = 1024; // at DIV1, 1024 period is 23.5kHz
	// write low first, bc bussing / xmega 8-bit oddities cc datasheet @ 3.11
	uint8_t perl = (uint8_t) per;
	uint8_t perh = (uint8_t) (per >> 8);
	
	TCC0.CTRLA = TC_CLKSEL_DIV1_gc;
	TCC0.PERBUFL = perl;
	TCC0.PERBUFH = perh;
	
	// turnt to dual-slope pwm to have center aligned, and eventually sampling on top event
	
	TCC0.CTRLB = TC_WGMODE_DS_T_gc;// | (1 << 7) | (1 << 6) | (1 << 5); // dual slope, and enable channels a, b, c for capture
	
	AWEXC.CTRL = AWEX_DTICCAEN_bm | AWEX_DTICCBEN_bm | AWEX_DTICCCEN_bm;
	AWEXC.DTBOTHBUF = 2; // four counts of pwm clock for deadtime
	AWEXC.OUTOVEN = (1 << 0) | (1 << 1) | (1 << 2) | (1 << 3) | (1 << 4) | (1 << 5);
	
	pwm_periods(0, 0, 0);
}

void drv_init(void){
	// mode pins
	pin_init(&drvEnPin, &PORTD, PIN1_bm, 1, 1);
	pin_init(&drvModePwm, &PORTB, PIN6_bm, 6, 1);
	pin_init(&drvModeGain, &PORTC, PIN6_bm, 6, 1);
	pin_init(&drvDcCal, &PORTB, PIN7_bm, 7, 1);
	// status
	pin_init(&drvFault, &PORTC, PIN7_bm, 7, 0);
	pin_init(&drvOCTW, &PORTD, PIN0_bm, 0, 0);
	
	// setup drv8302 mode
	pin_clear(&drvModePwm); // low for 6-channel pwm, hi and lo sides from uc
	pin_set(&drvModeGain); // low for 10v/v, hi for 40v/v current sense gains
	pin_clear(&drvDcCal); // turn DC cal off, we turn this high to set midpoint on amps
	pin_clear(&drvEnPin); // disable the gate driver, to start. also broken by no/go hardware switch 
}

void drv_enable(void){
	pin_set(&drvEnPin);
}

void drv_disable(void){
	pin_clear(&drvEnPin);
}

void tickers_init(void){
	// sets up two timers
	
	// compare and capture at value
	uint16_t perStartSpeed = 1024; 
	// write low first, bc bussing / xmega 8-bit oddities cc datasheet @ 3.11
	uint8_t perSl = (uint8_t) perStartSpeed;
	uint8_t perSh = (uint8_t) (perStartSpeed >> 8);
	
	// turn on TCC0
	TCD0.CTRLA = TC_CLKSEL_DIV8_gc;
	TCD0.PERBUFL = perSl;
	TCD0.PERBUFH = perSh;
	
	// set cca interrupt on
	TCD0.INTCTRLA = TC_OVFINTLVL_HI_gc;
	
	/*
	// and a reasonable speed for acceleration ticking
	
	uint16_t perAccelRate = 800;
	uint8_t perAl = (uint8_t) perAccelRate;
	uint8_t perAh = (uint8_t) (perAccelRate >> 8);
	
	// another ticker to execute accel
	TCD1.CTRLA = TC_CLKSEL_DIV64_gc;
	TCD1.PERBUFL = perAl;
	TCD1.PERBUFH = perAh;
	
	TCD1.INTCTRLA = TC_OVFINTLVL_HI_gc;
	*/
}

int main(void)
{
	clock_init();
	uarts_init();
	atkps_init();
	
	// enable interrupts
	sei();
	PMIC.CTRL |= PMIC_LOLVLEN_bm | PMIC_MEDLVLEN_bm | PMIC_HILVLEN_bm;

	pin_init(&stlclk, &PORTE, PIN1_bm, 1, 1);
	pin_init(&stlerr, &PORTE, PIN0_bm, 0, 1);
	pin_set(&stlerr);
	pin_set(&stlclk);
	
	// startup encoder
	encoder_init();
	// start timers for commutation, accel tickers 
	tickers_init();
	
	// start pwm system
	pwm_init();
	// initialize the bldc state structure
	bldc_init(&bldc);
	// on startup, speed (if clcomm) and duty
	//bldc_setTargetSpeed(&bldc, 1000);
	// bldc_setDuty(&bldc, 10);
	// startup the driver
	drv_init();
	// and enable the gate
	drv_enable();

	// runtime globals
	uint32_t tck = 0;

	while (1)
	{
		atkport_scan(&atkp0, 2);
		// just... as fast as we can 
 		tck++;
 		// this modulo op is slow AF
 		// that means streamlining atkport_scan without modulos is probably a rad thing
    	if(!(fastModulo(tck, 4096))){
     		pin_toggle(&stlclk);
     	}
	}
}

// commutation timer
ISR(TCD0_OVF_vect){
	// CL 6-step Commutate
	
	// get encoder reading
	ams5047_read(&ams5047, &enc_reading);
	// reversal if
	enc_reverse ? enc_reading = enc_resolution - enc_reading : (0);
	// target phase expressed in 0 - 2PI, where 2PI in terms of an encoder counts' worth of ticks
	// we add the encoder's resolution here so that we can safely have negative encoder offsets
	phase_target = (enc_resolution + enc_reading + enc_offset + pi_enc) % twoPi_enc;
	
	// we need to compute the respective pwm positions given the phase target 
	pwm_by_sin_duty(phase_target, 0.04);
}

ISR(USARTE1_RXC_vect){
	uart_rxhandler(&up0);
}

ISR(USARTE1_DRE_vect){
	uart_txhandler(&up0);
}