/**** Includes ****/
#include <avr/io.h>
#include <avr/eeprom.h>
#include "mcu_hal.h"

using namespace mcu;

/**** Private definitions ****/
/**** Private constants ****/
/**** Private variables ****/
static volatile uint16_t rtc_ms = 1000;

/**** Private function declarations ****/
static uint8_t gpio_read_level(uint8_t pin_reg, uint8_t mask);
static void pwm_write_ocx(uint8_t ch, uint16_t value);
static uint16_t pwm_read_ocx(uint8_t ch);

/**** Public function definitions ****/
void mcu::startup(startupCfg_t* hwCfg)
{
	// Fail-safe GPIO init
	PORTB = 0xF8;  // Set PORTB pull-ups
	DDRB = 0x00;   // Set all as inputs
	
	PORTC = 0x40;  // Set PORTC pull-ups
	DDRC = 0x00;   // Set all as inputs
	
	PORTD = 0x80;  // Set PORTD pull-ups
	DDRD = 0x00;   // Set all as inputs
	
	PORTE = 0x0A;  // Set PORTE pull-ups
	DDRE = 0x00;   // Set all as inputs
	
	// Half-bridge related pins
	PORTB &= ~0x03; //Set low
	DDRB |= 0x03;   //Set as output
	
	// Common OD PWM pin
	if(hwCfg->od_common_is_pwm) PORTB &= ~0x04; //Set low
	else PORTB |= 0x04;                         //Set high
	DDRB |= 0x04;                               //Set as output
	
	// OD control pins
	PORTD &= ~0x3F; //Set low (off)
	DDRD |= 0x3F;   //Set as outputs
	
	// Handbrake pull-up pin
	PORTB |= 0x20;  //Set high
	DDRB |= 0x20;   //Set as output
	
	// Handbrake and brakes pins
	PORTB |= 0xC0;  //Set pull-up on
	DDRB &= ~0xC0;   //Set as inputs
	
	// Dimm
	PORTD |= 0x80;  //Set pull-up on
	DDRD &= ~0x80;   //Set as input
	
	// Up and Down
	PORTE |= 0x0A;  //Set pull-up on
	DDRE &= ~0x0A;   //Set as inputs
	
	// Internal ADC inputs
	PORTC &= ~0x0F; //Pull-up off
	DDRC &= ~0x0F;  //Set as inputs
	
	// Potentiometer & Mode
	PORTC &= ~0x30; //Pull-up off
	DDRC &= ~0x30;  //Set as inputs
	
	// Freq-pull control pins
	PORTD &= ~0x40; //Set low
	DDRD |= 0x40;   //Set as output
	
	//ADC configuration
	PRR0 &= ~0x01; //Enable ADC power
	DIDR0 |= 0x0F; //Disable digital inputs, ADC0-ADC3
	
	ADMUX = 0x40;  //Set AVCC reference, Right adjust
	ADCSRA = 0x00; //ADC Disabled, Single conversion, no IT
	ADCSRA |= (uint8_t)hwCfg->adc_clk;
	ADCSRB = 0x00; //no trigger input
	
	ADCSRA |= 0x80;  //Enable ADC
	
	//DCCD and LED PWM configuration
	PRR0 &= ~0x80;  //Enable Timer1 power
	TCCR1A = 0xC2; //Connect OC1A, inverted mode
	if(hwCfg->od_common_is_pwm) TCCR1A |= 0x30; //Connect OC1B, inverted mode
	TCCR1B = 0x18; //PWM, Phase & Frequency Correct ICR1 top, no clock, WGM:0xE
	TCCR1C = 0x00;
	TCNT1 = 0x0000;
	OCR1A = 0xFFFF;
	OCR1B = 0xFFFF;
	ICR1 = hwCfg->pwm_top;
	TIMSK1 = 0x00;  //No interrupts
	TIFR1 = 0x00;	//Clear all flags
	
	uint8_t tim1_prescaler = (uint8_t)hwCfg->pwm_clk;
	TCCR1B |= tim1_prescaler;  //Enable timer
}

void mcu::rtc_set_calibration(uint16_t coef)
{
	rtc_ms = coef;
}

