#ifndef WiiChuck_h
#define WiiChuck_h

#include "Arduino.h" 
#include "Wire.h"
#include "math.h"


// these may need to be adjusted for each nunchuck for calibration
#define ZEROX 510  
#define ZEROY 490
#define ZEROZ 460
#define RADIUS 210  // probably pretty universal

#define DEFAULT_ZERO_JOY_X 124
#define DEFAULT_ZERO_JOY_Y 132

#define MAXANGLE 90
#define MINANGLE -90

class WiiChuck {
private:
  byte cnt;
  uint8_t status[6];		// array to store wiichuck output
  byte averageCounter;
  int i;
  int total;
  uint8_t zeroJoyX;   // these are about where mine are
  uint8_t zeroJoyY; // use calibrateJoy when the stick is at zero to correct
  int lastJoyX;
  int lastJoyY;
  int angles[3];

  boolean lastZ, lastC;


public:

  byte joyX;
  byte joyY;
  boolean buttonZ;
  boolean buttonC;
  boolean plugd;
  void begin() 
  {
    Wire.begin();
    cnt = 0;
    averageCounter = 0;
    Wire.beginTransmission (0x52);	// transmit to device 0x52
    Wire.write((byte)0x40);		// sends memory address
    Wire.write((byte)0x00);		// sends memory address
    Wire.endTransmission ();	// stop transmitting
    update();            
    for (i = 0; i<3;i++) {
      angles[i] = 0;
    }
    zeroJoyX = DEFAULT_ZERO_JOY_X;
    zeroJoyY = DEFAULT_ZERO_JOY_Y;
  }


  void calibrateJoy() {
    zeroJoyX = joyX;
    zeroJoyY = joyY;
  }

  void CheckifPlugd()
  {
    if (this->plugd == false){
      this->begin();
      
#ifdef GLCD
      GLCD.CursorTo(1, 1);
      GLCD.print("No Chuck");
      delay(100);
#endif

      this->begin();
      if (this->plugd)
        this->calibrateJoy(); 
    }
  }

  void update() {

    Wire.requestFrom (0x52, 6);	// request data from nunchuck
    while (Wire.available ()) {
      // receive byte as an integer
      status[cnt] = _nunchuk_decode_byte((byte)Wire.read()); //
      cnt++;
    }
    if (cnt > 5) {
      lastZ = buttonZ;
      lastC = buttonC;
      lastJoyX = readJoyX();
      lastJoyY = readJoyY();

      cnt = 0;
      joyX = (status[0]);
      joyY = (status[1]);
      for (i = 0; i < 3; i++)  
        angles[i] = (status[i+2] << 2) + ((status[5] & (B00000011 << ((i+1)*2) ) >> ((i+1)*2))); 

      buttonZ = !( status[5] & B00000001);
      buttonC = !((status[5] & B00000010) >> 1);
      _send_zero(); // send the request for next bytes
      plugd = true;
    }
    else if (cnt == 0)
      plugd = false;
  }

  float readAccelX() {
    return (float)angles[0] - ZEROX;
  }
  float readAccelY() {
    return (float)angles[1] - ZEROY;
  }
  float readAccelZ() {
    return (float)angles[2] - ZEROZ;
  }

  boolean zPressed() {
    return (buttonZ && ! lastZ);
  }
  boolean cPressed() {
    return (buttonC && ! lastC);
  }

  // for using the joystick like a directional button
  boolean rightJoy(int thresh=60) {
    return (readJoyX() > thresh and lastJoyX <= thresh);
  }

  // for using the joystick like a directional button
  boolean leftJoy(int thresh=60) {
    return (readJoyX() < -thresh and lastJoyX >= -thresh);
  }


  int readJoyX() {
    return (int) joyX - zeroJoyX;
  }

  int readJoyY() {
    return (int)joyY - zeroJoyY;
  }


  // R, the radius, generally hovers around 210 (at least it does with mine)
  int R() {
    return sqrt(readAccelX() * readAccelX() +readAccelY() * readAccelY() + readAccelZ() * readAccelZ());  
  }


  // returns roll degrees
  int readRoll() {
    return (int)(atan2(readAccelX(),readAccelZ())/ M_PI * 180.0);
  }

  // returns pitch in degrees
  int readPitch() {        
    return (int) (acos(readAccelY()/RADIUS)/ M_PI * 180.0);  // optionally swap 'RADIUS' for 'R()'
  }

private:
  byte _nunchuk_decode_byte (byte x)
  {
    x = (x ^ 0x17) + 0x17;
    return x;
  }

  void _send_zero()
  {
    Wire.beginTransmission (0x52);	// transmit to device 0x52
    Wire.write ((byte)0x00);		// sends one byte
    Wire.endTransmission ();	// stop transmitting
  }

};


#endif