#include "Joystick.h"
#include "Button.h"
#include "Reader.h"
#include <Mux.h>
#include <Arduino.h>
#include <stdio.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_buttons = 0;

  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("Joystick serial communication initialized.");
}

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

void Joystick::AddMuxButton(uint8_t channel, Mux* mux, ButtonType type, bool pullup) {
  _addButton(type, new MuxReader(channel, mux, pullup));
}

void Joystick::AddMatrixButton(uint8_t row, uint8_t col, Matrix* matrix, ButtonType type, bool pullup) {
  _addButton(type, new MatrixReader(row, col, matrix, pullup));
}

void Joystick::AddEncoder(uint8_t pin1, uint8_t pin2, int8_t tick_threshold, ButtonType type) {
  Button *button;
  switch (type) {
    case ENCODER_PULSED_SPLIT:
      // add an encoder button. _BuildButton() doesn't do everything we need, however...
      button = new EncoderButton(pin1, pin2, _virtual_buttons, tick_threshold);
      _buttons[_num_buttons] = button;
      _num_buttons++;
      _virtual_buttons += 2;
      break;
    default:
      if (_debug) {
        Serial.print("DEBUG: Unhandled encoder type: ");
        Serial.println(type);
      }
  }

  if (_debug) Serial.println("Added an encoder.");
}

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

void Joystick::Update() {
  JoyReport oldReport = _joyReport;
  
  for (uint8_t i = 0; i < _num_buttons; i++) {
    bool changed = _buttons[i]->Update(this);
    if (changed && _debug) {
      char buffer[25];
      sprintf(buffer, "Button %d changed state.", i);
      Serial.println(buffer);
    }
  }

  // TODO: implement this and also refactor it into a class or classes
  for (uint8_t i = 0; i < _num_axes; i++) {
    _UpdateAxis(i);
  }

  if (_joyReport != oldReport) {
    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::Write() {
  if (_debug) {
    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();
    return;
  }

  Serial.write((uint8_t *)&_joyReport, sizeof(JoyReport));
}



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

void Joystick::_addButton(ButtonType type, Reader* reader) {
  Button* button;
  switch (type) {
    case BUTTON_PASSTHRU:
      button = new PassthruButton(_virtual_buttons, reader);
      _virtual_buttons++;
      break;
    case BUTTON_PULSED:
      button = new PulsedButton(_virtual_buttons, reader, false, false);
      _virtual_buttons++;
      break;
    case BUTTON_PULSED_DOUBLE_ACTION:
      button = new PulsedButton(_virtual_buttons, reader, true, false);
      _virtual_buttons++;
      break;
    case BUTTON_PULSED_DOUBLE_ACTION_SPLIT:
      button = new PulsedButton(_virtual_buttons, reader, true, true);
      _virtual_buttons += 2;
      break;
    case BUTTON_LATCHED_MOMENTARY:
      button = new LatchedButton(_virtual_buttons, reader);
      _virtual_buttons++;
      break;
    default:
      return;
  }

  _buttons[_num_buttons] = button;
  _num_buttons++;
  if (_debug) {
    char buffer[100];
    sprintf(buffer, "Added button %d of type %d", _num_buttons - 1, button->type);
    Serial.println(buffer);
  }
}