#define WII_IDENT_LEN ((byte)6)
#define WII_TELEGRAM_LEN ((byte)6)
#define WII_NUNCHUCK_TWI_ADR ((byte)0x52)
#include <Wire.h>
#include <string.h>
#include <utility/twi.h>
#include <Servo.h> 
Servo myservo1;
Servo myservo2;
#undef int
#include <stdio.h>
uint8_t outbuf[WII_TELEGRAM_LEN];
int cnt = 0;
int servoPin1 = 3;
int servoPin2 = 4;
int locCx =   0;
int locCy =  79;
int locZx =  84;
int locZy =  97;
int ledPin = 17;
int servoPos1 = 90;
int servoPos2 = 90;
int pulseWidth = 0;
int pulseWidth2 = 0;
long lastPulse = 0;
long lastPulse2 = 0;
int refreshTime = 20;
int minPulse = 1000;
int minPulse2 = 500;
int dtime=10;
#define pwbuffsize 10
long pwbuff[pwbuffsize];
long pwbuffpos = 0;
long pwbuff2[pwbuffsize];
long pwbuffpos2 = 0;
int b00[] = { 60, 60, 60, 60, 60,160,160,160,160,160}; int b01[] = {160, 60, 60, 60, 60, 60,160,160,160,160}; int b02[] = { 60,160, 60, 60, 60,160, 60,160,160,160}; int b03[] = { 60, 60,160, 60, 60,160,160, 60,160,160}; int b04[] = {160, 60,160, 60, 60, 60,160, 60,160,160}; int b05[] = { 60,160,160, 60, 60,160, 60, 60,160,160}; int b06[] = { 60, 60, 60,160, 60,160,160,160, 60,160}; int b07[] = {160, 60, 60,160, 60, 60,160,160, 60,160}; int b08[] = { 60,160, 60,160, 60,160, 60,160, 60,160}; int b09[] = { 60, 60,160,160, 60,160,160, 60, 60,160}; int b10[] = {160, 60,160,160, 60, 60,160, 60, 60,160}; int b11[] = { 60,160,160,160, 60,160, 60, 60, 60,160}; int b12[] = { 60, 60, 60, 60,160,160,160,160,160, 60}; int b13[] = {160, 60, 60, 60,160, 60,160,160,160, 60}; int b14[] = { 60,160, 60, 60,160,160, 60,160,160, 60}; int b15[] = { 60, 60,160, 60,160,160,160, 60,160, 60}; int b16[] = {160, 60,160, 60,160, 60,160, 60,160, 60}; int b17[] = { 60,160,160, 60,160,160, 60, 60,160, 60}; int b18[] = { 60, 60, 60,160,160,160,160,160, 60, 60}; int b19[] = {160, 60, 60,160,160, 60,160,160, 60, 60}; int b20[] = { 60,160, 60,160,160,160, 60,160, 60, 60};
#define IRpin_PIN      PIND
#define IRpin          2
#define MAXPULSE 35000
#define RESOLUTION 20 
#define FUZZINESS 20
uint16_t pulses[100][2];  // pair is high and low pulse 
uint8_t currentpulse = 0; // index for pulses we're storing
int t = 0;

