#include "Joystick.h"
#include <Arduino.h>

bool operator ==(JoyReport a, JoyReport b){
  return a.axis == b.axis && a.button == b.button;
}

bool operator !=(JoyReport a, JoyReport b){
  return !(a == b);
}

Joystick::Joystick(bool debug) {
  _debug = debug;
  _num_buttons = 0;
  _num_axes = 0;
  _have_pulsed_button = false;
  
  for (uint8_t i=0; i < JOYSTICK_NUM_AXES; i++) {
    _joyReport.axis[i] = 0;
  }
  for (uint8_t i=0; i < JOYSTICK_NUM_BYTES; i++) {
    _joyReport.button[i] = 0;
  }
}

void Joystick::Init() {
  Serial.begin(115200);
  delay(100);

  if (_debug) Serial.println("DEBUG: Joystick library initialized.");
}

void Joystick::AddButton(uint8_t pin, ButtonType type, bool pullup) {
  if (pullup) pinMode(pin, INPUT_PULLUP);
  else pinMode(pin, INPUT);
  
  _buttons[_num_buttons].pin = pin;
  _buttons[_num_buttons].type = type;
  _buttons[_num_buttons].last_state = digitalRead(pin);
  _buttons[_num_buttons].pullup = pullup;
  _num_buttons++;

  if (type & _BUTTON_PULSED_TYPES) _have_pulsed_button = true;

  if (_debug) {
    Serial.print("Debug: added button of type ");
    Serial.print(type);
    Serial.print(" on digital pin ");
    Serial.println(pin);
  }
}

void Joystick::AddAxis(uint8_t pin) {
  _axes[_num_axes] = pin;
  _num_axes++;

  if (_debug) {
    Serial.print("Debug: added axis on analog pin ");
    Serial.println(pin);
  }
}

void Joystick::Update() {
  JoyReport oldReport = _joyReport;
  
  for (int i = 0; i < _num_buttons; i++) {
    _UpdateButton(_buttons[i], i);
  }

  for (int i = 0; i < _num_axes; i++) {
    _UpdateAxis(i);
  }

  if (_joyReport != oldReport) {
    Write();
    if (_have_pulsed_button) {
      _ReleasePulsedButtons();
      Write();
    }
  }
}

void Joystick::SetAxis(uint8_t axis, int16_t value) {
  if (axis >= JOYSTICK_NUM_AXES) return;
  _joyReport.axis[axis] = value;
  if (_debug) Serial.println("DEBUG: Axis change recorded.");
}

void Joystick::PressButton(uint8_t button) {
  if (button >= JOYSTICK_NUM_BUTTONS) return;
  uint8_t byte = button / 8;
  uint8_t bit = button % 8;
  _joyReport.button[byte] |= 1 << bit;
  if (_debug) Serial.println("DEBUG: Button press recorded.");
}

void Joystick::ReleaseButton(uint8_t button) {
  if (button >= JOYSTICK_NUM_BUTTONS) return;
  uint8_t byte = button / 8;
  uint8_t bit = button % 8;
  _joyReport.button[byte] |= 0 << bit;
  if (_debug) Serial.println("DEBUG: Button release recorded.");
}

void Joystick::ReleaseAllButtons() {
  for (uint8_t i = 0; i < JOYSTICK_NUM_BYTES; i++) {
    _joyReport.button[i] = 0;
  }
  if (_debug) Serial.println("DEBUG: All-button release recorded.");
}

void Joystick::_ReleasePulsedButtons() {
  for (uint8_t i = 0; i < _num_buttons; i++) {
    if (_buttons[i].type & _BUTTON_PULSED_TYPES)
      ReleaseButton(i);
  }

  if (_debug) Serial.println("DEBUG: Pulse button release recorded.");
}

void Joystick::Write() {
  Serial.write((uint8_t *)&_joyReport, sizeof(JoyReport));
  delay(250);
}

void Joystick::_UpdateButton(Button& button, uint8_t index) {
  uint8_t value = digitalRead(button.pin);
  // Treat pullup-resistor inputs as basically backwards.
  if (button.pullup) {
    if (value == LOW) value = HIGH;
    else value = LOW;
  }
    
  switch (button.type) {
    case BUTTON_MAINTAINED:
      if (value == HIGH) PressButton(index);
      else ReleaseButton(index);
      break;
    case BUTTON_PULSED:
      if (value == HIGH && button.last_state == LOW) PressButton(index);
      break;
    case BUTTON_PULSED_DOUBLE_ACTION:
      if (value != button.last_state) PressButton(index);
      break;
    default:
      if (_debug) {
        Serial.print("DEBUG: Unhandled button type: ");
        Serial.println(button.type);
      }
  }
  
  button.last_state = value;
}

void Joystick::_UpdateAxis(uint8_t index) {
  if (_debug) Serial.println("STUB: Joystick::_UpdateAxis");
}