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

/**** Private variables ****/
static uint8_t tim0_prescaler = 0x00;
static uint8_t tim1_prescaler = 0x00;
static uint8_t tim3_prescaler = 0x00;
static uint8_t tim4_prescaler = 0x00;

/**** Private function declarations ****/
static void PWM_SetOCx(pwmCh_t ch, uint16_t value);

/**** Public function definitions ****/
void HAL_Init_Min(hwConfig_t* hwCfg)
{
	// PIN Configuration
	//DCCD Enable and PWM
	PORTB &= ~0x03; //Set low
	DDRB |= 0x03;  //Set as outputs
	
	//LED PWM
	PORTB &= ~0x04;  //Set low
	DDRB |= 0x04;   //Set as output
	
	//UART TX
	PORTB |= 0x18;  //Set high / pull-up on
	DDRB |= 0x08;  //Set as output
	DDRB &= ~0x10;  //Set as input
	
	//Handbrake pull-up
	PORTB |= 0x20;  //Set high
	DDRB |= 0x20;   //Set as output
	
	//Handbrake and Brake inputs
	PORTB |= 0xC0;  //Pull-up on
	DDRB &= ~0xC0;  //Set as input
	
	// ADC inputs
	PORTC &= ~0x3F; //Pull-up off
	DDRC &= ~0x3F;  //Set as inputs
	
	// Reset
	PORTC |= 0x40; //Pull-up on
	DDRC &= ~0x40;  //Set as input
	
	// LED control
	PORTD &= ~0x3F;  //Set low
	DDRD |= 0x3F;   //Set as outputs
	
	// Speed pull
	PORTD &= ~0x40;  //Set low
	DDRD |= 0x40;   //Set as outputs
	
	// Dim input
	PORTD |= 0x80;  //Set pull-up on
	DDRD &= ~0x80;   //Set as input
	
	// Speed inputs
	PORTE &= ~0x05;  //Set pull-down
	DDRE |= 0x05;   //Set as output
	
	// Up/Down inputs
	PORTE |= 0x0A;  //Set pull-up on
	DDRE &= ~0x0A;   //Set as input
	
	
	//ADC configuration
	PRR0 &= ~0x01; //Enable ADC power
	DIDR0 |= 0x1F; //Disable digital inputs, ADC0-ADC4
		
	ADMUX = 0x40;  //Set AVCC reference, Right adjust
	ADCSRA = 0x00; //ADC Disabled, Single conversion, no IT
	ADCSRA |= (uint8_t)hwCfg->adc_clk_prescaler;
	ADCSRB = 0x00; //no trigger input
	
	if(hwCfg->adc_auto_wake) ADCSRA |= 0x80;  //Enable ADC
	else PRR0 |= 0x01;


	//DCCD and LED PWM configuration
	PRR0 &= ~0x80;  //Enable Timer1 power
	TCCR1A = 0xF2; //Connect OC1A and OC1B, normal logic
	TCCR1B = 0x18; //PWM, Phase & Frequency Correct ICR1 top, no clock, WGM:0xE
	TCCR1C = 0x00;
	TCNT1 = 0x0000;
	OCR1A = 0x0000;
	OCR1B = 0x0000;
	ICR1 = (hwCfg->pwm_timer_top);
	TIMSK1 = 0x00;  //No interrupts
	TIFR1 = 0x00;	//Clear all flags
	
	tim1_prescaler = (uint8_t)hwCfg->pwm_timer_prescaler;
	TCCR1B |= tim1_prescaler;  //Enable timer
}

