#include <Wire.h>

bool was_pressed = false;

void setup()
{
  Wire.begin();
  Serial.begin(9600);
  delay(600);
  sendShort(944, 15);
  sendShort(944, 12);
  sendShort(944, 17);
  sendShort(944, 18);
  delay(1500);
  sendShort(0, 15);
  sendShort(0, 12);
  sendShort(0, 17);
  sendShort(0, 18);
  delay(1500);
}

uint32_t data_send;
int pageNums[4];
uint32_t counter = 10;
int screenNum = 0;
int pageNum = 0;
int gearNum = 5;

void loop()
{
  if (Serial.available()) {
    char c = Serial.read();
    if (c == 'U') {
      pageNums[screenNum] = (pageNums[screenNum] + 1) % 9;
    }
    else if (c == 'D') {
      pageNums[screenNum] = (((pageNums[screenNum] - 1) % 9) + 9) % 9;
    }
    else if (c == 'L') {
      screenNum = (((screenNum - 1) % 4) + 4) % 4;
    }
    else {
      screenNum = (screenNum + 1) % 4;
    }
    was_pressed = true;
  }

  // MENU SCREEN
  if (was_pressed) {
    sendShort(1000 + 100 * gearNum + 10 * pageNums[screenNum] + screenNum, 10);
  }
  else {
    sendShort(100 * gearNum + 10 * pageNums[screenNum] + screenNum, 10);
  }
  was_pressed = false;

  // 4 SCREENS
  sendLong(data_send + ((uint32_t) pageNums[0]) * ((uint32_t) 10000), 19);
  sendLong(data_send + ((uint32_t) pageNums[3]) * ((uint32_t) 10000), 20);
  sendLong(data_send + ((uint32_t) pageNums[2]) * ((uint32_t) 10000), 21);
  sendLong(data_send + ((uint32_t) pageNums[1]) * ((uint32_t) 10000), 22);

  // MOTORS
  sendShort(data_send % 600, 15);
  sendShort(data_send % 600, 12);
  sendShort(data_send % 600, 17);
  sendShort(data_send % 600, 18);
  sendShort(data_send % 600, 16);
  sendShort(data_send % 600, 11);

  // REV LIGHTS
  sendShort(data_send, 13);

  delay(60);
  data_send = (data_send + 10) % 7300;
}

void sendShort(short x, int address) {
  Wire.beginTransmission(address);
  Wire.write((byte) x);
  Wire.write((byte) (x >> 8));
  Wire.endTransmission();
}

void sendLong(long x, int address) {
  Wire.beginTransmission(address);
  Wire.write((byte) x);
  Wire.write((byte) (x >> 8));
  Wire.write((byte) (x >> 16));
  Wire.write((byte) (x >> 24));
  Wire.endTransmission();
}