/*
 * Nunchuck -- Use a Wii Nunchuck
 * Tim Hirzel http://www.growdown.com
 * 
 * This file is an adaptation of the code by these two authors:
 * Tod E. Kurt, http://todbot.com/blog/
 *
 * The Wii Nunchuck reading code is taken from Windmeadow Labs
 * http://www.windmeadow.com/node/42
 */

#ifndef WiiChuck_h
#define WiiChuck_h

#include <Wire.h>

#define AVERAGE_N 6
#define FAST_WEIGHT 8
#define MIN_ACCEL 290
#define MAX_ACCEL 690

#define XJOYZERO  124
#define YJOYZERO  132


class WiiChuck {    
    private:
        byte cnt;
        uint8_t nunchuckStatus[6];		// array to store wiichuck output
        byte averageCounter;
        //int accelArray[3][AVERAGE_N];  // X,Y,Z
        int i;
        int total;
        byte zeroJoyX;
        byte zeroJoyY;
    int lastJoyX;
    int lastJoyY;
        boolean lastZ, lastC;

    public:
    int angles[3];  // X,Y,Z

        byte joyX;
        byte joyY;
        boolean buttonZ;
        boolean buttonC;

        void begin() 
        {
            cnt = 0;
            averageCounter = 0;
            Wire.beginTransmission (0x52);	// transmit to device 0x52
            Wire.send (0x40);		// sends memory address
            Wire.send (0x00);		// sends memory address
            Wire.endTransmission ();	// stop transmitting
            update();            
        }


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

            Wire.requestFrom (0x52, 6);	// request data from nunchuck
            while (Wire.available ()) {
                // receive byte as an integer
                nunchuckStatus[cnt] = _nunchuk_decode_byte (Wire.receive()); //
                cnt++;
            }
            if (cnt > 5) {
                lastZ = buttonZ;
                lastC = buttonC;
                lastJoyX = readJoyX();
                lastJoyY = readJoyY();
                //averageCounter ++;
                //if (averageCounter >= AVERAGE_N)
                //    averageCounter = 0;
                
                
                cnt = 0;
                joyX = (nunchuckStatus[0]);
                joyY = (nunchuckStatus[1]);
                for (i = 0; i < 3; i++) 
                    //accelArray[i][averageCounter] = ((int)nunchuckStatus[i+2] << 2) + ((nunchuckStatus[5] & (B00000011 << ((i+1)*2) ) >> ((i+1)*2))); 
                    angles[i] = ((int)nunchuckStatus[i+2] << 2) + ((nunchuckStatus[5] & (B00000011 << ((i+1)*2) ) >> ((i+1)*2))); 
                
                //accelYArray[averageCounter] = ((int)nunchuckStatus[3] << 2) + ((nunchuckStatus[5] & B00110000) >> 4); 
                //accelZArray[averageCounter] = ((int)nunchuckStatus[4] << 2) + ((nunchuckStatus[5] & B11000000) >> 6); 
                
                buttonZ = !( nunchuckStatus[5] & B00000001);
                buttonC = !((nunchuckStatus[5] & B00000010) >> 1);
                _send_zero(); // send the request for next bytes
        
            }
        }

    byte scaleAngle(byte xyz, int range=255) {
         return range * (angles[xyz] - MIN_ACCEL) / (MAX_ACCEL - MIN_ACCEL);
    }
 /*   int readAngle(byte xyz, int range=1024) {
        total = accelArray[xyz][averageCounter] * FAST_WEIGHT;
        
        for (i=0; i< AVERAGE_N; i++) {
            total += accelArray[xyz][i];
        }
        return range * (total / (AVERAGE_N +  FAST_WEIGHT) - MIN_ACCEL) / (MAX_ACCEL - MIN_ACCEL);
    }
   */ 
    boolean zPressed() {
        return (buttonZ && ! lastZ);
    }
    boolean cPressed() {
        return (buttonC && ! lastC);
    }
    
    boolean rightJoy(int thresh=60) {
        return (readJoyX() > thresh and lastJoyX <= thresh);
    }
    
    boolean leftJoy(int thresh=60) {
        return (readJoyX() < -thresh and lastJoyX >= -thresh);
    }
   
    
    int readJoyX() {
        return (int) joyX - XJOYZERO;
      //  else
      //      return -1 * (int)(zeroJoyX - joyX);
    }
        int readJoyY() {
           // if (joyY >= zeroJoyY) 
                return (int)joyY - YJOYZERO;
    //        else
      //          return -1 * (int)(zeroJoyY - joyY);
}
            
    private:
        byte _nunchuk_decode_byte (byte x)
        {
            x = (x ^ 0x17) + 0x17;
            return x;
        }

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


/*  Some to allow a click-drag type behavior with mouse and gyros
 if (chuck.buttonZ) {
 if (! lastZ) {
 xStart = chuck.readAngle(0, ANGLE_RANGE);
 cursorStart = currentPosition;
 }
 else {
 currentPosition = cursorStart - (chuck.readAngle(0, ANGLE_RANGE) - xStart) ;    
 
 if (currentPosition < 0)
 currentPosition = 0;
 if (currentPosition > range)
 currentPosition = range; 
 
 }
 }
 
 lcdPrint(&lineTwoBuff[currentPosition]);

 
 */
#endif