/*! \file 
*	\details ATMEGA16 peripherals and subsystems initialization library.
*
*	\ingroup WheelchairControl
*	@{
*
*/

//includes
#include "MiscInits.h"

//functions
void TIMER0_Init(void)	//initializes TIMER0 (period 1 ms, used to clear UART Rx index every 4ms)
{
	//8-bit timer, interrupt flag is set on everflow
	//overflow interrupt period: 16 MHz / 64(prescaler) / 256 (8-bit resolution) = 1 ms

	TCNT0 = 0x00;	//reset timer

	OCR0 = 0x00;	//reset OCR0 (not necessary)

	TCCR0 = 0x03;	//normal mode, prescaler F_CPU/64

	TIMSK |= (1<<TOIE0);	//enable TIMER0 overflow interrupt
	TIMSK &= ~(1<<OCIE0);	//disable TIMER0 output compare match interrupt
}

void TIMER1_Init(void)	//initializes TIMER1 (period 20 ms, used to generate R/C signal for Sabertooth 2x25)
{
	//16-bit timer, R/C generation on OCR1X and TCNT1 match (non-inverting mode), TCNT1 top = ICR1
	//R/C signal period 20 ms, positive pulse length: 	1.5 ms: Stop motor				OCR1X = 375
	//													1 ms:	Full speed backwards	OCR1X = 250
	//													2 ms:	Full speed forward		OCR1X = 500	
	//No interrupts enabled for TIMER1 										
					
	TCNT1 = 0x00;	//reset counter/timer register

	ICR1 =	5000;	//top of the timer, 16 MHz / 64 (prescaler) / 5000 = 20 ms

	TCCR1A = 0xA2;	//output compare signal on pins OC1X (DDRD reg. for OC1X must be set to output!), fast-PWM mode (two MSB bits), top = ICR1
	TCCR1B = 0x1B;	//no noise canceller, fast-PWM mode (two LSB bits), top = ICR1, prescaler: F_CPU/64 (must be the same as presc. value of TIMER0)

	OCR1A =	375;	//1.5 ms -> motor stop (left wheel)
	OCR1B = 375;	//1.5 ms ->	motor stop	(right wheel)

	TIMSK &= ~( (1<<TICIE1) | (1<<OCIE1A) |  (1<<OCIE1B) | (1<<TOIE1) );	//disable all interrupts for TIMER1
}

void TIMER2_Init(void)	//initializes TIMER2 (period 2 ms)
{
	//8-bit timer, interrupt flag is set on everflow
	//overflow interrupt period: 16 MHz / 128(prescaler) / 256 (8-bit resolution) = 2 ms

	TCNT2 = 0x00;	//reset counter/timer register

	OCR2 = 0x00;	//reset OCR2 (not necessary)

	ASSR = 0x00;	//synchronous mode

	TCCR2 = 0x05;	//normal mode, prescaler: F_CPU/128

	TIMSK |= (1<<TOIE2);	//enable TIMER2 overflow interrupt
	TIMSK &= ~(1<<OCIE2);	//disable TIMER2 output compare match interrupt
}

void PORTS_Init(void) //initializes I/O port A - D (input/output, pull-ups etc.)
{
	//Pin state:	0 - Output, low
	//				1 - Output, high
	//				P - Input, pull-up enabled
	//				- - Input, pull-up disabled


					//PORT A:		7	6	5	4	3	2	1	0
	DDRA = 0x06;	//DIRECTION:	In	In	In	In	In	Out	Out	In
	PORTA = 0x00;	//PIN STATE:	-	-	-	-	-	0	0	-

					//PORT B:		7	6	5	4	3	2	1	0
	DDRB = 0x40;	//DIRECTION:	In	Out	In	In	In	In	In	In
	PORTB = 0x44;	//PIN STATE:	-	1	-	-	-	P	-	-

					//PORT C:		7	6	5	4	3	2	1	0
	DDRC = 0xFC;	//DIRECTION:	Out	Out	Out	Out	Out	Out In	In
	PORTC = 0x03;	//PIN STATE:	0	0	0	0	0	0	P	P

					//PORT D:		7	6	5	4	3	2	1	0
	DDRD = 0x30;	//DIRECTION:	In	In	Out	Out	In	In	In	In
	PORTD = 0x00;	//PIN STATE:	-	-	0	0	-	-	-	-
}

