#include "dc_motor.h"
#include "LCD.h"
#include <stdio.h>
#include <string.h>
#include "RFID.h"  
#include "IR.h"
#include "Robot_Control.h"

#define PWMcycle 199 //This was calculated.
#pragma config MCLRE=OFF, LVP=OFF, OSC = IRCIO, WDTEN = OFF //Needed in case code does not run with PICkit3 connected. Internal oscillator selected, WDT off.

char buf = 0;            //buffer for characters for LCD
char RFIDData[11];     //to store RFID tag data
char counter = 0;       //counts 10 RFID characters to be displayed 
int timeCounter = 0;    //Seconds counter for motor
int distanceTime = 0;   //Variable to store time/distance travelled
char reached = 0;       //Boolean value to signal if the RFID tag has been reached

//High priority interrupt routine to handle the reading of the RFID tag.
void __interrupt(high_priority)InterruptHandlerHigh(void)
{
    if (PIR1bits.RCIF) {
        PIR1bits.RCIF = 0; //Clear interrupt flag.
        distanceTime = timeCounter;
        reached = 1;

        RFIDData[10] = 0; //to register as a string.
        buf = getCharSerial();
        if (buf == 0x02) {
            SendLCD(0b00000001, 0); //Display CLEAR
            __delay_ms(2);
            SetLine(2); //Set Line 2
            while (counter < 10) { //Only read the 10 Data bytes.
                buf = getCharSerial();
                SendLCD(buf, 1); //output string to LCD
                counter++;
            }
            counter = 0;

            //The EUSART receive buffer, RCREG, is full (cleared when RCREG is read).
            SetLine(1);
            LCD_String("RFID FOUND      ");
        }
    }
}

//Low priority interrupt routine to count seconds.
void __interrupt(low_priority)InterruptHandlerLow(void) 
{
    if (INTCONbits.TMR0IF) {
        timeCounter++; //Counter to count number of seconds
        //Initiate timer with a base value to get 1/2 sec at overflow
        TMR0H = 0b00001011;
        TMR0L = 0b11011100;
        INTCONbits.TMR0IF = 0; //Clear the interrupt flag to return to main code
    }
}

//Main Function
void main(void) {

    struct DC_motor motorL, motorR; //declare 2 motor structures

    OSCCON = 0x72; //Set clock to 8MHz
    while (!OSCCONbits.IOFS); //wait until stable

    InitPWM(PWMcycle); //setup PWM registers

    //Assign initial values of each structure
    motorL.power = 0; //zero power to start
    motorL.direction = 1; //set default motor direction
    motorL.dutyLowByte = (unsigned char *) (&PDC1L); //store address of PWM duty low byte
    motorL.dutyHighByte = (unsigned char *) (&PDC1H); //store address of PWM duty high byte
    motorL.dir_pin = 2; //pin RB2/PWM2 controls direction
    motorL.PWMperiod = PWMcycle; //store PWMperiod for motor

    motorR.power = 0; //zero power to start
    motorR.direction = 1; //set default motor direction
    motorR.dutyLowByte = (unsigned char *) (&PDC0L); //store address of PWM duty low byte
    motorR.dutyHighByte = (unsigned char *) (&PDC0H); //store address of PWM duty high byte
    motorR.dir_pin = 0; //pin RB0/PWM0 controls direction
    motorR.PWMperiod = PWMcycle; //store PWMperiod for motor

    InitTimer(); //Initialise the Timer0 module to count time for motors ON.
    InitIR(); //Initialise the IR
    InitLCD(); //Initialise the LCD
    InitSerial(); //Initialise the RFID Serial Interface

    TRISCbits.RC3 = 1; //Set pin RC3 to input;
    
    SetLine(1);
    LCD_String("PRESS BUTTON    "); //Instructions for user

    while (!PORTCbits.RC3); //check until the button is pressed to start the routine.
    while (reached==0) {
            SendLCD(0b1,0); //Clear the screen before every start.
            __delay_ms(2);

            findBeacon(&motorL, &motorR); //Find the beacon

            SendLCD(0b1, 0);    //Clear LCD display
            __delay_ms(2);
            SetLine(1);
            LCD_String("BEACON FOUND    ");

            timeCounter = 0; //Start the time counter that ticks when TMR0 module overflows.
            while (reached == 0 && timeCounter<12) { //Run this loop until RFID tag is reached or until timeCounter reaches 12 seconds (i.e. the robot has gone past it and missed it by then)
                 moveMotors(&motorL, &motorR, 1, 1, 80); //Set both motors forward to go to beacon
            }

            reverse(&motorL, &motorR, distanceTime); //Reverse towards the starting position

            stop(&motorL, &motorR); //Stop the robot
    }
}