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

using namespace admux;

bool operator ==(JoyReport a, JoyReport b){
  for (uint8_t i=0; i < JOYSTICK_NUM_AXES; i++) {
    if (a.axis[i] != b.axis[i]) return false;
  }
  for (uint8_t i=0; i < JOYSTICK_NUM_BYTES; i++) {
    if (a.button[i] != b.button[i]) return false;
  }
  return true;
}

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

Joystick::Joystick(bool debug) {
  _debug = debug;
  _virtual_buttons = 0;
  _num_axes = 0;
  _num_mux = 0;
  _num_buttons = 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);
}

void Joystick::AddButton(uint8_t pin, ButtonType type, bool pullup) {
  _BuildButton(pin, type, pullup);
}

void Joystick::AddMuxButton(uint8_t pin, uint8_t mux_id, uint8_t mux_channel, ButtonType type, bool pullup) {
  Button *button = _BuildButton(pin, type, pullup);
  button->mux = true;
  button->mux_id = mux_id;
  button->mux_channel = mux_channel;
}

Button* Joystick::_BuildButton(uint8_t pin, ButtonType type, bool pullup) {
  uint8_t mode;
  if (pullup) mode = INPUT_PULLUP;
  else mode = INPUT;

  Button button;
  button.type = type;
  button.inverted = pullup;
  button.bouncer.attach(pin, mode);
  uint8_t increment = 1;
  button.vbutton0 = _virtual_buttons;

  if (type == BUTTON_PULSED_DOUBLE_ACTION_SPLIT) {
    increment = 2;
    button.vbutton1 = _virtual_buttons + 1;
  }

  if (_virtual_buttons + increment > JOYSTICK_NUM_BUTTONS) {
      // todo: fail here
  }

  _buttons[_num_buttons] = button;
  _num_buttons++;
  _virtual_buttons += increment;

  if (type & _BUTTON_PULSED_TYPES) _have_pulsed_button = true;

  return &button;
}

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

uint8_t Joystick::AddMux(uint8_t signal_pin, Pinset addr_pins, bool pullup) {
  uint8_t mux_id = _num_mux;
  uint8_t mode = INPUT_PULLUP;
  if (!pullup) mode = INPUT;
  Mux mux(Pin(signal_pin, mode, PinType::Digital), addr_pins);
  _mux[mux_id] = &mux;
  _num_mux++;
  return mux_id;
}

void Joystick::Update() {
  JoyReport oldReport = _joyReport;
  
  for (uint8_t i = 0; i < _num_buttons; i++) {
    _UpdateButton(i);
  }

  for (uint8_t 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;
}

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;
}

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] &= ~(1 << bit);
}

void Joystick::ReleaseAllButtons() {
  for (uint8_t i = 0; i < JOYSTICK_NUM_BYTES; i++) {
    _joyReport.button[i] = 0;
  }
}

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

void Joystick::Write() {
  if (!_debug) {
    Serial.write((uint8_t *)&_joyReport, sizeof(JoyReport));
  }
  else {
    Serial.print("DEBUG: Writing data: ");
    for (uint8_t i=0; i < JOYSTICK_NUM_AXES; i++) {
      Serial.print(_joyReport.axis[i]);
      Serial.print(" ");
    }
    for (uint8_t i=0; i < JOYSTICK_NUM_BYTES; i++) {
      Serial.print(_joyReport.button[i]);
      Serial.print(" ");
    }
    Serial.println();
  }
  delay(250);
}

void Joystick::_UpdateButton(uint8_t button_num) {
  Button *button = &_buttons[button_num];
  if (button->mux) {
    _mux[button->mux_id]->channel(button->mux_channel);
  }
  bool changed = button->bouncer.update();
  if (!changed) return;

  bool on = button->bouncer.rose();
  if (button->inverted) on = button->bouncer.fell();

  switch (button->type) {
    case BUTTON_PASSTHRU:
      if (on) PressButton(button->vbutton0);
      else ReleaseButton(button->vbutton0);
      break;
    case BUTTON_PULSED:
      if (on) PressButton(button->vbutton0);
      break;
    case BUTTON_PULSED_DOUBLE_ACTION:
      PressButton(button->vbutton0);
      break;
    case BUTTON_PULSED_DOUBLE_ACTION_SPLIT:
      if (on) PressButton(button->vbutton0);
      else PressButton(button->vbutton1);
      break;
    case BUTTON_LATCHED_MOMENTARY:
      if (on) {
        if (!button->pressed) {
          PressButton(button->vbutton0);
          button->pressed = true;
        } else {
          ReleaseButton(button->vbutton0);
          button->pressed = false;
        }
      }
      break;
    default:
      if (_debug) {
        Serial.print("DEBUG: Unhandled button type: ");
        Serial.println(button->type);
      }
  }
}

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