#ifdef __AVR_ATmega1280__
/*
	APM2_RC.cpp - Radio Control Library for Ardupilot Mega. Arduino
	Code by Jordi Muņoz and Jose Julio. DIYDrones.com

	This library is free software; you can redistribute it and / or
		modify it under the terms of the GNU Lesser General Public
		License as published by the Free Software Foundation; either
		version 2.1 of the License, or (at your option) any later version.

	RC Input : PPM signal on IC4 pin
	RC Output : 11 Servo outputs (standard 20ms frame)

	Methods:
		Init() : Initialization of interrupts an Timers
		OutpuCh(ch, pwm) : Output value to servos (range : 900 - 2100us) ch = 0..10
		InputCh(ch) : Read a channel input value.	ch = 0..7
		GetState() : Returns the state of the input. 1 => New radio frame to process
								 Automatically resets when we call InputCh to read channels
		
*/

#include "APM2_RC.h"

#define REVERSE 3050

// Variable definition for Input Capture interrupt
volatile uint16_t 	ICR4_old;
volatile uint8_t 	PPM_Counter = 0;
volatile uint16_t 	raw[8] = {1200, 1200, 1200, 1200, 1200, 1200, 1200, 1200};

// Constructors ////////////////////////////////////////////////////////////////
APM2_RC::APM2_RC()
{
	_direction_mask = 255;	// move to super class

}

void 
APM2_RC::init()
{
	// Init PWM Timer 1
	pinMode(11, OUTPUT); //		 (PB5 / OC1A)
	pinMode(12, OUTPUT); // OUT2 (PB6 / OC1B)
	pinMode(13, OUTPUT); // OUT3 (PB7 / OC1C)

	// Timer 3
	pinMode(2, OUTPUT); // OUT7 (PE4 / OC3B)
	pinMode(3, OUTPUT); // OUT6 (PE5 / OC3C)
	pinMode(4, OUTPUT); //		 (PE3 / OC3A)

	// Timer 5
	pinMode(44, OUTPUT); // OUT1 (PL5 / OC5C)
	pinMode(45, OUTPUT); // OUT0 (PL4 / OC5B)
	pinMode(46, OUTPUT); //		 (PL3 / OC5A)

	// Init PPM input and PWM Timer 4
	pinMode(49, INPUT);	// ICP4 pin (PL0) (PPM input)
	pinMode(7, OUTPUT);	 // OUT5 (PH4 / OC4B)
	pinMode(8, OUTPUT);	 // OUT4 (PH5 / OC4C)

	//Remember the registers not declared here remains zero by default... 
	TCCR1A =((1 << WGM11) | (1 << COM1A1) | (1 << COM1B1) | (1 << COM1C1)); // Please read page 131 of DataSheet, we are changing the registers settings of WGM11, COM1B1, COM1A1 to 1 thats all... 
	TCCR1B = (1 << WGM13) | (1 << WGM12) | (1 << CS11); // Prescaler set to 8, that give us a resolution of 0.5us, read page 134 of data sheet
	OCR1A = 3000; // PB5, none
	//OCR1B = 3000; // PB6, OUT2
	//OCR1C = 3000; // PB7 OUT3
	ICR1 = 40000; // 50hz freq...Datasheet says	(system_freq / prescaler) / target frequency. So (16000000hz / 8) / 50hz = 40000,

	// Init PWM Timer 3
	TCCR3A =((1 << WGM31) | (1 << COM3A1) | (1 << COM3B1) | (1 << COM3C1));
	TCCR3B = (1 << WGM33) | (1 << WGM32) | (1 << CS31); 
	OCR3A = 3000; // PE3, NONE
	//OCR3B = 3000; // PE4, OUT7
	//OCR3C = 3000; // PE5, OUT6
	ICR3 = 40000; // 50hz freq

	
	// Init PWM Timer 5
	TCCR5A =((1 << WGM51) | (1 << COM5A1) | (1 << COM5B1) | (1 << COM5C1)); 
	TCCR5B = (1 << WGM53) | (1 << WGM52) | (1 << CS51);
	OCR5A = 3000; // PL3, 
	//OCR5B = 3000; // PL4, OUT0
	//OCR5C = 3000; // PL5, OUT1
	ICR5 = 40000; // 50hz freq

			
	// Init PPM input and PWM Timer 4
	TCCR4A = ((1 << WGM40) | (1 << WGM41) | (1 << COM4C1) | (1 << COM4B1) | (1 << COM4A1));	
	TCCR4B = ((1 << WGM43) | (1 << WGM42) | (1 << CS41) | (1 << ICES4));
	OCR4A = 40000; // /50hz freq.
	//OCR4B = 3000; // PH4, OUT5
	//OCR4C = 3000; // PH5, OUT4
 
	//TCCR4B |=(1<<ICES4); //Changing edge detector (rising edge). 
	//TCCR4B &=(~(1<<ICES4)); //Changing edge detector. (falling edge)
	TIMSK4 |= (1 << ICIE4); // Enable Input Capture interrupt. Timer interrupt mask

	// trim out the radio
	for(int c = 0; c < 50; c++){
		delay(20);
		read();
	}
	
	trim();
	
	for(int y = 0; y < 8; y++) { 
		set_ch_pwm(y, radio_trim[y]);
	}
}

