#define LATCH 2
#define CLOCK 3

#define DATA 4
#define LATCH_O 5
#define CLOCK_O 6

#define R_BUTTON 7
#define L_BUTTON 8

#define SPEED 9

#define Y_AXIS 0
#define X_AXIS 1

#define MAX_ACCEL 511
#define MIN_ACCEL -512

// Max size of this struct is 32 bytes - NRF24L01 buffer limit
struct Data_Package {
  byte idx1 = 0x1; //last bit set to 1 indicates presence
  byte idx2 = 0x1;
  byte idx3 = 0x1;
};

Data_Package data; //Create a variable with the above structure

uint8_t cnt;
static uint8_t input[3], stored_data[5], mouse_Y_data, mouse_X_data, idx_debug, mask_debug;
volatile static uint8_t mask, idx, mouse_speed, mouse_update;
int16_t ac_X, ac_Y;
bool bit_state, speed_set, mouse_Y_direction, mouse_X_direction;
volatile bool mouse_updated;
unsigned long waitTime;

void resetData() {
  // Reset the values when there is no radio connection - Set initial default values
  data.idx1 = 0x1;
  data.idx2 = 0x0;
  data.idx3 = 0x0;
}

void setup() {
  //initiate pin input
  pinMode(LATCH, INPUT_PULLUP);
  pinMode(CLOCK, INPUT_PULLUP);
  pinMode(DATA, OUTPUT);

  pinMode(LATCH_O, OUTPUT);
  pinMode(CLOCK_O, OUTPUT);
  pinMode(SPEED, INPUT_PULLUP);

  pinMode(R_BUTTON, INPUT_PULLUP);
  pinMode(L_BUTTON, INPUT_PULLUP);
  pinMode(Y_AXIS, INPUT);
  pinMode(X_AXIS, INPUT);

  digitalWrite(LATCH_O, HIGH);
  digitalWrite(CLOCK_O, HIGH);

  resetData();
  attachInterrupt(digitalPinToInterrupt(LATCH), enterLatch, RISING);
  idx = 4;
  
  delay(5);
  Serial.begin(115200);
}

void printSerialData() {
  // DEBUG
  for (idx_debug = 0; idx_debug < 4; idx_debug++) {
    for (mask_debug = 0x80; mask_debug > 0x00; mask_debug >>= 1) {
      if (stored_data[idx_debug] & mask_debug) Serial.print(1); else Serial.print(0);
    }
    Serial.print(" ");
  }
  Serial.println(" ");
}

void enterLatch() {
  // SET FIRST BIT
  if (idx > 3) {
    mask = 0x80;
    idx = 0;
  }
  if (!idx) digitalWrite(DATA, HIGH);
  
  // ALLOW SENSIVITY TO BE CHANGED
  attachInterrupt(digitalPinToInterrupt(CLOCK), setSensivity, FALLING);
  attachInterrupt(digitalPinToInterrupt(LATCH), exitLatch, FALLING);
}

void exitLatch() {
  attachInterrupt(digitalPinToInterrupt(CLOCK), setupData, FALLING);
  attachInterrupt(digitalPinToInterrupt(LATCH), enterLatch, RISING);
}

void setSensivity() {
  // SENSIVITY SETUP
  if (digitalRead(LATCH) == HIGH) {
    digitalWrite(DATA, HIGH);
    mouse_update++;
  }
  if (mouse_update > 30) {
    mouse_speed += 0x10;
    if (mouse_speed > 0x20) mouse_speed = 0x00;
    mouse_update = 0;
    attachInterrupt(digitalPinToInterrupt(CLOCK), sendData, RISING);
  }
}

void sendData() {
  // SEND DATA TO CONSOLE
  digitalWrite(DATA, !(stored_data[idx] & mask));
  attachInterrupt(digitalPinToInterrupt(CLOCK), setupData, FALLING);
}