// GPIO interface functions
uint8_t mcu::gpio_read(uint8_t ch)
{
	switch(ch)
	{
		case GPIO_DIN1:  // Mode DIN1
		return gpio_read_level(PINC,0x20);
		
		case GPIO_DIN2:  // Pot DIN2
		return gpio_read_level(PINC,0x10);
		
		case GPIO_DIN3:  // Down DIN3
		return gpio_read_level(PINE,0x02);
		
		case GPIO_DIN4:  // Up DIN4
		return gpio_read_level(PINE,0x08);
		
		case GPIO_HVDIN1:  // Dimm DIN5
		return gpio_read_level(PIND,0x80);
		
		case GPIO_HVDIN2:  // Brakes DIN6
		return gpio_read_level(PINB,0x80);
		
		case GPIO_HVDIN3:  // Handbrake DIN7
		return gpio_read_level(PINB,0x40);
		
		case GPIO_HVDIN3_PULL:  // Handbrake pull DIN8
		return gpio_read_level(PINB,0x20);
		
		case GPIO_OD1:  // LED 0
		return gpio_read_level(PIND,0x01);
		
		case GPIO_OD2:  // LED 1
		return gpio_read_level(PIND,0x02);
		
		case GPIO_OD3:  // LED 2
		return gpio_read_level(PIND,0x04);
		
		case GPIO_OD4:  // LED 3
		return gpio_read_level(PIND,0x08);
		
		case GPIO_OD5:  // LED 4
		return gpio_read_level(PIND,0x10);
		
		case GPIO_OD6:  // LED 5
		return gpio_read_level(PIND,0x20);
		
		case GPIO_OUT_LOW:  // DCCD Enable
		return gpio_read_level(PINB,0x01);	
		
		case GPIO_OUT_HIGH:  // DCCD PWM
		return gpio_read_level(PINB,0x02);
		
		case GPIO_OD_PWM:  // LED PWM
		return gpio_read_level(PINB,0x04);
		
		case GPIO_FREQ1:  // Speed 1
		return gpio_read_level(PINE,0x04);
		
		case GPIO_FREQ2:  // Speed 2
		return gpio_read_level(PINE,0x01);
		
		case GPIO_FREQ_PULL:  // Speed-pull
		return gpio_read_level(PIND,0x40);
		
		case GPIO_TX:  // 
		return gpio_read_level(PINB,0x08);
		
		case GPIO_RX:  // 
		return gpio_read_level(PINB,0x10);	
		
		default:
		return 0;
	}
}

void mcu::gpio_write(uint8_t ch, int8_t lvl)
{
	switch(ch)
	{		
		case GPIO_DIN1:  // Mode DIN1
		if(lvl>0)
		{
			PORTC |= 0x20;
			DDRC |= 0x20;
		}
		else if(lvl<0)
		{
			DDRC &= ~0x20;
			PORTC &= ~0x20;
		}
		else
		{
			PORTC &= ~0x20;
			DDRC |= 0x20;
		}
		return;
		
		case GPIO_DIN2:  // Pot DIN2
		if(lvl>0)
		{
			PORTC |= 0x10;
			DDRC |= 0x10;
		}
		else if(lvl<0)
		{
			DDRC &= ~0x10;
			PORTC &= ~0x10;
		}
		else
		{
			PORTC &= ~0x10;
			DDRC |= 0x10;
		}
		return;
		
		case GPIO_DIN3:  // Down DIN3
		if(lvl>0)
		{
			PORTE |= 0x02;
			DDRE |= 0x02;
		}
		else if(lvl<0)
		{
			DDRE &= ~0x02;
			PORTE &= ~0x02;
		}
		else
		{
			PORTE &= ~0x02;
			DDRE |= 0x02;
		}
		return;
		
		case GPIO_DIN4:  // Up DIN4
		if(lvl>0)
		{
			PORTE |= 0x08;
			DDRE |= 0x08;
		}
		else if(lvl<0)
		{
			DDRE &= ~0x08;
			PORTE &= ~0x08;
		}
		else
		{
			PORTE &= ~0x08;
			DDRE |= 0x08;
		}
		return;
		
		case GPIO_HVDIN3_PULL:  // Handbrake pull DIN
		if(lvl>0)
		{
			PORTB |= 0x20;
			DDRB |= 0x20;
		}
		else if(lvl<0)
		{
			DDRB &= ~0x20;
			PORTB &= ~0x20;
		}
		else
		{
			PORTB &= ~0x20;
			DDRB |= 0x20;
		}
		return;
		
		case GPIO_OD1:  // LED 0
		if(lvl>0) PORTD |= 0x01;
		else PORTD &= ~0x01;
		return;
		
		case GPIO_OD2:  // LED 1
		if(lvl>0) PORTD |= 0x02;
		else PORTD &= ~0x02;
		return;
		
		case GPIO_OD3:  // LED 2
		if(lvl>0) PORTD |= 0x04;
		else PORTD &= ~0x04;
		return;
		
		case GPIO_OD4:  // LED 3
		if(lvl>0) PORTD |= 0x08;
		else PORTD &= ~0x08;
		return;
		
		case GPIO_OD5:  // LED 4
		if(lvl>0) PORTD |= 0x10;
		else PORTD &= ~0x10;
		return;
		
		case GPIO_OD6:  // LED 5
		if(lvl>0) PORTD |= 0x20;
		else PORTD &= ~0x20;
		return;
		
		case GPIO_OUT_LOW:  // DCCD Enable
		if(lvl>0) PORTB |= 0x01;
		else PORTB &= ~0x01;
		return;
		
		case GPIO_FREQ_PULL:  // Speed-pull
		if(lvl>0) PORTD |= 0x40;
		else PORTD &= ~0x40;
		return;
		
		default:
		return;
	}
}

