//#define MAINIO_TEST

/*
Main PIC I/O
- A/D converter
- 2 PWM outputs
- 1 Servo run w/ output compare

*/

#include <stdio.h>
#include <htc.h>
#define _LEGACY_HEADERS
#include "BITDEFS.h"
#include "TimerPIC.h"
#include "MainIO.h"

#define	HI				1
#define LO				0

static unsigned int ServoHighTime;
static unsigned char LastServoState;
static unsigned char CurrADValue;
static unsigned char GoDoneSetFlag;


#ifdef MAINIO_TEST
__CONFIG(UNPROTECT & WDTDIS & PWRTEN & HS);
//__CONFIG(CP_OFF & WDTE_OFF & PWRTE_ON & FOSC_HS );
#endif


#ifdef MAINIO_TEST
// Rising edge
void interrupt IO_ISR(void) {

	if(CCP3IF)
		OutputCompareISR();

	if (TMR0IF) {
		RB3 = 1;
		TimerPIC_ISR();
		RB3 = 0;
	}
}
#endif



void InitPWM(void) {

	// Set CCP2 to RB3
	//CCPMX = 0;

	// Set PWM period = (PR2 + 1)*4*Tosc*TMR2Prescale
	// = (255+1)*4/(20e6)*4 = 0.2048ms => 4.88 kHz
	PR2 = 0xFF;

	// Set PWM initial duty cycle
	SetDuty(0,0);

	// Make CCP1 and CCP2 an output
	TRISC2 = 0;
	TRISC1 = 0;

	// Use timer 2 frequency: 19.53 kHz
	// Set TMR2 prescale to 1 and enable TMR2
	T2CKPS0 = 0;
	T2CKPS1 = 0;
	TMR2ON = 1;

	// Configure CCP1 & CCP2 for PWM
	CCP1M2 = 1;
	CCP1M3 = 1;
	CCP2M2 = 1;
	CCP2M3 = 1;
}

void InitServo(void){

	// Configure CCP3 to output compare with software interrupt
	CCP3M3 = 1;
	CCP3M2 = 0;
	CCP3M1 = 1;
	CCP3M0 = 0;

	// Clock source: fosc/4 (20MHz/4)
	TMR1CS = 0;

	// Prescale: 1:4 (timer runs at 5MHz/4 = 1250kHz)
	T1CKPS0 = 0;
	T1CKPS1 = 1;

	// Don't use timer 1 as the system clock (default)
	T1RUN = 0;

	// Don't turn on Timer1 interrupt
	TMR1IE = 0;

	// We'll be using the OC interrupt instead
	CCP3IF = 0;
	CCP3IE = 1;
	PEIE = 1;

	// Set the output line initially low
	// Make sure at least our pin (AN13) is not analog
	TRISB5 = 0;
	PCFG1 = 1;
	RB5 = 0;

	// Set initial flag to NO_FLAG
	ServoHighTime = NO_FLAG;

	// Schedule how long we wait before we start our OC
	CCPR3L = (unsigned char)1_MS_;
	CCPR3H = (1_MS_)>>8;
	LastServoState = LO;

	// Clear the timer before we time our first OC
	TMR1L = 0;
	TMR1H = 0;

	// Turn on timer1
	TMR1ON = 1;
}

void InitADC(void){
	int i;

	// Disable TRISB0
	TRISB0 = 0;
	// Configure AN0 as Analog (See Page 155 in DataSheet)
	PCFG3 = 1;
	PCFG2 = 1;
	PCFG1 = 1;
	PCFG0 = 0;

	// Select ADC Conversion Clock (32 Tosc, shortest for 20 Mhz Clock)
	ADCS0 = 0;
	ADCS1 = 1;
	ADCS2 = 0;
	// Configure Voltage Reference (Vref connected to Vdd/Vss)
	VCFG1 = 0;
	VCFG0 = 0;
	// Select ADC Acquisition Time
	ACQT0 = 0;
	ACQT1 = 0;
	ACQT2 = 0;
	// Turn On ADC Module
	ADON = 1;

	// Select ADC Input Channel (AN0)
	CHS0 = 0;
	CHS1 = 0;
	CHS2 = 0;
	// Select Result Format
	ADFM = 0;  // Left Justified

	// Wait
	for (i = 0; i< 50; i++){
	}

}

void OutputCompareISR(void) {

	static unsigned int CurrHighTime, CurrLowTime;

	// Clear flag
	CCP3IF = 0;

	// We need to set our high time
	if(LastServoState == LO) {
		CurrHighTime = ServoHighTime;
		CCPR3L = (unsigned char)CurrHighTime;
		CCPR3H = CurrHighTime>>8;
		LastServoState = HI;
		RB5 = 1;
	}

	// We need to set our low time
	else if(LastServoState == HI) {
		CurrLowTime = PERIOD - CurrHighTime;
		CCPR3L = (unsigned char)(CurrLowTime);
		CCPR3H = (CurrLowTime)>>8;
		LastServoState = LO;
		RB5 = 0;
	}
	// Clear the timer
	TMR1L = 0;
	TMR1H = 0;
}

unsigned char ReadADC(void){

	// Check if we need to get a new A/D value
	if(GoDoneSetFlag == 0) {
		GODONE = 1;
		GoDoneSetFlag = 1;
	}

	// If the new value is ready, store it
	if(GODONE == 0){
		CurrADValue = ADRESH;
		GoDoneSetFlag = 0;
	}
	return CurrADValue;
}
/*
unsigned char ReturnWaterSpeed(void) {




}*/

void SetServo(unsigned int Position) {
	ServoHighTime = Position;
}

void SetDuty(unsigned char LeftDuty, unsigned char RightDuty) {

	static unsigned int SentLeftDuty, SentRightDuty;

	if (LeftDuty >= 100)
		LeftDuty = 100;
	SentLeftDuty = (unsigned int)(LeftDuty*255)/100;

	if (RightDuty >= 100)
		RightDuty = 100;
	SentRightDuty = (unsigned int)(RightDuty*255)/100;
	CCPR1L = (unsigned char)SentLeftDuty;
	CCPR2L = (unsigned char)SentRightDuty;
}

#ifdef MAINIO_TEST

void main(void) {

	static unsigned char leftDuty = 0;
	static unsigned char rightDuty = 0;
	static unsigned char Value,Cycle = 0;
	// Turn off analog inputs
	PCFG0 = 1;
	PCFG1 = 1;
	PCFG2 = 1;
	PCFG3 = 1;
	TRISB4 = 0;
	TRISB3 = 0;
	RB4 = 0;
	RB3 = 0;

	PEIE = 1;
	GIE = 1;

	InitPWM();
	InitServo();
	TimerPIC_Init();
	TimerPIC_InitTimer(1,100);
	TimerPIC_InitTimer(0,2000);
	InitADC();

	while(1) {
		if(TimerPIC_IsTimerExpired(1) == TimerPIC_EXPIRED){
			RB4 = 1;
			TimerPIC_InitTimer(1,100);
			RB4 = 0;
			Value = ReadADC();
			//SetDuty((long) 100*Value/255, (long) 100*Value/255);
		}

		if(TimerPIC_IsTimerExpired(0) == TimerPIC_EXPIRED){
			TimerPIC_InitTimer(0,2000);
			if(Cycle == 0)
				SetServo(NO_FLAG);
			else if(Cycle == 1)
				SetServo(RED_FLAG);
			else if(Cycle == 2)
				SetServo(GREEN_FLAG);
			else if(Cycle == 3)
				SetServo(TIMEOUT_FLAG);
			Cycle = (++Cycle)%4;
		}
	}


}

#endif //MAINIO_TEST