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

uint8_t AcXH, AcXL;

constexpr float Tau = 6.28318530718;

constexpr float mapRange(float input, float inputMin, float inputMax, float outputMin, float outputMax)
{
  return outputMin + (input - inputMin) * ((outputMax - outputMin) / (inputMax - inputMin));
}

constexpr uint8_t screenWidth = WIDTH;
constexpr uint8_t screenHeight = HEIGHT;
constexpr uint8_t halfScreenWidth = WIDTH / 2;
constexpr uint8_t halfScreenHeight = HEIGHT / 2;

Arduboy2 arduboy;

// amplitude is the sine wave's
// distance from the vertical baseline in pixels.
// i.e. the height of the wave is 2 * amplitude
uint8_t amplitude = 20;

// frequency is the number of pixels a full wave spans
// on the horizontal axis.
uint8_t frequency = 16;

// sampleOffset is used to animate the sine wave
uint8_t sampleOffset = 0;

void setup()
{
  arduboy.boot();

  TCCR1B = (bit(WGM32) | bit(CS31)); // CTC mode. Divide by 8 clock prescale

  // Initialize MPU-6050
  Wire.begin();
  Wire.beginTransmission(0x69);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);
}

// previousX and previousY are used for drawing
// the lines that make up the sine waves.
// They represent the endpoint of the previous line.
uint8_t previousX = 0;
uint8_t previousY = halfScreenHeight;

void loop()
{
  Wire.beginTransmission(0x69); // I2C address of the MPU-6050
  Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(true);
  Wire.requestFrom(0x69,2,true);
  AcXH = Wire.read(); // 0x3B (ACCEL_XOUT_H)
  AcXL = Wire.read(); // 0x3C (ACCEL_XOUT_L)

  OCR1A = AcXH<<8|AcXL; // load the count (16 bits), which determines the frequency
  frequency = (127 & AcXH);

  arduboy.pollButtons();

  if (arduboy.justPressed(UP_BUTTON))
    TCCR1A = bit(COM1A0); // set toggle on compare mode (which connects the pin)

  if (arduboy.justPressed(DOWN_BUTTON))
    TCCR1A = 0; // set normal mode (which disconnects the pin)

  // x represents the x coordinate of the current screen pixel
  // This is used as an offset for sampling sine
  for(uint8_t x = 0; x < screenWidth; ++x)
  {
    // theta is derived by mapping the sampleOffset
    // from a (0, frequency) range to a (0, Tau) range.
    float theta = mapRange((sampleOffset + x) % frequency, 0, frequency, 0, Tau);

    double y = halfScreenHeight + (-amplitude * sin(theta));

    if (x >= previousX)
      arduboy.drawLine(previousX, previousY, x, y, WHITE);

    previousX = x;
    previousY = y;
  }

  ++sampleOffset;
  sampleOffset %= frequency;

  arduboy.display(CLEAR_BUFFER);
}