#include <math.h>

#include "Wire.h"
#include "WiiChuck.h"

#define MOTION_STOP     (0 << 4)
#define MOTION_FWD      (1 << 4)
#define MOTION_REV      (2 << 4)

#define ANGLE_STRAIGHT  (0 << 2)
#define ANGLE_LEFT      (1 << 2)
#define ANGLE_RIGHT     (2 << 2)

#define BUTTON_C        (1 << 1)
#define BUTTON_Z        (1 << 0)

#define HALF_PI (3.1415 / 2)

WiiChuck chuck = WiiChuck();

void setup() {
  Serial.begin(19200);
  chuck.begin();
  chuck.update();
}

int i;

void loop() {
  delay(50);
  chuck.update();

  int x = chuck.readJoyX();
  int y = chuck.readJoyY();

  uint8_t magnitude = sqrt(sq(x) + sq(y));
  if (magnitude > 100)
    magnitude = 100;

  uint8_t angle = ( 100 * (float) atan((float)abs(x)/(float)abs(y)) / HALF_PI );

  uint8_t flags;

  if (y >= 10)
    flags |= MOTION_FWD;
  else if (y <= -10)
    flags |= MOTION_REV;
  else
    flags |= MOTION_STOP;

  if (x >= 10)
    flags |= ANGLE_RIGHT;
  else if (x <= -10)
    flags |= ANGLE_LEFT;
  else
    flags |= ANGLE_STRAIGHT;

  if (chuck.buttonC)
    flags |= BUTTON_C;
  if (chuck.buttonZ)
    flags |= BUTTON_Z;

  // curly braces have an ascii val > 100, all of the other data is <= 100
  uint8_t packet[6];
  packet[0] = '{';
  packet[1] = magnitude;
  packet[2] = angle;
  packet[3] = flags;
  packet[4] = (magnitude ^ angle ^ flags) & 0b00111111; // checksum
  packet[5] = '}';

  Serial.write(packet, 6);
}