/*
 * File:   main.c
 * Author:
 *
 * Created on September 14, 2019, 12:00 AM
  *

 *----------------------
  Revision History

 *----------------------
 */


#include <stdio.h>
#include <p18cxxx.h>
#include "ConfigRegsPIC18f4520.h"
#include <delays.h>     // For the delay routines used
#include <string.h>
#include <stdlib.h>

/*
 * LCD display
 *
 * PORTD
 * 0-3: DATA -> LCD DB4-7
 * 4: RS -> LCD 4
 * 5: R/W -> LCD 5
 * 6: E -> LCD 6

 * One timing cycle is 100ns
 * One instruction cycle is 400ns
 */

// The RS pin is either 0 or 1 to select the corresponding register
#define DATA 1
#define CMD 0

#define LCD_PORT PORTD        // LCD is on Port D
#define LCD_DIR  TRISD        // Data direction register for Port D
#define LCD_E    PORTDbits.RD6   // LCD Enable bit mask (pin 6)
#define LCD_RS   PORTDbits.RD4   // LCD Register Select signal
#define LCD_RW   PORTDbits.RD5  // RW pin

#define LCD_INIT 0x28   // set 4-bit data, 2-line display, 5x7 font
#define CLR_SCREEN 0x01         // Clears the screen
#define CUR_LINE2 0xC0          // Brings the cursor to the beginning of line 2
#define CUR_SHIFT_DIR 0x6       // Makes the cursor shift right (rather than left) after data is written
#define CUR_RIGHT 0b00010100    // This command shifts the cursor right

#define BUFF_SIZE 20    // Input buffer size

// The different states of the program
#define ROW_SEL 0
#define POS_SEL 1
#define MSG_IN 2

// At 10Mhz, each timing cycle is 100ns
#define delay1ms Delay10KTCYx


//#pragma code

unsigned char rowSelStr[] = " ";
unsigned char posSelStr[] = " ";
unsigned char msgSelStr[] = " ";

char string1[] = "Entering";
char string2[] = "User Mode...";
//char string3[] = "Manual Mode...";
//char string4[] = "Assist Mode...";
//char string5[] = "Auto Mode...";
//char string6[] = "Factory Mode...";

int modeState = 0; // 0: manual, 1: assist, 2: auto, 3: factory (change to struct/enum later)
int editing = 0;
int selected = 0;
int leftFlag = 0;
int rightFlag = 0;
int functionState = 0;
int displayModeState = 0; // 0: manual, 1: assist, 2: auto, 3: factory (change to struct/enum later)


char ModeStrings[] =
"Manual Mode...  "
"Assist Mode...  "
"Auto Mode...    "
"Factory Mode... "
;

char ManualFunctions[] =
"Motors:         "
"Speed:          "
"User mode:      "
;

unsigned char FactoryFunctions[] =
"SET_PID_GAINS   "
"Max Speed:      "
"Max Yaw Rate    "
"IR Samples/Est. "
//"SET_IR_SMPL_RATE"
//"SHOW_STATISTICS "
;
//#define MSG_DISP 3
char state = ROW_SEL;   // Initialise

// Temporary variable for serial input
char inChar;

// Buffer for message input
char msgIn[BUFF_SIZE];
char function1[16];
char description[16];

char msgPos = 0;
unsigned char row, pos;


char * motorString = "OFF";
char * modeString = "MANUAL ";


unsigned char bufPos = 0;

int i;
char rxStr[] = "";
char rxStrPos = 0;

void high_interrupt(void);
void highPriorityISR(void);
void rx232(void);
void tx232(char * );
void sendChar(char);

char * ptrDesc;

/*void delay1ms(char num) {
    Delay10KTCYx(num);
}*/

#pragma code highIntAddress=0x8
void high_interrupt(void) {
    _asm GOTO highPriorityISR _endasm
}

void char2LCD(char c, char rs) {

    // Make sure we've received a valid value
  if (rs == CMD || rs == DATA) {

    char byteHigh, byteLow;

    // We need to output data on bits 0-3 of PORTD, 4 at a time,
    // so we'll prepare the low half and high half
    byteLow = (c & 0x0F);
    byteHigh = (c & 0xF0) >> 4;

    // Set the RS pin high or low and clear the other pins
    // Also write the high half of the byte
    //LCD_PORT = byteHigh|rs;
    //PORTD |= LCD_E;    // Set PD1 high (the Enable (E) pin)
    LCD_PORT = byteHigh;   // Reset the pins and put our new data in
    LCD_RS = rs;      //RS

    LCD_E = 1;      // E

     // We need to keep E high for at least 140ns ~= 2.3 cycles
    Nop();
    Nop();
    Nop();

    //LCD_PORT &= ~(LCD_E); // Set PK1 low (the Enable pin)
     LCD_E = 0;      // E

     Nop(); // Enable cycle time is 1200ns
     Nop();
     Nop();

    LCD_PORT = byteLow;   // Reset the pins and put our new data in
     LCD_RS = rs;      //RS
    //LCD_PORT |= LCD_E;    // Set PK1 high (the Enable pin)
    LCD_E = 1;      // E

    // We need to keep E high for it to read the data
    Nop();
    Nop();
    Nop();

    LCD_E = 0;      // E
    //delay1ms(1);              // Wait for command to execute
    Delay10TCYx(10);
    //LCD_PORT = 0;              // Clear the pins (not really necessary)
  }
}

