#define DATA_PIN 10
#define LATCH_PIN 11
#define CLOCK_PIN 12
#define RS_PIN 9
#define EN_PIN 8
#define SCREENSIZE_X 20
#define SCREENSIZE_Y 4

#include "cgrom.h"

const static int internal_size_y = 4;
const static int internal_size_x = 20;

int screenCursor = 0;
bool cursor_in_ddram = 1;

char ddram[internal_size_x * internal_size_y] = {};
char cgram[64] = {};

auto printBin = [](byte a){
  for(int i = 0; i < 8; ++i){
    Serial.print(a & 0x80 ? '#' : ' ');
    a <<= 1;
  }
};
const char * charAt(int idx){
  static const char def[8] = {};
  if(idx < 8) return &cgram[idx << 3];
  if(idx >= 16 && idx < 128) return &cgrom_low[(idx - 32) << 3];
  if(idx >= 160 && idx < 256) return &cgrom_high[(idx - 160) << 3];
  return def;
}


#define VERT_PIN A0
#define HORZ_PIN A1

int v, h, x = 1, y = 1;

byte customCharR[] = {
  B00000,
  B00000,
  B01110,
  B11110,
  B11000,
  B11110,
  B01110,
  B01010
};
byte customCharL[] = {
  B00000,
  B00000,
  B01110,
  B01111,
  B00011,
  B01111,
  B01110,
  B01010
};

void initializeLCD() {
  lcdCommand(0x30);
  lcdCommand(0xC);
  lcdCommand(0x01);
  delay(3);
}
#define clamp(_val, _min, _max) (min(max(_val, _min), _max))

void lcdShift(byte command) {
  shiftOut(DATA_PIN, CLOCK_PIN, MSBFIRST, command);
  digitalWrite(LATCH_PIN, HIGH);
  digitalWrite(LATCH_PIN, LOW);
  digitalWrite(EN_PIN, HIGH);
  delayMicroseconds(1);
  digitalWrite(EN_PIN, LOW);
  delayMicroseconds(100);
}
void lcdCommand(byte command) {
  digitalWrite(RS_PIN, LOW);
  lcdShift(command);
}
void lcdData(byte command) {
  digitalWrite(RS_PIN, HIGH);
  lcdShift(command);
}

void updateChar(char addr, char arr[]){
  cursor_in_ddram = 0;
  screenCursor = addr<<3;
  lcdCommand(0x40 | addr << 3);
  for(auto i = 0; i < 8; ++i){
    cgram[screenCursor++] = arr[i];
    lcdData(arr[i]);
  }
}

void displayGoto(short X, short Y, bool commit = true) {
  int lut[] = {0, 2, 1, 3};
  cursor_in_ddram = 1;
  screenCursor = X+internal_size_x*lut[Y];
  if(commit)lcdCommand(0x80 + screenCursor);
}

void displayString(char data[], int len, bool commit = true, bool modify = true) {
  for(auto i = 0; i < len; ++i){
    if(modify)(cursor_in_ddram ? ddram : cgram)[screenCursor++] = data[i];
    if(commit)lcdData(data[i]);
  }
}
void commit(){
  displayGoto(0,0);
  displayString(ddram, sizeof ddram, true, false);
}
void renderPic(short x, short y, bool flip, bool lr){
  short majorX = x / 5;
  short majorY = y / 8;
  short minorX = x - majorX*5;
  short minorY = y - majorY*8;
  char topLeft[8] = {}, topRight[8] = {};
  char bottomLeft[8] = {}, bottomRight[8] = {};
  
  auto memcpyInBounds = [&](short X, short Y, char* data){
    int lut[] = {0, 2, 1, 3};
    if (X < internal_size_x && Y < internal_size_y){
      auto S = charAt(ddram[X + lut[Y] * internal_size_x]);
      for(int i = 0; i < 8; ++i) data[i] = S[i] >> 3;
    }
  };

  memcpyInBounds(majorX, majorY, topLeft);
  memcpyInBounds(majorX + 1, majorY, topRight);
  memcpyInBounds(majorX, majorY + 1, bottomLeft);
  memcpyInBounds(majorX + 1, majorY + 1, bottomRight);

  auto ch = lr ? customCharL : customCharR;
  for(int i = 0, j = minorY; j < 8; ++i, ++j){
    topLeft[j] |= ch[i] >> minorX;
    topRight[j] |= ch[i] << (5 - minorX);
  }
  for(int i = 0; i < minorY; ++i){
    bottomLeft[i] |= ch[i + 8-minorY] >> minorX;
    bottomRight[i] |= ch[i + 8-minorY] << (5 - minorX);
  }
  int flipOffset = flip ? 4 : 0;
  updateChar(0 + flipOffset, topLeft);
  updateChar(1 + flipOffset, topRight);
  updateChar(2 + flipOffset, bottomLeft);
  updateChar(3 + flipOffset, bottomRight);
  char dataString[internal_size_x * internal_size_y];
  for(auto& c: dataString) c = '1';
  auto writeInBounds = [&](short X, short Y, char data){
    if (X < internal_size_x && Y < internal_size_y){
      displayGoto(X,Y, false);
      ddram[screenCursor] = data;
    }
  };
  writeInBounds(majorX, majorY, 0 + flipOffset);
  writeInBounds(majorX + 1, majorY, 1 + flipOffset);
  writeInBounds(majorX, majorY + 1, 2 + flipOffset);
  writeInBounds(majorX + 1, majorY + 1, 3 + flipOffset);
  commit();
}

void setup() {
  Serial.begin(9600);
  pinMode(DATA_PIN, OUTPUT);
  pinMode(LATCH_PIN, OUTPUT);
  pinMode(CLOCK_PIN, OUTPUT);
  pinMode(RS_PIN, OUTPUT);
  pinMode(EN_PIN, OUTPUT);
  initializeLCD();
}

void loop() {
  v = map(analogRead(VERT_PIN), 0, 1023, 1, -1);
  h = map(analogRead(HORZ_PIN), 0, 1023, 1, -1);

  static bool flip = 0;
  flip ^= true;
  static bool lr = false;
  if(h > 0) lr = false;
  if(h < 0) lr = true;

  x += h;
  y += v;
  y = clamp(y, 0, (SCREENSIZE_Y-1)*8);
  x = clamp(x, 0, (SCREENSIZE_X-1)*5);
  char str[] = "111111111222222222333333333444444444AAAAAAAAAAAAAAAAA4044ABBBBBBBBBBBBCCCCCCC111";
  displayGoto(0,0, false);
  displayString(str, sizeof str - 1, false);
  Serial.print(h);
  Serial.print(' ');
  Serial.println(lr);
  renderPic(x, y, flip, lr);
  
  delay(10);
}