void setup (){
  Serial.begin (115200);
  Wire.begin();
  #define TWI_FREQ_NUNCHUCK 400000L
  TWBR = ((CPU_FREQ / TWI_FREQ_NUNCHUCK) - 16) / 2;
  nunchuck_init(0);
  pinMode(servoPin1, OUTPUT);      
  pinMode(servoPin2, OUTPUT);      
  pinMode(ledPin, OUTPUT);      
  pulseWidth = minPulse;
  pulseWidth2 = minPulse2;
  byte i;
  if(readControllerIdent(outbuf) == 0){
    Serial.print("Ident=");
    for (i = 0; i < WII_TELEGRAM_LEN; i++){
      Serial.print(outbuf[i], HEX);
      Serial.print(' ');
    }
    Serial.println();
  }
  myservo1.attach(servoPin1);
  myservo2.attach(servoPin2);
  Serial.println("Finished setup");
}
void loop (){
  t++;
  long last = millis();
  if( t == 1) {
    t = 0;
    Wire.requestFrom (WII_NUNCHUCK_TWI_ADR, WII_TELEGRAM_LEN); // request data from nunchuck
    for (cnt = 0; (cnt < WII_TELEGRAM_LEN) && Wire.available (); cnt++){
      outbuf[cnt] = nunchuk_decode_byte (Wire.receive ()); // receive byte as an integer
      digitalWrite (ledPin, HIGH); // sets the LED on
    }
    clearTwiInputBuffer();
    if (cnt >= WII_TELEGRAM_LEN){ print(); }
    send_zero ();
  }
  updateServo();
  delay (20);
  int numberpulses = listenForIR();
       if (IRcompare(numberpulses, b00)) { Serial.println("VOL-"); }
  else if (IRcompare(numberpulses, b01)) { Serial.println("PLAY/PAUSE"); }
  else if (IRcompare(numberpulses, b02)) { Serial.println("VOL+"); }
  else if (IRcompare(numberpulses, b03)) { Serial.println("SETUP"); }
  else if (IRcompare(numberpulses, b04)) { Serial.println("UP (PREV)"); }
  else if (IRcompare(numberpulses, b05)) { Serial.println("STOP/MODE"); }
  else if (IRcompare(numberpulses, b06)) { Serial.println("LEFT (CH-)"); }
  else if (IRcompare(numberpulses, b07)) { Serial.println("ENTER/SAVE"); }
  else if (IRcompare(numberpulses, b08)) { Serial.println("RIGHT (CH+)"); }
  else if (IRcompare(numberpulses, b09)) { Serial.println("0 10+"); }
  else if (IRcompare(numberpulses, b10)) { Serial.println("DOWN (NEXT)"); }
  else if (IRcompare(numberpulses, b11)) { Serial.println("CONTINUE"); }
  else if (IRcompare(numberpulses, b12)) { Serial.println("1");preset( 0,79); }
  else if (IRcompare(numberpulses, b13)) { Serial.println("2");preset(84,97); }
  else if (IRcompare(numberpulses, b14)) { Serial.println("3"); }
  else if (IRcompare(numberpulses, b15)) { Serial.println("4"); }
  else if (IRcompare(numberpulses, b16)) { Serial.println("5"); }
  else if (IRcompare(numberpulses, b17)) { Serial.println("6"); }
  else if (IRcompare(numberpulses, b18)) { Serial.println("7"); }
  else if (IRcompare(numberpulses, b19)) { Serial.println("8"); }
  else if (IRcompare(numberpulses, b20)) { Serial.println("9"); }
}
byte nunchuck_init (unsigned short timeout){
  byte rc = 1;
  Wire.beginTransmission (WII_NUNCHUCK_TWI_ADR); // transmit to device 0x52
  Wire.send (0x40); // sends memory address
  Wire.send (0x00); // sends sent a zero.
  Wire.endTransmission (); // stop transmitting
  unsigned long time = millis();
  do{
    Wire.beginTransmission (WII_NUNCHUCK_TWI_ADR); // transmit to device 0x52
    Wire.send (0xF0); // sends memory address
    Wire.send (0x55); // sends data.
    if(Wire.endTransmission() == 0){ // stop transmitting
      Wire.beginTransmission (WII_NUNCHUCK_TWI_ADR); // transmit to device 0x52
      Wire.send (0xFB); // sends memory address
      Wire.send (0x00); // sends sent a zero.
      if(Wire.endTransmission () == 0){ // stop transmitting
        rc = 0;
      }
    }
  }
  while (rc != 0 && (!timeout || ((millis() - time) < timeout)));
  return rc;
}
byte readControllerIdent(byte* pIdent){
  byte rc = 1;
  Wire.beginTransmission (WII_NUNCHUCK_TWI_ADR); // transmit to device 0x52
  Wire.send (0xFA); // sends memory address of ident in controller
  if(Wire.endTransmission () == 0){ // stop transmitting
    byte i;
    Wire.requestFrom (WII_NUNCHUCK_TWI_ADR, WII_TELEGRAM_LEN); // request data from nunchuck
    for (i = 0; (i < WII_TELEGRAM_LEN) && Wire.available (); i++){
      pIdent[i] = Wire.receive(); // receive byte as an integer
    }
    if(i == WII_TELEGRAM_LEN){ rc = 0; }
  }
  return rc;
}
void clearTwiInputBuffer(void){
  while( Wire.available ())
  Wire.receive ();
}
void send_zero (){
  for(byte i = 0; i < 3; i++){
    Wire.beginTransmission (WII_NUNCHUCK_TWI_ADR); // transmit to device 0x52
    Wire.send (0x00); // sends one byte
    Wire.endTransmission (); // stop transmitting
  }
}
void preset(int x, int y){
    servoPos1=x;                  // sets the servo position according to the scaled value 
    servoPos2=y;                  // sets the servo position according to the scaled value 
    myservo1.write(servoPos1);                  // sets the servo position according to the scaled value 
    myservo2.write(servoPos2);                  // sets the servo position according to the scaled value 
    delay(500);                           // waits for the servo to get there 
}
void print (){
  int joy_x_axis = outbuf[0];
  int joy_y_axis = outbuf[1];
  int accel_x_axis = outbuf[2];// * 2 * 2
  int accel_y_axis = outbuf[3];// * 2 * 2
  int accel_z_axis = outbuf[4];// * 2 * 2
  int z_button = 0;
  int c_button = 0;
  if ((outbuf[5] >> 0) & 1){ z_button = 1; }
  if ((outbuf[5] >> 1) & 1){ c_button = 1; }
  if (c_button && z_button){ muovi(); }

Serial.print (joy_x_axis, DEC);
Serial.print ("\t");
Serial.print (joy_y_axis, DEC);
Serial.print ("\t");
Serial.print (accel_x_axis, DEC);
Serial.print ("\t");
Serial.print (accel_y_axis, DEC);
Serial.print ("\t");
Serial.print (accel_z_axis, DEC);
Serial.print ("\t");
Serial.print (z_button, DEC);
Serial.print ("\t");
Serial.println (c_button, DEC);

  if(
    (joy_x_axis == 255) && 
    (joy_y_axis == 255) && 
    (accel_x_axis == 255) && 
    (accel_y_axis == 255) && 
    (accel_z_axis == 255) && 
    (c_button == 1) && 
    (z_button == 1)
  ){ 
    Serial.println("\tError: Outof sync.");
    Serial.println("----RESET----");
    delay(500);
  }
}
char nunchuk_decode_byte (char x){
  x = (x ^ 0x17) + 0x17;
  return x;
}
void muovi (){
  float serv1 = outbuf[0];// joy x 0-255 
  if (serv1 < 200 && serv1 > 50){
    // centered
  } else if (serv1 > 250){
    servoPos1=servoPos1-5;
  } else if (serv1 > 200){
    servoPos1=servoPos1-2;
  } else if (serv1 < 5){
    servoPos1=servoPos1+5;
  } else if (serv1 < 50){
    servoPos1=servoPos1+2;
  }
    
  float serv2 = outbuf[1];// joy y 0-255
  if (serv2 < 200 && serv2 > 50){
    // centered
  } else if (serv2 > 250){
    servoPos2=servoPos2+5;
  } else if (serv2 > 200){
    servoPos2=servoPos2+2;
  } else if (serv2 < 5){
    servoPos2=servoPos2-5;
  } else if (serv2 < 50){
    servoPos2=servoPos2-2;
  }
  if (servoPos1 < 0){ servoPos1 = 0; }
  if (servoPos1 > 179){ servoPos1 = 179; }
  if (servoPos2 < 0){ servoPos2 = 0; }
  if (servoPos2 > 179){ servoPos2 = 179; }

Serial.print (servoPos1, DEC);
Serial.print ("\t");
Serial.print (servoPos2, DEC);
Serial.print ("\t");
Serial.print ("\r\n");
}
void updateServo() {
  myservo1.write(servoPos1);                  // sets the servo position according to the scaled value 
  myservo2.write(servoPos2);                  // sets the servo position according to the scaled value 
  delay(250);                           // waits for the servo to get there 
}
int listenForIR(void) {
  currentpulse = 0;
  while (1) {
    uint16_t highpulse, lowpulse;  // temporary storage timing
    highpulse = lowpulse = 0; // start out with no pulse length
    while (IRpin_PIN & (1 << IRpin)) {
       highpulse++;
       delayMicroseconds(RESOLUTION);
       if ((highpulse >= MAXPULSE) && (currentpulse != 0)) {
         return currentpulse;
       }
    }
    pulses[currentpulse][0] = highpulse;
    while (! (IRpin_PIN & _BV(IRpin))) {
       lowpulse++;
       delayMicroseconds(RESOLUTION);
       if ((lowpulse >= MAXPULSE)  && (currentpulse != 0)) {
         return currentpulse;
       }
    }
    pulses[currentpulse][1] = lowpulse;
    currentpulse++;
  }
}
boolean IRcompare(int numpulses, int Signal[]) {
  int com[] = {880,430,60,60,60,60,60,60,60,60,60,60,60,60,60,60,60,60,60,160,60,160,60,160,60,160,60,160,60,160,60,60,60,160,60,999,60,999,60,999,60,999,60,999,60,60,60,60,60,60,60,160,60,999,60,999,60,999,60,999,60,160,60,160,60,160,60,4000,880,220,60, 0};
  for (int i=0; i< numpulses-1; i++) {
    int oncode = pulses[i][1] * RESOLUTION / 10;
    int offcode = pulses[i+1][0] * RESOLUTION / 10;
    if ( abs(oncode - com[i*2 + 0]) > (com[i*2 + 0] * FUZZINESS / 100)) {
     return false;
    }
    int iO = i*2 + 1;
    if((iO==35)||(iO==37)||(iO==39)||(iO==41)||(iO==43)||(iO==51)||(iO==53)||(iO==55)||(iO==57)||(iO==59)){
      switch(iO){
        case 35: iO = 0; break;
        case 37: iO = 1; break;
        case 39: iO = 2; break;
        case 41: iO = 3; break;
        case 43: iO = 4; break;
        case 51: iO = 5; break;
        case 53: iO = 6; break;
        case 55: iO = 7; break;
        case 57: iO = 8; break;
        case 59: iO = 9; break;
      }
      if ( abs(offcode - Signal[iO]) > (Signal[iO] * FUZZINESS / 100)) {
        return false;
      }
    } else {
      if (abs(offcode - com[i*2 + 1]) > (com[i*2 + 1] * FUZZINESS / 100)) {
        return false;
      }
    } 
  }
  return true;
}