void HAL_Init_Extra(hwConfig_t* hwCfg)
{
	//Speed 1 input timer configuration
	PRR1 &= ~0x01;  //Enable Timer3 power
	TCCR3A = 0x00;  //OCx disconnected, WGM:0x0
	TCCR3B = 0x80;  //ICP Noise filter, Falling edge, no clock
	TCCR3C = 0x00;
	TCNT3 = 0x0000;
	OCR3A = 0x0000;
	OCR3B = 0x0000;
	ICR3 = 0x0000;
	TIMSK3 = 0x00;
	//TIMSK3 |= 0x21; //ICP and OVF interrupts
	TIFR3 = 0x00;	//Clear all flags
	
	tim3_prescaler = (uint8_t)hwCfg->freq_timer_prescaler;
	TCCR3B |=  tim3_prescaler; //Enable timer
	
	
	//Speed 0 input timer configuration
	PRR1 &= ~0x08;  //Enable Timer4 power
	TCCR4A = 0x00;  //OCx disconnected, WGM:0x0
	TCCR4B = 0x80;  //ICP Noise filter, Falling edge, no clock
	TCCR4C = 0x00;
	TCNT4 = 0x0000;
	OCR4A = 0x0000;
	OCR4B = 0x0000;
	ICR4 = 0x0000;
	TIMSK4 = 0x00;
	//TIMSK4 |= 0x21;	//ICP and OVF interrupts
	TIFR4 = 0x00;	//Clear all flags
	
	tim4_prescaler = (uint8_t)hwCfg->freq_timer_prescaler;
	TCCR4B |= tim4_prescaler;  //Enable timer
	
	
	//UART1 configuration
	PRR0 &= 0x10;   //Enable UART1 power
	UCSR1A = 0x00;  //Clear flags, Single UART speed, Single processor mode
	UCSR1B = 0x18;  //Enable RX/TX hardware, 8bit char
	//UCSR1B |= 0xC0; //Enable RX/TX interrupt,
	UCSR1C = 0x06; ; //async, No parity, 1 stop bit, 8bit char,
	UBRR1 = hwCfg->uart_prescaler; //UART baud rate select
	
	
	//"Systick" timer configuration
	PRR0 &= ~0x20; //Enable Timer0 power
	TCCR0A = 0x02 ;//OC0x not connected, WGM 0x01-CTC OC0A TOP
	TCCR0B = 0x00; //WGM 0x01-CTC, No clock
	TIMSK0 = 0x00;
	//TIMSK0 |= 0x01; //OVF interrupt enabled
	TCNT0 = 0x00;
	OCR0A = hwCfg->systick_timer_top;
	OCR0B= 0x00;
	TIFR0 = 0x00; //Reset all flags
	
	tim0_prescaler = (uint8_t)hwCfg->systick_timer_prescaler;
	TCCR0B |= tim0_prescaler;
	
	
	//Disabled not used power configuration
	if(hwCfg->disable_unused)
	{
		//Disable power to not used peripherals
		PRR0 |= 0xC6; //Disable TWI0, TIM2, SPI0, UART0
		PRR1 |= 0x34; //Disable TWI1, PRTC, SPI1
	}
	
	//Watchdog configuration
	if(hwCfg->en_watchdog)
	{
		//watchdog timer setup
		WDTCSR |= 0x10; //Change enable
		WDTCSR |= 0x0D; //System reset mode, 0.5s period.
		//use special instruction to reset watchdog timer
	};
}


// Handbrake input
level_t HAL_ReadLvl_Handbrake(void)
{
	if(PINB & 0x40) return HIGH;
	else return LOW;
}

// Brakes input
level_t HAL_ReadLvl_Brake(void)
{
	if(PINB & 0x80) return HIGH;
	else return LOW;
}

// Dimm input
level_t HAL_ReadLvl_Dimm(void)
{
	if(PIND & 0x80) return HIGH;
	else return LOW;
}

// UP button
level_t HAL_ReadLvl_BtnUp(void)
{
	if(PINE & 0x08) return HIGH;
	else return LOW;
}

// Down button
level_t HAL_ReadLvl_BtnDown(void)
{
	if(PINE & 0x02) return HIGH;
	else return LOW;
}

// Mode button
level_t HAL_ReadLvl_BtnMode(void)
{
	if(PINC & 0x20) return HIGH;
	else return LOW;
}

// Handbrake pull
level_t HAL_ReadLvl_HandbrakePull(void)
{
	if(PINB & 0x20) return HIGH;
	else return LOW;
}

// Coil driver control low
level_t HAL_ReadLvl_CoilLow(void)
{
	if(PINB & 0x01) return HIGH;
	else return LOW;
}

// Coil driver control high
level_t HAL_ReadLvl_CoilHigh(void)
{
	if(PINB & 0x02) return HIGH;
	else return LOW;
}

// LED PWM control
level_t HAL_ReadLvl_LedsPwm(void)
{
	if(PINB & 0x04) return HIGH;
	else return LOW;
}

// Speed 0 input pin
level_t HAL_ReadLvl_Speed0(void)
{
	if(PINE & 0x04) return HIGH;
	else return LOW;
}

// Speed 1 input pin
level_t HAL_ReadLvl_Speed1(void)
{
	if(PINE & 0x01) return HIGH;
	else return LOW;
}

// Speed common pull pin
level_t HAL_ReadLvl_SpeedPull(void)
{
	if(PIND & 0x40) return HIGH;
	else return LOW;
}


// Set handbrake pull-up
void HAL_SetPull_Handbrake(level_t lvl)
{
	switch(lvl)
	{
		case HIGH:
		PORTB |= 0x20;  //Set high
		DDRB |= 0x20;   //Set as output
		break;
		
		case LOW:
		PORTB &= ~0x20;  //Set low
		DDRB |= 0x20;   //Set as output
		
		default:
		DDRB &= ~0x20;   //Set as input
		PORTB |= 0x20;  //Set high
		break;
	}
}