void LCDinit(void) {

    LCD_DIR = 0;
    LCD_PORT = 0;
    LCD_RS = 0;
    LCD_RW = 0; // Writing only

    delay1ms(250);  // give it time

    char2LCD(0x32, CMD);    //reset
    char2LCD(LCD_INIT, CMD);  // set 4-bit data, 2-line display, 5x7 font
    char2LCD(0x0C, CMD);  // turn on display, cursor, not blinking
    char2LCD(CUR_SHIFT_DIR, CMD);  // move cursor right
    char2LCD(0x01, CMD);  // clear screen,
    char2LCD(0x2, CMD);   // move cursor to home

    //char2LCD(0b010, CMD);
    delay1ms(2);          // wait until "clear display" command is complete

}

void str2LCD(char * str, unsigned char pos, unsigned char row) {
    // Clear the screen
    //char2LCD(CLR_SCREEN, CMD);
    //delay1ms(2); // needed for screen clear

    // Put the cursor on the second row if necessary
    if(row) {
        char2LCD(CUR_LINE2, CMD);
        delay1ms(1);
    }

    // Increment the cursor to the right position
    while (pos > 0) {
        //char2LCD(' ', DATA);
        char2LCD(CUR_RIGHT, CMD);
        pos--;
    }

    // Send the string through
    while (*str != 0) {
        char2LCD(*str, DATA);
        str++;
    }
}

void Print2Lines(char * str1, unsigned char pos1, unsigned char row1, char *    str2, unsigned char pos2, unsigned char row2){
    char2LCD(CLR_SCREEN, CMD);
    delay1ms(2); // needed for screen clear
    str2LCD(str1, pos1, row1);
    str2LCD(str2, pos2, row2);
}



 int motorState = 0;

 void GetMotorState(){

     // IN EDITING MODE: if left or right button is pressed, switch the motor state
     if(editing&&functionState==0){
         if(leftFlag||rightFlag){
         motorState^=1;
         leftFlag = 0;
         rightFlag = 0;
         //editing=0;
         }

     }

     if(motorState){
//         strcpypgm2ram(motorString,"ON ");
         motorString = "ON ";


     }
     else{
//         strcpypgm2ram(motorString,"OFF");
         motorString = "OFF";


     }

     if(selected){

             // COMMUNICATE TO ROBOT THAT WHETHER IT IS ON/OFF
           editing = 0;
           selected = 0;
    }
 }

 int tempModeState=0;

 void GetModeState() {

     if(editing){

         if(leftFlag){
            if(tempModeState!=3){
             tempModeState++;
            }
            else {
             tempModeState=0;
            }
            leftFlag=0;
         }

         else if(rightFlag){
            if(tempModeState!=0){
             tempModeState--;
            }
            else {
             tempModeState=3;
            }
            rightFlag=0;
         }

     }

     // SWITCH CASE
     if(tempModeState==1){
//         strcpypgm2ram(modeString,"ASSIST");
         modeString = "ASSIST ";

     }
     else if(tempModeState==2){
//         strcpypgm2ram(modeString,"AUTO ");
         modeString = "AUTO   ";


     }
     else if(tempModeState==3){
//         strcpypgm2ram(modeString,"FACTORY");
         modeString = "FACTORY";


     }
     else{
//        strcpy(modeString,"MANUAL1 ");
         modeString = "MANUAL ";
     }

      if(selected){


        // GET MODE string that it is entering
        for(i = 0; i < 16; i++){
            description[i] = ModeStrings[tempModeState*16+i];
        }

        Print2Lines(string1, 0, 0, description, 0, 1);

        for (i = 0; i < 2; i++){
                delay1ms(250);
        };

        // COMMUNICATE TO ROBOT WHICH MODE IT IS ENTERING
        modeState = tempModeState;

        functionState = 0; //Will reset function state to be back to 0 so that when you change modes, it displays from the beginning

        editing = 0;
        selected = 0;
         }

 }





