/** Header Files ***************************************************/
#include <p18f4520.h>
#include <stdio.h>
#include <adc.h>
#include <delays.h>

/** Configuration Bits *********************************************/
#pragma config OSC = INTIO67
#pragma config WDT = OFF
#pragma config LVP = OFF
#pragma config BOREN = OFF
#pragma config XINST = OFF

/** Define Constants Here ******************************************/
#define PRESSED 0

/** Global Variables ***********************************************/
unsigned char timeDelayMs = 250;
int RA2result;
int distanceFromCenter;

/*******************************************************************
 * Function:        void main(void)
 ********************************************************************/
#pragma code

void main(void) {
    // Set the clock to 4 MHz
    OSCCONbits.IRCF2 = 1;
    OSCCONbits.IRCF1 = 1;
    OSCCONbits.IRCF0 = 0;

    // Pin IO Setup
    OpenADC(ADC_FOSC_8 & ADC_RIGHT_JUST & ADC_12_TAD,
            ADC_CH0 & ADC_INT_OFF & ADC_REF_VDD_VSS,
            0x0B); // Four analog pins
    TRISA = 0xFF; // All of PORTA input
    TRISB = 0xFF; // All of PORTB input
    TRISC = 0x00; // All of PORTC output
    TRISD = 0x00; // All of PORTD output
    PORTC = 0x00; // Turn off all 8 Port C outputs

    while (1) {
        
        if (PORTAbits.RA4 == PRESSED) {
            PORTC = 0x00;
        } else {
            SetChanADC(ADC_CH2);	// Select the pin
            ConvertADC(); 			// Start conversion
            while( BusyADC() ); 	// Wait for completion
            RA2result = ReadADC(); 	// Read result
            
            distanceFromCenter = 512 - RA2result;
            if (distanceFromCenter < 0) {
                distanceFromCenter = -distanceFromCenter;
            }
            timeDelayMs = 250 - distanceFromCenter * 2 / 5;
            
            PORTC = 0xAA;
            Delay1KTCYx(timeDelayMs);
            PORTC = 0x55;
            Delay1KTCYx(timeDelayMs);            
        }
    }
}