//EXIT 0x01
//JUMP 0x02
//VAR 0x03
//ADD 0x04
//SUB 0x05
//MUL 0x06
//DIV 0x07
//DEC 0x08
//INC 0x09
//IF 0x0A
//READBYTE 0x0B   specify a port (0:Serial  1:Port1  etc)
//WAIT 0x0C
//RETJMP 0x0D   STILL NEEDS CODING
//SETCOL 0x0E
//PIXEL 0x0F
//RECT 0x10
// LOADFILE 0x11   Loads a code file and runs it
// PREVFILE 0x12   Closes current code file and loads the previously opened file


#include <Adafruit_GFX.h>       // include Adafruit graphics library
#include <Adafruit_ILI9341.h>   // include Adafruit ILI9341 TFT library
#include <SPI.h>

#define TFT_CS 10     // TFT CS  pin is connected to arduino pin 8
#define TFT_RST 9     // TFT RST pin is connected to arduino pin 9
#define TFT_DC 8     // TFT DC  pin is connected to arduino pin 10

#include <SD.h>
#include <SoftwareSerial.h>

SoftwareSerial Port1(2, 3); // RX, TX

Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC, TFT_RST);

const int chipSelect = 4;
File file;
boolean FileOpen = false;

int Opcode = 0;
uint16_t Location = 0;
int8_t JumpLocation = 0;
uint16_t JumpBuffer[5];


// Memory stuff
unsigned long Value1;
unsigned long Value2;
unsigned long Value3;
unsigned long Value4;

int8_t R;
int8_t G;
int8_t B;

char FilenameBuffer[15];
int16_t Ram[10];

void Reset(){
    Opcode = 0;
    Location = 0;
    JumpLocation = 0;
    memset(JumpBuffer, 0, sizeof(JumpBuffer));


    // Memory stuff
    Value1 = 0;
    Value2 = 0;
    Value3 = 0;
    Value4 = 0;

    R = 0;
    G = 0;
    B = 0;

    memset(FilenameBuffer, 0, sizeof(FilenameBuffer));
    memset(Ram, 0, sizeof(Ram));
}


void OpenCodeFile(int Len){
    Location = 0;
    const String Extension = ".txt";
    char FileNameArray[Len + 4];
    
    
    for(int i = 0; i < Len; i++){
        FileNameArray[i] = FilenameBuffer[i];
    }

    FileNameArray[Len] = Extension[0];
    FileNameArray[Len + 1] = Extension[1];
    FileNameArray[Len + 2] = Extension[2];
    FileNameArray[Len + 3] = Extension[3];

    file = SD.open(FileNameArray);
}

void setup() {
    Serial.begin(9600);
   
    Serial.println("Loading");

    tft.begin();
    delay(100);

    if (!SD.begin(chipSelect)) {
        Serial.println("SD Initialization failed.");
        while (true);
    }
    
    file = SD.open("Main.txt");
    if (!file) {
        Serial.println("error opening FILE");
        while(true);
    }

    delay(500);
    tft.setRotation(1);
    
    tft.fillScreen(ILI9341_BLACK);
    FileOpen = true;
    
    Port1.begin(4800);

    Serial.println("Initialized");
}

void loop() {
    if(FileOpen){
        file.seek(Location);
        
        Opcode = file.read();
    
        Interpret(Opcode);
    
        Location++;
       
    }

    tft.fillRect(160, 0, 2, 80, rgbToHex(255, 255, 255));
    tft.fillRect(0, 80, 160, 2, rgbToHex(255, 255, 255));

}

void Interpret(int Code){
    switch (Code) {
        case 0x01: // EXIT
            EXIT();
            break;
        case 0x02: // JUMP
            JUMP();
            break;
        case 0x03: // VAR
            VAR();
            break;
        case 0x04: // ADD
            ADD();
            break;
        case 0x05: // SUB
            SUB();
            break;
        case 0x06: // MUL
            MUL();
            break;
        case 0x07: // DIV
            DIV();
            break;
        case 0x08: // DEC
            DECR();
            break;
        case 0x09: // INC
            INC();
            break;
        case 0x0A: // IF
            IF();
            break;
        case 0x0B: // END
            READBYTE();
            break;
        case 0x0C: // WAIT
            WAIT();
            break;
        case 0x0D: // RETJMP
            RETJMP();
            break;
        case 0x0E: // SETCOL
            SETCOL();
            break;
        case 0x0F: // PIXEL
            PIXEL();
            break;
        case 0x10: // CLS
            RECT();
            break;
        case 0x11: // LOADFILE
            LOADFILE();
            break;
        default:
            // statements
            break;
    }
}

uint16_t rgbToHex(uint8_t r, uint8_t g, uint8_t b) {
  return ((r & 0xF8) << 8) | ((g & 0xFC) << 3) | (b >> 3);
}

void EXIT(){
    file.close();
    Reset();
    file = SD.open("Main.txt");
}

void JUMP(){
    Location += 2;
    
    //ReturnJumpLocation = Location;
    
    JumpBuffer[JumpLocation] = Location;
    JumpLocation += 1;
    if(JumpLocation >= 4){
        JumpLocation = 0;
    }
    
    Value1 = (unsigned long)file.read() << 8 | file.read();
    
    Location = Value1 - 1; // may or may not me - 1
}

void VAR(){
    Location += 4;
    
    Value1 = (unsigned long)file.read() << 8 | file.read();
    Value2 = (unsigned long)file.read() << 8 | file.read();
    
    Ram[Value1] = Value2;
}

void ADD(){
    Location += 6;
    
    Value1 = (unsigned long)file.read() << 8 | file.read();
    Value2 = (unsigned long)file.read() << 8 | file.read();
    Value3 = (unsigned long)file.read() << 8 | file.read();
    
    Ram[Value3] = Value1 + Value2;
}