// Set speed inputs common pull
void HAL_SetPull_Speed(level_t lvl)
{
	switch(lvl)
	{
		case HIGH:
		PORTD |= 0x40;  //Set high
		break;
		
		default:
		PORTD &= ~0x40;  //Set low
		break;
	}
}


// ADC Wake
void HAL_ADC_Wake(void)
{
	//Enable ADC power
	PRR0 &= ~0x01;
	//Enable ADC
	ADCSRA |= 0x80;
}

// ADC Sleep
void HAL_ADC_Sleep(void)
{
	//wait to finish
	while(ADCSRA&0x40);
	//Disable ADC
	ADCSRA &= ~0x80;
	//Disable ADC power
	PRR0 |= 0x01;
}

// ADC Read
uint16_t HAL_ADC_Read(adcCh_t ch)
{
	//check if ADC is enabled
	if(!(ADCSRA&0x80)) return 0xFFFF;
	
	uint8_t mux = (uint8_t)ch;
	//Safe guard mux
	if(mux > 15) return 0xFFFF;
	// Not available channels
	if((mux > 8) && (mux<14)) return 0xFFFF;
	
	ADMUX &= ~0x0F;
	ADMUX |= mux;
	ADCSRA |= 0x40;
	while(ADCSRA&0x40); //wait to finish
	return ADC;
}


// Coil Driver Low Side control
void HAL_Coil_SetLowSide(uint8_t on)
{
	if(on) PORTB |= 0x01;
	else PORTB &= ~0x01;
}

void HAL_Coil_SetPWM(uint8_t percent)
{
	HAL_PWM_SetDuty100(PWM_COIL, percent);
}

void HAL_Coil_SetPWM16b(uint16_t value)
{
	HAL_PWM_SetDuty16b(PWM_COIL, value);
}


// LED Display
void HAL_LEDS_Set(uint8_t image)
{
	//Read current PORTD pin6, pin7
	uint8_t keep = PORTD & 0xC0;
	
	//Safe guard display
	image &= 0x3F;
	
	//Calculate new PORTD
	keep |= image;
	
	//Set PORTD
	PORTD = keep;
}

uint8_t HAL_LEDS_Get(void)
{
	return (PIND & 0x3F);
}

void HAL_LEDS_SetPWM(uint8_t percent)
{
	HAL_PWM_SetDuty100(PWM_LED, percent);
}


// PWM Direct functions
void HAL_PWM_Wake(void)
{
	//Enable Timer1 power
	PRR0 &= ~0x80;
	//Prepare Timer1 settings
	TCNT1 = 0x0000;
	OCR1A = 0x0000;
	OCR1B = 0x0000;
	//Enable clock
	TCCR1B |= tim1_prescaler;  //Enable timer
}

void HAL_PWM_Sleep(void)
{
	// Turn off outputs
	OCR1A = 0x0000;
	OCR1B = 0x0000;
	// Force timer to bottom
	TCNT1 = (ICR1-1);
	// Wait for outputs to be off
	while((PINB&0x06)!=0x00) continue;
	// Disable clock
	TCCR1B &= ~0x07;
	// Disable Timer1 power
	PRR0 |= 0x80;
}

void HAL_PWM_SetDuty16b(pwmCh_t ch, uint16_t value)
{
	value = 0xFFFF - value;
	
	uint32_t top = (uint32_t)ICR1;
	uint32_t temp = (uint32_t)value * top;
	temp = temp/0x0000FFFF;
	//Limit temp
	if(temp>0x0000FFFF) temp = 0x0000FFFF;
	uint16_t ocrx = (uint16_t) temp;
	
	PWM_SetOCx(ch, ocrx);
}

void HAL_PWM_SetDuty100(pwmCh_t ch, uint8_t percent)
{
	if(percent > 100) percent = 100;
	percent = 100 - percent;
	
	uint32_t top = (uint32_t)ICR1;
	uint32_t temp = (uint32_t)percent * top;
	temp = temp/100;
	//Limit temp
	if(temp>0x0000FFFF) temp = 0x0000FFFF;
	uint16_t ocrx = (uint16_t) temp;
	
	PWM_SetOCx(ch, ocrx);
}

/**** Private function definitions ****/
static void PWM_SetOCx(pwmCh_t ch, uint16_t value)
{
	switch(ch)
	{
		case PWM_COIL:
		OCR1A = value;
		return;
		
		case PWM_LED:
		OCR1B = value;
		return;
		
		default:
		return;
	}
}