#include <LiquidCrystal.h>

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

int row = 0;
int line = 0;
bool controller = false;
int temp = 0;
int ignore = 0;

void setup() {
  Serial.begin(115200);
  // set up the LCD's number of columns and rows:
  lcd.begin(40, 4);
  lcd.clear();
  lcd.home();
}

#define LCDSERIALIZER_LCDI 0xFE

byte serial_getch() {
  int incoming;
  while (Serial.available() == 0) {
  }
  // read the incoming byte:
  incoming = Serial.read();
  //Serial.write(incoming);

  return (incoming & 0xff);
}

void loop() {
  byte rxbyte = serial_getch(); // Fetch byte

  if (rxbyte == LCDSERIALIZER_LCDI) // If LCD instruction
  {
    //Serial.write("[CMD]");

    rxbyte = serial_getch(); // Fetch byte

    if (rxbyte == 0x01)
    {
      Serial.println("Clear screen");
    }
    else if (rxbyte == 0x0c)
    {
      Serial.println("Restore the display (with cursor hidden)");
    }
    else if (rxbyte == 0x08)
    {
      Serial.println("Blank the display (without clearing)");
    }
    else if (rxbyte == 0x0c)
    {
      Serial.println("Make cursor invisible");
    }
    else if (rxbyte == 0x0f)
    {
      Serial.println("Turn on visible blinking-block cursor");
    }
    else if (rxbyte == 0x0e)
    {
      Serial.println("Turn on visible underline cursor");
    }
    else if (rxbyte == 0x10)
    {
      Serial.println("Move cursor one character left");
    }
    else if (rxbyte == 0x14)
    {
      Serial.println("Move cursor one character right");
    }
    else if (rxbyte == 0x15)
    {
      Serial.println("(15)Move cursor one character right");
    }
    else if (rxbyte == 0x02)
    {
      Serial.println("Home (move cursor to top/left character position)");
    }
    else if (rxbyte == 0x18)
    {
      Serial.println("Scroll display one character left (all lines)");
    }
    else if (rxbyte == 0x1e)
    {
      Serial.println("Scroll display one character right (all lines)");
    }
    else if (rxbyte == 0x20)
    {
      Serial.println("Function set (4-bit interface, 1 line, 5*7 Pixels)");
    }
    else if (rxbyte == 0x28)
    {
      Serial.println("Function set (4-bit interface, 2 lines, 5*7 Pixels)");
    }
    else if (rxbyte == 0x30)
    {
      Serial.println("Function set (8-bit interface, 1 line, 5*7 Pixels)");
    }
    else if (rxbyte == 0x38)
    {
      Serial.println("Function set (8-bit interface, 2 lines, 5*7 Pixels)");
    }
    else if (rxbyte & 0x80)
    {
      byte address = rxbyte & 0x7f;

      row = address <= 0x27 ? 0 : 1;
      line = address <= 0x27 ? address : address - 64;

      Serial.print("Address received " + String(row) + "," + String(line) + " \t");

      if (controller)
        row = row + 2;

      if (row == 1 && line == 0)
        controller = true;
      else if (row == 3)
        controller = false;

      lcd.setCursor(line, row);

      Serial.println("Set Cursor to " + String(row) + "," + String(line));

      return;
    }
    else if (rxbyte & 0x40)
    {
      //Serial.println("Set pointer in character-generator RAM (CG RAM address)");
      Serial.println();

      // uhh?
      //ignore = 8;

      lcd.setController(0);
      lcd.command(rxbyte);
      lcd.setController(1);
      lcd.command(rxbyte);

      // get custom char
      for (int i = 0; i < 8; i++)
      {
        rxbyte = serial_getch(); // Fetch byte
        lcd.setController(0);
        lcd.write(rxbyte);
        lcd.setController(1);
        lcd.write(rxbyte);
      }

      return;
    }
    else
    {
      // print byte.
      String thisString = String(rxbyte, HEX);
      Serial.println("UNKNOWN " + thisString);
    }

    //if (rxbyte != 0x80 && !(rxbyte & 0x40))
    lcd.command(rxbyte);
  }
  else
  {
    // print byte.
    //String thisString = String(rxbyte, HEX);
    //Serial.println(thisString);
    //Serial.println("position " + String(row) + " " + String(line));


    //if (ignore == 0)
    //{
    lcd.write(rxbyte);
    line++;
    //}
    /*else
    {
      lcd.setController(0);
      lcd.write(rxbyte);
      lcd.setController(1);
      lcd.write(rxbyte);
      ignore--;
    }*/

    /*if (line > 39)
    {
      row = row < 3 ? row + 1 : 0;
      line = 0;
      Serial.println("row increased by line count");
    }
    lcd.setCursor(line, row);*/
  }
}