#define USE_NEW_WAY_INIT 1
#define WII_IDENT_LEN ((byte)6)
#define WII_TELEGRAM_LEN ((byte)6)
#define WII_NUNCHUCK_TWI_ADR ((byte)0x52)
#define pwbuffsize 10
#include <Wire.h>
#include <string.h>
#include <utility/twi.h>
#include <Servo.h> 
#include <SdFat.h>
#include <stdio.h>
#include <MAX6675.h>
uint8_t outbuf[WII_TELEGRAM_LEN];
Servo myservo1;
Servo myservo2;
int cnt = 0;
int servoPin1 = 7;
int servoPin2 = 8;
int ledPin = 13;
int servoPos1 = 90;
int servoPos2 = 90;
int pulseWidth = 0;
int pulseWidth2 = 0;
int refreshTime = 20;
int minPulse = 1000;
int minPulse2 = 500;
int dtime=10;
long lastPulse = 0;
long lastPulse2 = 0;
long pwbuff[pwbuffsize];
long pwbuffpos = 0;
long pwbuff2[pwbuffsize];
long pwbuffpos2 = 0;
/*
 ** CS MAX6675 pin6
 ** SO MAX6675 pin7
 ** SCK MAX6675 pin5
 ** vcc MAX6675 pin4
 ** gnd to MAX6675 pin1
 ** tc - to MAX6675 pin2
 ** tc + to MAX6675 pin3
*/
int LED1 = 6; // Status LED Pin
int maxCS = 9; // CS pin on MAX6675
int SO = 10; // SO pin of MAX6675
int SCK = 13; // SCK pin of MAX6675
int units = 2; // Units to readout temp (0 = raw, 1 = ˚C, 2 = ˚F)
float temperature = 0.0; // Temperature output variable
MAX6675 temp(maxCS,SO,SCK,units);// Initialize the MAX6675 Library for our chip

/*
 ** SD card attached to SPI bus as follows:
 ** MOSI - pin 11
 ** MISO - pin 12
 ** CLK - pin 13
 ** CS - pin 4
*/
const uint8_t chipSelect = 4;// SD chip select pin
SdFat sd;// file system object
ArduinoOutStream cout(Serial);// create Serial stream
#define error(s) sd.errorHalt_P(PSTR(s))// store error strings in flash to save RAM

void setup (){
  // filename for this example
  char name[] = "chart.txt";
  Serial.begin (19200);
  cout << endl << pstr("Type any character to start\n");
  while (!Serial.available());

  // initialize the SD card at SPI_HALF_SPEED to avoid bus errors with
  // breadboards.  use SPI_FULL_SPEED for better performance.
  if (!sd.init(SPI_HALF_SPEED, chipSelect)){ sd.initErrorHalt(); }
  cout << pstr("Appending to: ") << name;
  for (uint8_t i = 0; i < 100; i++) {
    // open stream for append
    ofstream sdout(name, ios::out | ios::app);
    if (!sdout){ error("open failed"); }
    // append 100 lines to the file
    for (uint8_t j = 0; j < 100; j++) {
      // use int() so byte will print as decimal number
      sdout << "line " << int(j) << " of pass " << int(i);
      sdout << " millis = " << millis() << endl;
    }
    // close the stream
    sdout.close();
    if (!sdout){ error("append data failed"); }
    // output progress indicator
    if (i % 25 == 0){ cout << endl; }
    cout << '.';
  }
  cout << endl << "Done" << endl;

  Wire.begin(); // initialize i2c
  #define TWI_FREQ_NUNCHUCK 400000L
  TWBR = ((CPU_FREQ / TWI_FREQ_NUNCHUCK) - 16) / 2;
  nunchuck_init(0); // send the initialization handshake
  pinMode(servoPin1, OUTPUT);      
  pinMode(servoPin2, OUTPUT);      
  pinMode(LED1, 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");
}

byte nunchuck_init (unsigned short timeout){
  byte rc = 1;
  #ifndef USE_NEW_WAY_INIT
    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
  #else
    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)));
  #endif
  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
  }
}

int t = 0;
void loop (){
  temperature = temp.read_temp();
  if(temperature < 0) {
    // If there is an error with the TC, temperature will be < 0
    Serial.print("Thermocouple Error on CS");
    Serial.println( temperature );
    digitalWrite(LED1, HIGH);
  } else {
    Serial.print("Current Temperature: ");
    Serial.println( temperature );
    digitalWrite(LED1, LOW);
  }

  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){  // If we recieved the 6 bytes, then go print them
      print ();
      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(); }
    }
    send_zero (); // send the request for next bytes
  }
  updateServo();
  delay (200);
}

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; }
  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.print (c_button, DEC);
  Serial.print ("\t");
  Serial.print ("\r\n");
  if(!c_button && !z_button){ } else {
    if(!c_button){ delay(1000); }
    if(!z_button){ delay(5000); }
  }
}

char nunchuk_decode_byte (char x){
  #ifndef USE_NEW_WAY_INIT
    x = (x ^ 0x17) + 0x17;
  #endif
  return x;
}

void muovi (){
  float serv1 = outbuf[0];// joy x 0-255
  float serv2 = outbuf[1];// joy y 0-255
  servoPos1 = map(serv1, 0, 255, 179, 0);// scale it to use it with the servo (value between 0 and 180) 
  servoPos2 = map(serv2, 0, 255, 0, 179);// scale it to use it with the servo (value between 0 and 180) 
}

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(15);                             // waits for the servo to get there 
}