void APM2_RC::read()
{
	//Serial.print("ch1 in ");
	//Serial.print(raw[CH1],DEC);
	
	// reverse any incoming PWM if needed
	for(int y = 0; y < 8; y++) { 
		if((_direction_mask & (1 << y)) == 0)
			radio_in[y] = REVERSE - raw[y];
		else
			radio_in[y] = raw[y];
	}

	//Serial.print("\tch1 in ");
	//Serial.print(radio_in[CH1],DEC);

	if(_mix_mode == 1){
		// elevons
		int16_t ailerons = (float)(radio_in[CH1] - radio_trim[CH1]);
		int16_t elevator = (float)(radio_in[CH2] - radio_trim[CH2]) * .7;

		//Serial.print("\tailerons ");
		//Serial.print(ailerons,DEC);

		//Serial.print("\tradio_trim ");
		//Serial.print(radio_trim[CH1],DEC);

		radio_in[CH1] = (elevator - ailerons); // left
		radio_in[CH2] = (elevator + ailerons); // right		

		radio_in[CH1] += radio_trim[CH1];
		radio_in[CH2] += radio_trim[CH2];
		
		//Serial.print("\tch1 in ");
		//Serial.print(radio_in[CH1],DEC);

		//Serial.print("\tch1 trim ");
		//Serial.print(radio_trim[CH1],DEC);

		//Serial.print("radio_in[CH1] ");
		//Serial.print(radio_in[CH1],DEC);
		//Serial.print(" \tradio_in[CH2] ");
		//Serial.println(radio_in[CH2],DEC);
	}

	// output servos
	for (uint8_t i = 0; i < 8; i++){
		if (i == 3) continue;
		if(radio_in[i] >= radio_trim[i])
			servo_in[i] = (float)(radio_in[i] - radio_min[i]) / (float)(radio_max[i] - radio_min[i]) * 100.0;
		else
			servo_in[i] = (float)(radio_in[i] - radio_trim[i]) / (float)(radio_trim[i] - radio_min[i]) * 100.0;
	}
	servo_in[CH3] = (float)(radio_in[CH3] - radio_min[CH3]) / (float)(radio_max[CH3] - radio_min[CH3]) * 100.0;
	servo_in[CH3] = constrain(servo_out[CH3], 0, 100);	
}

void
APM2_RC::output()
{
	uint16_t out;
	for (uint8_t i = 0; i < 8; i++){
		if (i == 3) continue;
		if(radio_in[i] >= radio_trim[i])
			out = ((servo_in[i] * (radio_max[i] - radio_trim[i])) / 100) + radio_trim[i];
		else
			out = ((servo_in[i] * (radio_max[i] - radio_trim[i])) / 100) + radio_trim[i];
		set_ch_pwm(i, out);
	}

	out = (servo_out[CH3] * (float)(radio_max[CH3] - radio_min[CH3])) / 100.0;
	out += radio_min[CH3];
	set_ch_pwm(CH3, out);
}