void AC_Disable(void)	//disables analog comparator
{
	ACSR |= (1<<ACD);		//disable comparator
	SFIOR &= ~(1<<ACME);	//disable AC multiplexer
}

void ADC_Init(void)	//initializes A/D converter
{
	ADCSRA = 0x85;		//enable A/D converter, disable interrupt on complete conversion, 500 kHz clock
	ADMUX = 0x44; 		//reference voltage on AVCC pin, measured voltage on pin ADC4
	SFIOR &= ~(0xE0);	//does not matter, ADATE bit in ADCSRA (=autorun) is set to 0
}

void EXT_INT_Init(void)	//initializes registers of external interrupts
{
	MCUCR |= ( (1 << ISC10) | (1 << ISC00) );			//ext. interrupts 0 and 1 sense: both edges
	MCUCSR &= ~(1<<ISC2);	//ext. interrupt 2 sense: falling edge

	GIFR = 0xE0; 			//reset ext. interrupt flags (write '1' to reset!)

	GICR |= 0xE0;			//enable external interrputs 0, 1, 2
}

void Init_ISRsSharedstruct(t_ISRsSharedStruct * sharedStruct)	//Initializes inter-ISRs shared structure
{
	sharedStruct->UARTRxIndex = 0;	//Index in UARTReceivedData[] array
	sharedStruct->Timer0Prescaler = 0;	//Software prescaler for TIMER0
	sharedStruct->UARTWatchdog = 0;	//UART watchdog counter
	sharedStruct->BufferOdoL = 0; sharedStruct->BufferOdoR = 0;	//Couts impulses from wheel speed sensor for calculation of odometry
	sharedStruct->BufferSpeedL = 0; sharedStruct->BufferSpeedR = 0;	//Counts impulses from wheel speed sensor for calculation of current wheel speed
	sharedStruct->MomSpeedR = SPEED_OFFSET; sharedStruct->MomSpeedL = SPEED_OFFSET;	//Momentum (speed) of the motor
	sharedStruct->EnableSpRegR = FALSE; sharedStruct->EnableSpRegL = FALSE;	//Determines whether speed regulator of the motor is enabled
	sharedStruct->EnableWallFollowing = FALSE; //Determines ehether wall-following is enabled
	sharedStruct->ActualRPML = 0; sharedStruct->ActualRPMR = 0;	//Dctual RPM of right and left wheel (impulses per 50 ms)
	sharedStruct->RegIntegratorR = 0; sharedStruct->RegIntegratorL = 0;	//Integrators value
	sharedStruct->SRF08_DistanceFront = 0; sharedStruct->SRF08_DistanceRear = 0; //Distance measured by SRF08 sensors
	sharedStruct->y0_des = 70; sharedStruct->V_des = SPEED_OFFSET; //Desired value of y0 (distance from the tracked wall), desired speed (cm/s)
	sharedStruct->Ky = 0; sharedStruct->Kfi = 0; //Position loop gain, angular speed loop gain
	sharedStruct->X = 0; sharedStruct->Y = 0; sharedStruct->Phi = 0;	//Odometry data (X, Y [mm]; Phi [mRad])
	sharedStruct->EnablePositionRegulator = FALSE;	//Determines whether position regulator is enabled
	sharedStruct->Xdes = 0; sharedStruct->Ydes = 0;	//Desired coordinates for the position regulator
	sharedStruct->TotalImpulsesR = 0; sharedStruct->TotalImpulsesR = 0;	//Total number of impulses captured by encoders
	sharedStruct->RegulateAngleOnly = 0;	//If enabled, position regulator regulates only angle
}