void DisplayManualCommands(){


    for(i = 0; i < 16; i++){
        tx232(string1);
        function1[i] = ManualFunctions[functionState*16+i];

    }

     switch(functionState){
         case 0:
             GetMotorState();
             strcpypgm2ram(description,motorString);
             break;
        case 1:
            strcpypgm2ram(description,"-128 to 128");
            break;
         case 2:
             GetModeState();
            strcpypgm2ram(description,modeString);
            break;
         default:
             return;

     }

     Print2Lines(function1, 0, 0, description, 0, 1);
}
void DisplayAssistCommands(){
for(i = 0; i < 16; i++){
//        tx232(string1);
        function1[i] = FactoryFunctions[functionState*16+i];

    }

     switch(functionState){
         case 0:
             //PID GAINS

             strcpypgm2ram(description,"Kp=0 Ki=0 Kd=0");
             break;
        case 1:
            // SPEED
            strcpypgm2ram(description,"Yeetus Feetus");
            break;
         case 2:
             //YAW RATE
//             GetModeState();
            strcpypgm2ram(description,"Yeehaw 500");
            break;
         case 3:
             // IR samples/est
            strcpypgm2ram(description,"IR KMS lol");
         default:
             return;

     }

     Print2Lines(function1, 0, 0, description, 0, 1);}
void DisplayAutoCommands(){
for(i = 0; i < 16; i++){
//        tx232(string1);
        function1[i] = FactoryFunctions[functionState*16+i];

    }

     switch(functionState){
         case 0:
             //PID GAINS

             strcpypgm2ram(description,"Kp=0 Ki=0 Kd=0");
             break;
        case 1:
            // SPEED
            strcpypgm2ram(description,"Yeetus Feetus");
            break;
         case 2:
             //YAW RATE
//             GetModeState();
            strcpypgm2ram(description,"Yeehaw 500");
            break;
         case 3:
             // IR samples/est
            strcpypgm2ram(description,"IR KMS lol");
         default:
             return;

     }

     Print2Lines(function1, 0, 0, description, 0, 1);}

void DisplayFactoryCommands(){


    for(i = 0; i < 16; i++){
//        tx232(string1);
        function1[i] = FactoryFunctions[functionState*16+i];

    }

     switch(functionState){
         case 0:
             //PID GAINS

             strcpypgm2ram(description,"Kp=0 Ki=0 Kd=0");
             break;
        case 1:
            // SPEED
            strcpypgm2ram(description,"Yeetus Feetus");
            break;
         case 2:
             //YAW RATE
//             GetModeState();
            strcpypgm2ram(description,"Yeehaw 500");
            break;
         case 3:
             // IR samples/est
            strcpypgm2ram(description,"IR KMS lol");
         default:
             return;

     }

     Print2Lines(function1, 0, 0, description, 0, 1);
}


void SerialInit(void) {
    // Enable SCI TX/RX on RC6 (TX) and RC7 (RX)
    TRISC = 0b11000000;

    //TRISA = 0;
    // 1 is input, 0 is output
    TRISB = 0b00100111;

    // Enable serial port, SPEN = 1, CREN = 1
    RCSTA = 0b10010000;

    // For 9600 baud rate
    // SPBRG = (Fosc/(16*BR))-1 for 8 bit, BRGH = 0
    SPBRG = 15;

     // Enable TX, low speed baud BRGH (/64), CSRC = 1, TXEN = 1
    TXSTA = 0b00100000;

    // GIE, PIE, INT0IE, RBIE on (Note: INT0IF and RBIF are the flags)
    //INTCON = 0b11010000; // NO RBIE
    INTCON = 0b11011000;

    // PORTB pull-ups are disabled, all external bits interrupt on rising edge, RB port change is high priority
   INTCON2 = 0b11110001;

    // INT1:2 are high priority, INT1:2 are enabled (Note: INT1IF and INT2IF are the flags)
    INTCON3 = 0b11011000;



    // RCON = // IPEN = 0 by default

    // RCIE = 1
    PIE1 = 0b00100000;
}

// Send a char via serial
void txChar (char c) {
    while(((TXSTA >> 1) & 1) == 0) {};
    TXREG = c;
}

// Send a string via serial
void tx232(char * str) {
    // Loop till null terminator
    while(*str != 0) {
        txChar(*str);
        str++;
    }
}

