#include <Wire.h>
#include "encoder.h"

constexpr auto kSlaveAddress = 0x08;       // I2C address for Arduino
constexpr auto kXEncoderClockPin = 2;
constexpr auto kXEncoderDirectionPin = 8;
constexpr auto kZEncoderClockPin = 3;
constexpr auto kZEncoderDirectionPin = 9;

static void x_isr(void);
static void z_isr(void);
static void sendData(void);

static RotaryEncoder x_encoder{"X", kXEncoderClockPin, kXEncoderDirectionPin, INPUT};
static RotaryEncoder z_encoder{"Z", kZEncoderClockPin, kZEncoderDirectionPin, INPUT_PULLUP};
static char data[4] = {0, 0, 0, 0};

void setup(){
  //Serial.begin(38400);
  //Serial.setTimeout(500);
  //Serial.println("setup()");

  Wire.begin(kSlaveAddress);
  Wire.onRequest(sendData);

  attachInterrupt(digitalPinToInterrupt(kXEncoderClockPin), x_isr, RISING);
  attachInterrupt(digitalPinToInterrupt(kZEncoderClockPin), z_isr, RISING);
}
void loop() {
  /*Serial.println(x_encoder.getPosition());
  Serial.println(z_encoder.getPosition());
  delay(250);*/
}

// Handle request to send I2C data
void sendData() { 
  int x_pos = x_encoder.getPosition();
  int z_pos = z_encoder.getPosition();
  
  data[0] = 0x00ff & x_pos;
  data[1] = (0xff00 & x_pos) >> 8;
  
  data[2] = 0x00ff & z_pos;
  data[3] = (0xff00 & z_pos) >> 8;


  Wire.write(data, 4);
}

static void x_isr(void)
{
  x_encoder.update();
}

static void z_isr(void)
{
  z_encoder.update();
}