void SUB(){
    Location += 6;
    
    Value1 = (unsigned long)file.read() << 8 | file.read();
    Value2 = (unsigned long)file.read() << 8 | file.read();
    Value3 = (unsigned long)file.read() << 8 | file.read();
    
    Ram[Value3] = Value1 - Value2;
}

void MUL(){
    Location += 6;
    
    Value1 = (unsigned long)file.read() << 8 | file.read();
    Value2 = (unsigned long)file.read() << 8 | file.read();
    Value3 = (unsigned long)file.read() << 8 | file.read();
    
    Ram[Value3] = Value1 * Value2;
}

void DIV(){
    Location += 6;
    
    Value1 = (unsigned long)file.read() << 8 | file.read();
    Value2 = (unsigned long)file.read() << 8 | file.read();
    Value3 = (unsigned long)file.read() << 8 | file.read();
    
    Ram[Value3] = Value1 / Value2;
}

void DECR(){
    Location += 2;
    
    Value1 = (unsigned long)file.read() << 8 | file.read();
    
    Ram[Value1] -= 1;
}

void INC(){
    Location += 2;
    
    Value1 = (unsigned long)file.read() << 8 | file.read();
    
    Ram[Value1] += 1;
}

void IF(){
    // IF Ram1 Condition Ram2 JumpIndex
    // 0x0A 0x00 0x00 0x01 0x00 0x00 0x00 0x00
    
    // Conditions
    // =  0x00
    // <  0x01
    // >  0x02
    // != 0x03
    // <= 0x04
    // >= 0x05
    
    Location += 7;
    
    Value1 = (unsigned long)file.read() << 8 | file.read(); // Ram1
    Value2 = file.read(); // Condition
    Value3 = (unsigned long)file.read() << 8 | file.read(); // Ram2
    Value4 = (unsigned long)file.read() << 8 | file.read(); // JumpIndex
    
    switch (Value2) {
        case 0x00: // =
            if (Ram[Value1] == Ram[Value3]){
                break;
            }
            else{
                Location = Value4 - 1; // not sure if need the (- 1)
            }
            break;
        case 0x01: // <
            if (Ram[Value1] < Ram[Value3]){
                break;
            }
            else{
                Location = Value4 - 1; // not sure if need the (- 1)
            }
            break;
        case 0x02: // >
            if (Ram[Value1] > Ram[Value3]){
                break;
            }
            else{
                Location = Value4 - 1; // not sure if need the (- 1)
            }
            break;
        case 0x03: // !=
            if (Ram[Value1] != Ram[Value3]){
                break;
            }
            else{
                Location = Value4 - 1; // not sure if need the (- 1)
            }
            break;
        case 0x04: // <=
            if (Ram[Value1] <= Ram[Value3]){
                break;
            }
            else{
                Location = Value4 - 1; // not sure if need the (- 1)
            }
            break;
        case 0x05: // >=
            if (Ram[Value1] >= Ram[Value3]){
                break;
            }
            else{
                
                Location = Value4 - 1; // not sure if need the (- 1)
            }
            break;
        default:
            break;
    }
}

void READBYTE(){
    Location += 3;
    Value1 = file.read(); // Port ID
    Value2 = (unsigned long)file.read() << 8 | file.read(); // Ram Index
    
    if (Value1 == 0){ // Reading From Hardware Serial
        if (Serial.available()) {
            Ram[Value2] = Serial.read();
        }
    }
    else if(Value1 == 1){ // Reading From Port1 Software Serial
        if(Port1.available()){
            Ram[Value2] = Port1.read();    
        }
    }
}

void WAIT(){
    Location += 2;
    
    Value1 = (unsigned long)file.read() << 8 | file.read();
    
    delay(Value1);
}

void RETJMP(){
    if(JumpLocation > 0){
        JumpLocation -= 1;
    }
    else{
        JumpLocation = 4;
    }
    Location = JumpBuffer[JumpLocation];
}

void SETCOL(){
    Location += 3;
    
    Value1 = file.read();
    Value2 = file.read();
    Value3 = file.read();
    
    R = Value1;
    G = Value2;
    B = Value3;
}

void PIXEL(){
    Location += 4;
    
    Value1 = (unsigned long)file.read() << 8 | file.read();
    Value2 = (unsigned long)file.read() << 8 | file.read();
    
    tft.drawPixel(Ram[Value1], Ram[Value2], rgbToHex(R, G, B));
}

void RECT(){
    Location += 8;
    
    Value1 = (unsigned long)file.read() << 8 | file.read(); // X Pos
    Value2 = (unsigned long)file.read() << 8 | file.read(); // Y Pos
    Value3 = (unsigned long)file.read() << 8 | file.read(); // Width
    Value4 = (unsigned long)file.read() << 8 | file.read(); // Height
    
    tft.fillRect(Ram[Value1], Ram[Value2], Ram[Value3], Ram[Value4], rgbToHex(R, G, B));
}

void LOADFILE(){
    // LOADFILE FILENAME
    // LOADFILE 1 BYTES FOR LEN FILENAME
    
    
    Value1 = file.read();
    
    Location += 1 + Value1;
    
    if(Value1 <= 15){
        
        // Load Prev filename into temp buffer
        char TempBuffer[15];
        for(int i = 0; i < 15; i++){
            TempBuffer[i] = FilenameBuffer[i];
        }
        
        // Load in the new filename into buffer
        for(int i = 0; i < Value1; i++){
            FilenameBuffer[i] = file.read();   
        }
      
        
        OpenCodeFile(Value1);
        
        // restore the prev filename
        for(int i = 0; i < 15; i++){
            FilenameBuffer[i] = TempBuffer[i];
        }
    }
}

void PREVFILE(){

}