#include "ThrottleQuadrant.hpp"

#include <limits.h>

#include <Arduino.h>

auto SaitekThrottleQuadrant::beginTransaction() -> void {
  noInterrupts();
  digitalWrite(latch_pin, LOW);
  delayMicroseconds(latchDelayMicros);
}

auto SaitekThrottleQuadrant::endTransaction() -> void {
  digitalWrite(latch_pin, HIGH);
  delayMicroseconds(latchDelayMicros);
  interrupts();
}

template<typename T, size_t Bits>
auto SaitekThrottleQuadrant::doRead() -> T {
  auto value = T{};

  for (auto i = 0u; i < Bits; ++i) {
    digitalWrite(clock_pin, LOW);
    delayMicroseconds(clockDelayMicros);

    value |= digitalRead(data_pin) << i;

    digitalWrite(clock_pin, HIGH);
    delayMicroseconds(clockDelayMicros);
  }

  return value;
}

auto SaitekThrottleQuadrant::begin(unsigned clock_pin, unsigned latch_pin, unsigned data_pin) -> void {
  clock_pin = clock_pin;
  latch_pin = latch_pin;
  data_pin = data_pin;

  pinMode(clock_pin, OUTPUT);
  pinMode(latch_pin, OUTPUT);
  pinMode(data_pin, INPUT);
}

auto SaitekThrottleQuadrant::read() noexcept -> Data {
  auto result = Data{};

  beginTransaction();

  result.left = doRead<uint8_t>();
  result.middle = doRead<uint8_t>();
  result.right = doRead<uint8_t>();
  result.buttons = doRead<uint16_t, numberOfButtons>();

  endTransaction();

  return result;
}