#include <SoftwareSerial.h>

// TX/RX
SoftwareSerial Bluetooth(10, 9);

// Button pins
int const UP_BTN = 2;
int const DOWN_BTN = 4;
int const LEFT_BTN = 5;
int const RIGHT_BTN = 3;
int const E_BTN = 6;
int const F_BTN = 7;
int const JOYSTICK_BTN = 8;
int const JOYSTICK_AXIS_X = A0;
int const JOYSTICK_AXIS_Y = A1;
int buttons[] = {UP_BTN, DOWN_BTN, LEFT_BTN, RIGHT_BTN, E_BTN, F_BTN, JOYSTICK_BTN};

bool halt_packets = false;

void setup() {
  Serial.begin(38400);
  Serial.println("Serial Started");
  for (int i=0; i < 7; i++)  pinMode(buttons[i], INPUT_PULLUP);
  Serial.println("Finished pin setup");
  Bluetooth.begin(38400);
  Serial.println("Send 's' to sniff packets, 'h' to halt packets:");
  delay(2000);
}

unsigned long pack = 0;

void loop() {
  // Joystick range is from 0 to 1023
  // A deadzone of 10 or 20 should be safe.
  if (!halt_packets) {
    pack = build_packet();
  }
  delay(100);

  // Read from HC-05 and send data to the Arduino Serial Monitor
  if (Bluetooth.available()) {
    if (!halt_packets) {
      Bluetooth.write(pack);
    }
    Serial.write(Bluetooth.read());
  }
  else {
    //Serial.println("Bluetooth unavailable, waiting 2 secs");
    //delay(2000);
  }

  // Read from Arduino Serial Monitor and send it to the HC-05
  if (Serial.available()) {
    char com = Serial.read();
    Serial.write(com);
    if (com == 's' && !halt_packets) {
      Serial.println();
      PrintBits32(pack);
    }
    else if (com == 'h') {
      Serial.println();
      Serial.println("!!! Packet generation halted !!!");
      halt_packets = !halt_packets;
    }
    Bluetooth.print(com);
  }
  
}

unsigned long build_packet(void) {
  /*
   * Packets will always contain analog values (10 bits each)
   * One bit containing high/low value for buttons is included
   */
  unsigned long built_packet = 0;
  // Start digital input
  built_packet += digitalRead(UP_BTN);
  built_packet |= digitalRead(DOWN_BTN) << 1;
  built_packet += digitalRead(LEFT_BTN) << 2;
  built_packet += digitalRead(RIGHT_BTN) << 3;
  built_packet += digitalRead(E_BTN) << 4;
  built_packet += digitalRead(F_BTN) << 5;
  built_packet += digitalRead(JOYSTICK_BTN) << 6;
  // Start analog input
  built_packet = built_packet << 10;
  built_packet += analogRead(JOYSTICK_AXIS_X);
  built_packet = built_packet << 10;
  built_packet += analogRead(JOYSTICK_AXIS_Y);
  return built_packet;
}

void PrintBits32(unsigned long u) {
  char str[34];
  str[32] = '\n';
  str[33] = 0;
  for (int i = 31; i >= 0; i--)
  {
    str[i] = '0' + (u & 1);
    u >>= 1;
  }

  Serial.print(str);
}