void mcu::gpio_write_pull(uint8_t ch, int8_t lvl)
{
	switch(ch)
	{
		case GPIO_DIN1:  // Mode DIN1
		if(lvl>0) PORTC |= 0x20;
		else PORTC &= ~0x20;
		return;
		
		case GPIO_DIN2:  // Pot DIN2
		if(lvl>0) PORTC |= 0x10;
		else PORTC &= ~0x10;
		return;
		
		case GPIO_DIN3:  // Down DIN3
		if(lvl>0) PORTE |= 0x02;
		else PORTE &= ~0x02;
		return;
		
		case GPIO_DIN4:  // Up DIN4
		if(lvl>0) PORTE |= 0x08;
		else PORTE &= ~0x08;
		return;
		
		case GPIO_HVDIN1:  // Dimm
		if(lvl>0) PORTD |= 0x80;
		else PORTD &= ~0x80;
		return;
		
		case GPIO_HVDIN2:  // Brakes
		if(lvl>0) PORTB |= 0x80;
		else PORTB &= ~0x80;
		return;
		
		case GPIO_HVDIN3:  // Handbrake
		if(lvl>0) PORTB |= 0x40;
		else PORTB &= ~0x40;
		return;
		
		default:
		return;
	}
}

// ADC interface functions
void mcu::adc_start(uint8_t ch)
{
	// check if already running
	if(ADCSRA&0x40) return;
	
	//check if ADC is enabled
	if(!(ADCSRA&0x80)) return;

	//Safe guard mux
	if(ch > 15) return;
	// Not available channels
	if((ch > 8) && (ch<14)) return;
	
	ADMUX &= ~0x0F;
	ADMUX |= ch;
	ADCSRA |= 0x10; // Reset int. flag
	ADCSRA |= 0x40;
}


uint8_t mcu::adc_is_running(void)
{
	if(ADCSRA&0x40) return 1;
	else return 0;
}

uint8_t mcu::adc_is_new(void)
{
	if(ADCSRA&0x10) return 1;
	else return 0;
}

uint16_t mcu::adc_read(void)
{
	ADCSRA |= 0x10; // Reset int. flag
	return ADC;
}

uint16_t mcu::adc_read(uint8_t ch)
{
	//check if ADC is enabled
	if(!(ADCSRA&0x80)) return 0xFFFF;

	//Safe guard mux
	if(ch > 15) return 0xFFFF;
	// Not available channels
	if((ch > 8) && (ch<14)) return 0xFFFF;
	
	ADMUX &= ~0x0F;
	ADMUX |= ch;
	ADCSRA |= 0x40;
	while(ADCSRA&0x40); //wait to finish
	return ADC;
}

// PWM interface functions
void mcu::pwm_write(uint8_t ch, uint16_t dc)
{
	dc = 0xFFFF - dc;
	
	// Calculate value as % of TOP
	uint32_t top = (uint32_t)ICR1;
	uint32_t temp = (uint32_t)dc * top;
	temp = temp/0x0000FFFF;
	
	//Limit temp
	if(temp>0x0000FFFF) temp = 0x0000FFFF;
	uint16_t ocrx = (uint16_t)temp;
	
	// Write register
	pwm_write_ocx(ch, ocrx);
}

uint16_t mcu::pwm_read(uint8_t ch)
{
	uint16_t ocrx = pwm_read_ocx(ch);
	
	// Check easy answers
	if(ocrx == 0) return 0;
	if(ocrx >= ICR1) return 0xFFFF;
	
	// Calculate
	uint32_t top = (uint32_t)ICR1;
	uint32_t temp = (uint32_t)ocrx * 0xFFFF;
	temp = temp/top;
	
	//Limit temp
	if(temp>0x0000FFFF) return 0xFFFF;
	return (uint16_t)temp;
}

// EEPROM interface functions
uint8_t mcu::eeprom_read8b(uint16_t address)
{
	return eeprom_read_byte((uint8_t*)address);
}

uint16_t mcu::eeprom_read16b(uint16_t address)
{
	return eeprom_read_word((uint16_t*)address);
}

uint32_t mcu::eeprom_read32b(uint16_t address)
{
	return eeprom_read_dword((uint32_t*)address);
}

void mcu::eeprom_write8b(uint16_t address, uint8_t value)
{
	eeprom_write_byte((uint8_t*)address, value);
}

void mcu::eeprom_write16b(uint16_t address, uint16_t value)
{
	eeprom_write_word((uint16_t*)address, value);
}

void mcu::eeprom_write32b(uint16_t address, uint32_t value)
{
	eeprom_write_dword((uint32_t*)address, value);
}

/**** Private function definitions ****/
static uint8_t gpio_read_level(uint8_t pin_reg, uint8_t mask)
{
	if(pin_reg&mask) return LEVEL_HIGH;
	else return LEVEL_LOW;
}

static void pwm_write_ocx(uint8_t ch, uint16_t value)
{
	switch(ch)
	{
		case PWM_OUT:
		OCR1A = value;
		return;
		
		case PWM_OD:
		OCR1B = value;
		return;
		
		default:
		return;
	}
}

static uint16_t pwm_read_ocx(uint8_t ch)
{
	switch(ch)
	{
		case PWM_OUT:
		return OCR1A;
		
		case PWM_OD:
		return OCR1B ;
		
		default:
		return 0x0000;
	}
}