void setupData() {
  // SHIFT BIT MASK AND INDEX
  mask >>= 1;
  if (!mask) {
    mask = 0x80;
    idx++;
  }

  // STORE DATA
  switch (idx) {
    case 0: // SET DATA TO HIGH STATE
      bit_state = 0;
      break;
    case 1: // STORE BUTTON DATA
      bit_state = (input[0] | mouse_speed) & mask;
      break;
    case 2: // STORE Y AXIS DATA
      bit_state = input[1] & mask;
      break;
    case 3: // STORE X AXIS DATA
      bit_state = input[2] & mask;
      break;
    default: // SET DATA TO LOW STATE, UPDATE MOUSE
      bit_state = 1;
      break;
  }
  stored_data[idx] |= bit_state * mask;
  if (!bit_state) stored_data[idx] &= 0xFF - mask;

  attachInterrupt(digitalPinToInterrupt(CLOCK), sendData, RISING);
}

void getAxisData() {
  ac_X = analogRead(X_AXIS) + MIN_ACCEL;
  ac_Y = analogRead(Y_AXIS) + MIN_ACCEL;
}

void mapAxisData() {
  if (ac_Y < 0) {
    mouse_Y_data = map(ac_Y, 0, MIN_ACCEL, 0x00, 0x7F);
    if (ac_Y < MIN_ACCEL) mouse_Y_data = 0x7F;
    mouse_Y_direction = 0; //UP
  }
  else if (ac_Y > 0) {
    mouse_Y_data = map(ac_Y, 0, MAX_ACCEL, 0x00, 0x7F);
    if (ac_Y > MAX_ACCEL) mouse_Y_data = 0x7F;
    mouse_Y_direction = 1; //DOWN
  }
  else mouse_Y_data = 0x00;
  if (ac_X < 0) {
    mouse_X_data = map(ac_X, 0, MIN_ACCEL, 0x00, 0x7F);
    if (ac_X < MIN_ACCEL) mouse_X_data = 0x7F;
    mouse_X_direction = 0; //LEFT
  }
  else if (ac_X > 0) {
    mouse_X_data = map(ac_X, 0, MAX_ACCEL, 0x00, 0x7F);
    if (ac_X > MAX_ACCEL) mouse_X_data = 0x7F;
    mouse_X_direction = 1; //RIGHT
  }
  else mouse_X_data = 0x00;
}

void packData() {
  data.idx1 = !digitalRead(L_BUTTON) << 7;
  data.idx1 |= !digitalRead(R_BUTTON) << 6;
  data.idx1 |= 0x1; //last bit set to 1 indicates presence

  data.idx2 = mouse_Y_data;
  data.idx3 = mouse_X_data;

  data.idx2 |= mouse_Y_direction << 7;
  data.idx3 |= mouse_X_direction << 7;
}

void loop() {
  //READ DATA FROM MOUSE
  if (idx > 3) {
    getAxisData();
    mapAxisData();
    packData();
    noInterrupts();
    input[0] = data.idx1;
    input[1] = data.idx2;
    input[2] = data.idx3;
    interrupts();
  }

  // CONSOLE EMULATION
  waitTime = micros() + 16639; //start time

  // SET LATCH
  digitalWrite(LATCH_O, HIGH);
  delayMicroseconds(12);
  digitalWrite(LATCH_O, LOW);
  delayMicroseconds(12);

  // FIRTS 16 BIT CLOCK CYCLE
  cnt = 0;
  while(cnt < 16) {
    digitalWrite(CLOCK_O, LOW);
    delayMicroseconds(6);
    digitalWrite(CLOCK_O, HIGH);
    delayMicroseconds(6);
    cnt++;
  }
  delay(1);

  // SENSIVITY SETUP
  if (digitalRead(SPEED) == LOW) {
    if (!speed_set) {
      cnt = 0;
      while(cnt < 31) {
        digitalWrite(LATCH_O, HIGH);
        delayMicroseconds(1);
        digitalWrite(CLOCK_O, LOW);
        delayMicroseconds(1);
        digitalWrite(CLOCK_O, HIGH);
        delayMicroseconds(1);
        digitalWrite(LATCH_O, LOW);
        delayMicroseconds(1);
        cnt++;
      }
      speed_set = 1;
    }
  }
  else speed_set = 0;

  delay(2);
  // SECOND 16 BIT CLOCK CYCLE
  cnt = 0;
  while(cnt < 16) {
    digitalWrite(CLOCK_O, LOW);
    delayMicroseconds(5);
    digitalWrite(CLOCK_O, HIGH);
    delayMicroseconds(8);
    cnt++;
  }
  printSerialData();
  while (micros() < waitTime); // wait until next latch
}