void 
APM2_RC::trim()
{
	uint8_t temp = _mix_mode;
	_mix_mode = 0;
	read();
	_mix_mode = temp;
	
	// Store the trim values
	// ---------------------
	for (int y = 0; y < 8; y++) 
		radio_trim[y] = radio_in[y];
}
void
APM2_RC::twitch_servos(uint8_t times)
{
	// todo
}
void
APM2_RC::set_ch_pwm(uint8_t ch, uint16_t pwm)
{	
	//pwm = constrain(pwm, MIN_PULSEWIDTH, MAX_PULSEWIDTH);
	
	switch(ch){
		case 0:
			//Serial.print("\tpwm out ");
			//Serial.print(pwm,DEC);

			if((_direction_mask & 1) == 0 )
				pwm = REVERSE - pwm;

			//Serial.print("\tpwm out ");
			//Serial.println(pwm,DEC);
			
			OCR5B = pwm << 1;
			break;	// ch0
			
		case 1:
			if((_direction_mask & 2) == 0 )
				pwm = REVERSE - pwm;
			OCR5C = pwm << 1;
			break;	// ch0
			
		case 2:
			if((_direction_mask & 4) == 0 )
				pwm = REVERSE - pwm;
			OCR1B = pwm << 1;
			break;	// ch0
			
		case 3:
			if((_direction_mask & 8) == 0 )
				pwm = REVERSE - pwm;
			OCR1C = pwm << 1;
			break;	// ch0
			
		case 4:
			if((_direction_mask & 16) == 0 )
				pwm = REVERSE - pwm;
			OCR4C = pwm << 1;
			break;	// ch0
			
		case 5:
			if((_direction_mask & 32) == 0 )
				pwm = REVERSE - pwm;
			OCR4B = pwm << 1;
			break;	// ch0
			
		case 6:
			if((_direction_mask & 64) == 0 )
				pwm = REVERSE - pwm;
			OCR3C = pwm << 1;
			break;	// ch0
			
		case 7:
			if((_direction_mask & 128) == 0 )
				pwm = REVERSE - pwm;
			OCR3B = pwm << 1;
			break;	// ch0

		case 8:
			OCR5A = pwm << 1;
			break;	// ch0

		case 9:
			OCR1A = pwm << 1;
			break;	// ch0

		case 10:
			OCR3A = pwm << 1;
			break;	// ch0
	} 
}

/****************************************************
	 Input Capture Interrupt ICP4 => PPM signal read
 ****************************************************/
ISR(TIMER4_CAPT_vect)	
{
	uint16_t pulse;
	uint16_t pulse_width;
	
	pulse = ICR4;
	if (pulse < ICR4_old){			 				// Take care of the overflow of Timer4 (TOP = 40000)
		pulse_width = (pulse + 40000) - ICR4_old;	// Calculating pulse 
	}else{
		pulse_width = pulse - ICR4_old;				// Calculating pulse 
	}
	ICR4_old = pulse;
	
	
	if (pulse_width > 8000){  						// SYNC pulse
		PPM_Counter = 0;
	} else {
		//PPM_Counter &= 0x07;						// For safety only (limit PPM_Counter to 7)
		raw[PPM_Counter++] = pulse_width >> 1;		// Saving pulse. 
	}
}



// InstantPWM implementation
// This function forces the PWM output (reset PWM) on Out0 and Out1 (Timer5). For quadcopters use
void APM2_RC::force_out_0_1(void)
{
	if (TCNT5 > 5000)	 // We take care that there are not a pulse in the output
		TCNT5 = 39990;	 // This forces the PWM output to reset in 5us (10 counts of 0.5us). The counter resets at 40000
}
// This function forces the PWM output (reset PWM) on Out2 and Out3 (Timer1). For quadcopters use
void APM2_RC::force_out_2_3(void)
{
	if (TCNT1 > 5000)
		TCNT1 = 39990;
}

// This function forces the PWM output (reset PWM) on Out6 and Out7 (Timer3). For quadcopters use
void APM2_RC::force_out_6_7(void)
{
	if (TCNT3 > 5000)
		TCNT3 = 39990;
}
#endif