#define LATCH 2
#define CLOCK 3
#define DATA 4

#define LATCH_O 5
#define CLOCK_O 6
#define SPEED 7

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

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

uint8_t cnt;
static uint8_t input[3], stored_data[5], idx_debug, mask_debug;
volatile static uint8_t mask, idx, mouse_speed, mouse_update;
bool bit_state, speed_set;
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);

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

  resetData();
  attachInterrupt(digitalPinToInterrupt(LATCH), enterLatch, RISING);
  idx = 4;

  delay(10);
  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 loop() {
  //READ DATA FROM MOUSE
  if (idx > 3) {
    noInterrupts();
    // DUMMY MOUSE DATA
    data.idx1 = 0b11000001;
    data.idx2 = 0b10111000;
    data.idx3 = 0b00100010;
    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
}