#include <SD.h>
#include "StringHelper.h"
 
File file;
bool jump;
bool jumping;
bool running;
char label [10];
char fileName [16];
byte storage [64];
const char* const PROGMEM instructions [] = { 
  "j", "li", "lb", "sb", "beq", "bgt", "blt", "bge", "ble",
   "add", "sub", "mul", "divu", "addi", "sll", "srl", "andi"
};

// Exits a program.
void exitProgram()
{
  file.close();
  running = false;
}

// Reserved storage handling.
void readFromStorage(uint8_t rd, uint8_t rs)
{
  switch(rs)
  {
    case 1:
      storage[rd] = 5 * (analogRead(A0) / 1023.0);
      break;
    case 2:
      storage[rd] = 5 * (analogRead(A1) / 1023.0);
      break;
    case 3:
      storage[rd] = DDRB;
      break;
    case 4:
      storage[rd] = PINB;
      break;
    case 5:
      storage[rd] = DDRD;
      break;
    case 6:
      storage[rd] = PIND;
      break;
    case 7:
      storage[rd] = DDRC;
      break;
    case 8:
      storage[rd] = PINC;
    case 9:
      storage[rd] = 5 * (analogRead(A2) / 1023.0);
      break;
    case 10:
      storage[rd] = 5 * (analogRead(A3) / 1023.0);
      break;
    case 11:
      storage[rd] = 5 * (analogRead(A4) / 1023.0);
      break;
    case 12:
      storage[rd] = 5 * (analogRead(A5) / 1023.0);
      break;
    default:
      break;
  }
}

// Reserved storage handling.
void writeToStorage(uint8_t rd)
{
  if (rd == 1)
  {
    char c = (char)storage[1];
    c = c == '[' ? ' ' : c;
    c = c == ']' ? '\n' : c;
    Serial.print(c);
  }
  else if (rd == 2)
  {
    Serial.print(storage[2]);
  }
  else if (rd == 3)
  {
    DDRB = storage[3];
  }
  else if (rd == 4)
  {
    PORTB = storage[4];
  }
  else if (rd == 5)
  {
    DDRD = storage[5];
  }
  else if (rd == 6)
  {
    PORTD = storage[6];
  }
  else if (rd == 7)
  {
    DDRC = storage[7];
  }
  else if (rd == 8)
  {
    PORTC = storage[8];
  }
  else if (rd == 9)
  {
    delayMicroseconds(storage[9]);
  }
  else if (rd == 10)
  {
    delay(storage[10]);
  }
  else if (rd == 11)
  {
    storage[11] = random(0, storage[11]);
  }
  else if (rd == 12)
  {
    exitProgram();
  }
}

// jump
void j(char* line)
{
  memset(&label[0], 0, sizeof(label));
  uint8_t len = strlen(line);
  subString(line, 2, len, label);
  jump = true;
}
 
// load immediate
void li(char* line)
{
  char* results [3];
  splitString(results, line, " ", 3);
  uint8_t rd = atoi(results[1]);
  char* imm = results[2];
  if (isAlpha(imm[0]))
  {
    storage[rd] = (byte)imm[0];
  }
  else if (imm[0] == '[' || imm[0] == ']')
  {
    storage[rd] = (byte)imm[0];
  }
  else
  {
    storage[rd] = (byte)atoi(imm);
  }
  writeToStorage(rd);
}
 
// load byte
void lb(char* line)
{
  char* results [3];
  splitString(results, line, " ", 3);
  uint8_t rd = atoi(results[1]);
  uint8_t rs = atoi(results[2]);
  storage[rd] = storage[rs];
  readFromStorage(rd, rs);
}
 
// store byte
void sb(char* line)
{
  char* results [3];
  splitString(results, line, " ", 3);
  uint8_t rs = atoi(results[1]);
  uint8_t rd = atoi(results[2]);
  storage[rd] = storage[rs];
  writeToStorage(rd);
}
 
// branch if equal
void beq(char* line)
{
  char* results [4];
  splitString(results, line, " ", 4);
  uint8_t rs1 = atoi(results[1]);
  uint8_t rs2 = atoi(results[2]);
  uint8_t val1 = storage[rs1];
  uint8_t val2 = storage[rs2];
  if (val1 == val2)
  {
    strcpy(label, results[3]);
    jump = true;
  }
}
 
// branch if greater than
void bgt(char* line)
{
  char* results [4];
  splitString(results, line, " ", 4);
  uint8_t rs1 = atoi(results[1]);
  uint8_t rs2 = atoi(results[2]);
  uint8_t val1 = storage[rs1];
  uint8_t val2 = storage[rs2];
  if (val1 > val2)
  {
    strcpy(label, results[3]);
    jump = true;
  }
}
 
// branch if less than
void blt(char* line)
{
  char* results [4];
  splitString(results, line, " ", 4);
  uint8_t rs1 = atoi(results[1]);
  uint8_t rs2 = atoi(results[2]);
  uint8_t val1 = storage[rs1];
  uint8_t val2 = storage[rs2];
  if (val1 < val2)
  {
    strcpy(label, results[3]);
    jump = true;
  }
}
 
// branch if greater than or equal to
void bge(char* line)
{
  char* results [4];
  splitString(results, line, " ", 4);
  uint8_t rs1 = atoi(results[1]);
  uint8_t rs2 = atoi(results[2]);
  uint8_t val1 = storage[rs1];
  uint8_t val2 = storage[rs2];
  if (val1 >= val2)
  {
    strcpy(label, results[3]);
    jump = true;
  }
}
 
// branch if less than or equal to
void ble(char* line)
{
  char* results [4];
  splitString(results, line, " ", 4);
  uint8_t rs1 = atoi(results[1]);
  uint8_t rs2 = atoi(results[2]);
  uint8_t val1 = storage[rs1];
  uint8_t val2 = storage[rs2];
  if (val1 <= val2)
  {
    strcpy(label, results[3]);
    jump = true;
  }
}
 