// Serial receive function
void rx232(void) {
    switch (state) {
        // Decide what to do with input depending on what stage of the program we're in
        case ROW_SEL:
            // Takes in a '0' or '1' (doesn't wait for \r)
            inChar = RCREG;

            if ((inChar >= '0') && (inChar <= '1')) {
                txChar(inChar);     // Send the character back
                row = inChar - '0'; // Convert from ASCII to number
                state = POS_SEL;    // Increment the state
                tx232(posSelStr);   // Display user prompt for next state
            }
            break;


        case POS_SEL:
            //state = ROW_SEL;
            inChar = RCREG;

            // Check that the input is a number
            if ((inChar >= '0') && (inChar <= '9')) {
                // Read the position into the msg buffer as it can be more than one character long
                msgIn[msgPos] = inChar;

                txChar(inChar);     // Send the character back
                msgPos++;           // Increment the position for next time

                // If the user has input two characters, progress to the next step
                if(msgPos > 1) {

                    msgIn[msgPos] = 0;  // Null terminate
                    pos = atoi(msgIn);  // Convert the input to a number
                    msgPos = 0;         // Reset the msg buffer for msg input
                    state = MSG_IN;     // Increment the state
                    tx232(msgSelStr);   // Send the user prompt for the next state
                }

            } else if((inChar == '\r') && (msgPos == 1)) {
                // If the user inputs a single-digit number, they can press enter to progress

                pos = msgIn[0] - '0';   // Convert the ASCII character previously input to number
                msgPos = 0;             // Reset the msg buffer for msg input
                state = MSG_IN;         // Increment the state
                tx232(msgSelStr);       // Send the user prompt for the next state
            }
            break;

        // Message input
        case MSG_IN:

            // Read the character into the next position in the buffer
            msgIn[msgPos] = RCREG;
            txChar(msgIn[msgPos]);  // Send character back

            // Message is terminated by a carriage return
            if(msgIn[msgPos] == '\r') {
                msgIn[msgPos] = 0;  // Null terminate

                // Display the message on the LCD
                str2LCD(msgIn, pos, row);
                state = ROW_SEL;    // Go back to the original state
                tx232(rowSelStr);   // Send the prompt for the new state
                msgPos = 0;         // Reset the msg buffer
            } else {
                // If it isn't a \r, increment to the next position of the buffer for the next character
                msgPos++;

                // If the input is longer than the buffer, stop taking data in and just display it
                if (msgPos >= BUFF_SIZE) {
                    // Display the message on the LCD
                    str2LCD(msgIn, pos, row);
                    state = ROW_SEL;    // Go back to the original state
                    tx232(rowSelStr);   // Send the prompt for the new state
                    msgPos = 0;         // Reset the msg buffer
                }
            }
            break;

        default:
            break;
    }

    return;
}


//int i;
void main(void) {

//    initFunctions();
    LCDinit();
    SerialInit();       // INITs interrupt and PORTB

    PORTCbits.RC0=0;


    // An initial welcome to show it's working
//    for(i = 0; i < 16; i++){
////        tx232(string1);
//        string2[i] = ModeStrings[modeState*16+i];
//
//    }

    Print2Lines(string1, 0, 0, string2, 0, 1);

    for (i = 0; i < 2; i++){
         delay1ms(250);
    }


        for(i = 0; i < 16; i++){
//        tx232(string1);
        description[i] = ModeStrings[modeState*16+i];

    }

    Print2Lines(string1, 0, 0, description, 0, 1);
     for (i = 0; i < 2; i++){
         delay1ms(250);
    };


    // The initial prompt
    tx232(rowSelStr);

    // Stay in a loop, wait for interrupts
    while(1){

        switch(modeState){
            case 0:
                 DisplayManualCommands();
            case 1:
                DisplayAssistCommands();
            case 2:
                DisplayAutoCommands();
            case 3:
                DisplayFactoryCommands();
        }
       };

    return;
}

#pragma interrupthigh highPriorityISR
void highPriorityISR(void) {
    int maxFunctions = 2;
    // SCROLL RIGHT
    if(INTCONbits.INT0IF){
        /*
         * if up is pressed ++position and ? the function of that particular position>?
         * if down is pressed --position
         * when enter is pressed enter
         */

        PORTCbits.RC0 ^= 1;

        if(!editing) {
           if(functionState<maxFunctions){
                functionState++;
            }
        }
        else {
            rightFlag = 1;
        }

        INTCONbits.INT0IF=0;

    }

    // ENTER
    else if(INTCON3bits.INT1IF) {
        PORTCbits.RC0 ^= 1;
        if(editing){
            selected = 1;
            //editing = 0;
        }
        else{
            editing = 1;
        }

        INTCON3bits.INT1IF=0;


    }

    // SCROLL LEFT
    else if(INTCON3bits.INT2IF) {
        PORTCbits.RC0 ^= 1;


        if(!editing) {
            if(functionState>0){
            functionState--;
            }
        }
        else {
            leftFlag = 1;
        }
        INTCON3bits.INT2IF=0;

    }

    // BACK
    else if (INTCONbits.RBIF){
        PORTCbits.RC0 ^= 1;
        editing = 0;
        INTCONbits.RBIF=0;
    }




    // Check that it is indeed the receive interrupt that has triggered
    if((PIR1bits.RCIF == 1) && (PIE1bits.RCIE == 1)) {
        // Clear the receive flag
        PIR1bits.RCIF = 0;
        // Deal with the input
        rx232();
    }
    // Shouldn't be necessary but I found that I had to manually re-enable interrupts
    INTCONbits.GIE = 1;



}