#include <LiquidCrystal.h>

LiquidCrystal lcd(7, 6, 5, 4, 3, 2);

void setup() {
  Serial.begin(9600);
  lcd.begin(16, 2);
  setProgram();
  compile();
  lcd.setCursor(0,1);
  lcd.write("ACC=");
  lcd.setCursor(8,1);
  lcd.write("BAK=");
}

const int MAX_LINES = 15;

typedef struct {
  char *id;
  char *cmd;
  char *arg1;
  char *arg2;
} line;

line program_lines[MAX_LINES];

static char program[MAX_LINES][16];

char mode[2] = "-";

void setProgram(){
  for(int i=0;i<MAX_LINES;i++){
    sprintf(program[i], "");
  }
  int i=0;
  //sprintf(program[i++], "MOV UP ACC");
  //sprintf(program[i++], "ADD ACC");
  //sprintf(program[i++], "SUB UP");
  //sprintf(program[i++], "MOV ACC DOWN");
  sprintf(program[i++], "MOV 10 ACC");
  sprintf(program[i++], "SWP");
  sprintf(program[i++], "MOV 5 ACC");
  sprintf(program[i++], "L:SUB 1");
  sprintf(program[i++], "SWP");
  sprintf(program[i++], "JEZ E");
  sprintf(program[i++], "JMP L");
  sprintf(program[i++], "E:SWP");
  sprintf(program[i++], "MOV ACC DOWN");
}

void debug(char *str){
   Serial.write("\"");
   Serial.write(str);
   Serial.write("\"\n");
}

void compile(){
  for(int i=0;i<MAX_LINES;i++){
    line *line = &program_lines[i];
    char *source = malloc(17);
    strcpy(source, program[i]);
    //Label
    char *found = strstr(source, ":");
    if(found == NULL){
      line->id = NULL;
    }else{
       line->id = source;
       *found = '\0';
       source = found+1;
    }
    //command
    found = strstr(source, " ");
    if(found == NULL){
      line->cmd = source; //no args
      line->arg1 = NULL;
      line->arg2 = NULL;
      continue;
    }else{
       line->cmd = source;
       *found = '\0';
       source = found+1;
    }
    //arg 1
    found = strstr(source, " ");
    if(found == NULL){
      line->arg1 = source; //no 2nd arg
      line->arg2 = NULL;
      continue;
    }else{
       line->arg1 = source;
       *found = '\0';
       line->arg2 = found+1;
    }
  }
}

void spitOutCompiled(){
  debug("All:");
   for(int i=0;i<MAX_LINES;i++){
      line *line = &program_lines[i];
      debug(line->id);
      debug(line->cmd);
      debug(line->arg1);
      debug(line->arg2);
      debug("---");
   }
}

int pc; //program counter
int acc; //accumulator register
int bck; //backup register

void lcdWriteDecimal(int value){
  static char temp[6];
  sprintf(&temp[0], "%03d ", value); //"-999 "
  temp[4]='\0';
  lcd.write(temp);
}

void displayCurrentCommand(){
  static char temp[17];
  sprintf(temp, "%-16s", program[pc]);
  lcd.setCursor(0,0);
  lcd.write(temp);
}

void displayRegisterValues(){
  lcd.setCursor(4,1);
  lcdWriteDecimal(acc);
  lcd.setCursor(12,1);
  lcdWriteDecimal(bck);
}

void displayTheMode(){
  lcd.setCursor(15, 0);
  lcd.write(mode);
}

void moveNextCommand(){
    do {
      pc=(pc+1)%MAX_LINES;
    } while(program_lines[pc].cmd == NULL);
}

bool startsWith(const char *str, const char *pre) {
    return strncmp(pre, str, strlen(pre)) == 0;
}

int serialReadInt(){
    setReadMode();
    Serial.write("Input required:");
    static char readString[15];
    int idx = 0;
    while (idx < 2 || readString[idx-1] != '\n') {
      delay(3);  //delay to allow buffer to fill
      if (Serial.available() >0) {
        readString[idx++] = Serial.read();
      }
    }
    readString[idx-1] = '\0';
    debug(readString);
    return atoi(readString);
}

int evaluate(char *arg){
  if(strcmp(arg, "UP")==0){
    return clamp(serialReadInt());
  }
  if(strcmp(arg, "ACC")==0){
    return acc;
  }
  return clamp(atoi(arg));
}

void executeMov(){
  line *line = &program_lines[pc];
  int inputValue = evaluate(line->arg1);
  char *dest = line->arg2;
  if(strcmp(dest, "ACC")==0){
    acc = inputValue;
    return;
  }
  if(strcmp(dest, "DOWN")==0){
    setWriteMode();
    char temp[5];
    sprintf(temp, "%03d\n", inputValue);
    Serial.write(temp);
    return;
  }
}

void setRunMode1(){
  mode[0] = '>';
  displayTheMode();
}

void setRunMode2(){
  mode[0] = 255;
  displayTheMode();
}

void setReadMode(){
  mode[0] = 'R';
  displayTheMode();
}

void setWriteMode(){
  mode[0] = 'W';
  displayTheMode();
}

int clamp(int val){
   if(val > 999) return 999;
   if(val < -999) return -999;
   return val;
}

void executeAdd(char *arg){
   acc = clamp(acc + evaluate(arg));
}

void executeSub(char *arg){
   acc = clamp(acc - evaluate(arg));
}

void executeSwp(){
   int temp = bck;
   bck = acc;
   acc = temp;
}

bool executeJmp(char *label){
  for(int i=0;i<MAX_LINES;i++){
    char *lineLabel = program_lines[i].id;
    if(lineLabel != NULL && strcmp(lineLabel, label)==0){
      pc = i;
      return true;
    }
  }
  return false;
}

bool executeCommand(){
  line *line = &program_lines[pc];
  if(strcmp(line->cmd, "MOV")==0){
    executeMov();
    return false;
  }
  if(strcmp(line->cmd, "ADD")==0){
    executeAdd(line->arg1);
    return false;
  }
  if(strcmp(line->cmd, "SUB")==0){
    executeSub(line->arg1);
    return false;
  }
  if(strcmp(line->cmd, "JMP")==0){
    return executeJmp(line->arg1);
  }
  if(strcmp(line->cmd, "JEZ")==0){
    if(acc==0)
      return executeJmp(line->arg1);
    return false;
  }
  if(strcmp(line->cmd, "JNZ")==0){
    if(acc!=0)
      return executeJmp(line->arg1);
    return false;
  }
  if(strcmp(line->cmd, "JGZ")==0){
    if(acc>0)
      return executeJmp(line->arg1);
    return false;
  }
  if(strcmp(line->cmd, "JLZ")==0){
    if(acc<0)
      return executeJmp(line->arg1);
    return false;
  }
  if(strcmp(line->cmd, "SWP")==0){
    executeSwp();
    return false;
  }

  return false;
}

void loop() {
  //digitalWrite(LED_BUILTIN, HIGH);   // turn the LED on (HIGH is the voltage level)
  //delay(1000);                       // wait for a second
  //digitalWrite(LED_BUILTIN, LOW);    // turn the LED off by making the voltage LOW
  //delay(1000);                       // wait for a second
    displayCurrentCommand();
    displayRegisterValues();
    setRunMode1();
    delay(500);
    bool jumped = executeCommand();
    if(!jumped)
      moveNextCommand();
    setRunMode2();
    displayRegisterValues();
    delay(500);
}