// add
void add(char* line)
{
  char* results [4];
  splitString(results, line, " ", 4);
  uint8_t rd = atoi(results[1]);
  uint8_t rs1 = atoi(results[2]);
  uint8_t rs2 = atoi(results[3]);
  uint8_t val1 = storage[rs1];
  uint8_t val2 = storage[rs2];
  storage[rd] = val1 + val2;
  writeToStorage(rd);
}
 
// subtract
void sub(char* line)
{
  char* results [4];
  splitString(results, line, " ", 4);
  uint8_t rd = atoi(results[1]);
  uint8_t rs1 = atoi(results[2]);
  uint8_t rs2 = atoi(results[3]);
  uint8_t val1 = storage[rs1];
  uint8_t val2 = storage[rs2];
  storage[rd] = val1 - val2;
  writeToStorage(rd);
}
 
// multiply
void mul(char* line)
{
  char* results [4];
  splitString(results, line, " ", 4);
  uint8_t rd = atoi(results[1]);
  uint8_t rs1 = atoi(results[2]);
  uint8_t rs2 = atoi(results[3]);
  uint8_t val1 = storage[rs1];
  uint8_t val2 = storage[rs2];
  storage[rd] = val1 * val2;
  writeToStorage(rd);
}
 
// divide unsigned
void divu(char* line)
{
  char* results [4];
  splitString(results, line, " ", 4);
  uint8_t rd = atoi(results[1]);
  uint8_t rs1 = atoi(results[2]);
  uint8_t rs2 = atoi(results[3]);
  uint8_t val1 = storage[rs1];
  uint8_t val2 = storage[rs2];
  storage[rd] = val1 / val2;
  writeToStorage(rd);
}
 
// add immediate
void addi(char* line)
{
  char* results [4];
  splitString(results, line, " ", 4);
  uint8_t rd = atoi(results[1]);
  uint8_t rs = atoi(results[2]);
  uint8_t imm = atoi(results[3]);
  uint8_t valS = storage[rs];
  storage[rd] = valS + imm;
  writeToStorage(rd);
}

// shift left
void sll(char* line)
{
  char* results [4];
  splitString(results, line, " ", 4);
  uint8_t rd = atoi(results[1]);
  uint8_t rs1 = atoi(results[2]);
  uint8_t rs2 = atoi(results[3]);
  uint8_t val1 = storage[rs1];
  uint8_t val2 = storage[rs2];
  storage[rd] = val1 << val2;
  writeToStorage(rd);
}

// shift right
void srl(char* line)
{
  char* results [4];
  splitString(results, line, " ", 4);
  uint8_t rd = atoi(results[1]);
  uint8_t rs1 = atoi(results[2]);
  uint8_t rs2 = atoi(results[3]);
  uint8_t val1 = storage[rs1];
  uint8_t val2 = storage[rs2];
  storage[rd] = val1 >> val2;
  writeToStorage(rd);
}

// AND Immediate
void andi(char* line)
{
  char* results [4];
  splitString(results, line, " ", 4);
  uint8_t rd = atoi(results[1]);
  uint8_t rs = atoi(results[2]);
  uint8_t imm = atoi(results[3]);
  uint8_t valS = storage[rs];
  storage[rd] = valS & imm;
  writeToStorage(rd);
}

// Interprets an instruction.
void execute(char line [64])
{
  char linecopy [64];
  strcpy(linecopy, line);
  char* instruction = strtok(line, " ");
  void (*insptr[])(char*) = { 
    j, li, lb, sb, beq, bgt, blt, bge, ble,
    add, sub, mul, divu, addi, sll, srl, andi
  };
  for (uint8_t i = 0; i < 17; ++i)
  {
    if (strcmp(instruction, (char*)pgm_read_word(&(instructions[i]))) == 0)
    {
      (*insptr[i])(linecopy);
      break;
    }
  }
}

// Reads program files and executes commands.
void runProgram()
{
  jump = false;
  file.close();
  file = SD.open(fileName, FILE_READ);
  if (file)
  {
    running = true;
    char line [64];
    uint8_t index = 0;
    while (file.available() && jump == false)
    {
      char c = file.read();
      if (c == '\n')
      {
        line[index] = '\0';
        execute(line);
        line[0] = '\0';
        index = 0;
      }
      else
      {
        line[index] = c;
        index++;
      }
    }
  }
  else 
  {
    Serial.println("Failed to open file!");
  }
}
 
// Determines where the next instruction is read from.
void jumpToLabel()
{
  jumping = true;
  file.close();
  file = SD.open(fileName, FILE_READ);
  if (file)
  {
    char line [64];
    uint8_t index = 0;
    while (file.available())
    {
      char c = file.read();
      if (c == '\n')
      {
        line[index] = '\0';
        if (jump == false)
        {
          execute(line);
        }
        else if (strcmp(line, label) == 0)
        {
          jump = false;
          jumping = false;
        }
        line[0] = '\0';
        index = 0;
      }
      else
      {
        line[index] = c;
        index++;
      }
    }
  }
  else 
  {
    exitProgram();
    Serial.println("Failed to open file!");
  }
}

// Initialization.
void setup()
{
  Serial.begin(115200);
  SD.begin(10);
  strcpy(fileName, "boot.txt");
  runProgram();
}
 
// Loops consecutively.
void loop() 
{
  if (jump == true && jumping == false)
  {
    jumpToLabel();
  }
  else if (Serial.available() && !running)
  {
    memset(&fileName[0], 0, sizeof(fileName));
    Serial.readBytesUntil('\n', fileName, 16);
    runProgram();
  }
}