#include "DynamixelSerial.h"
#include <SPI.h>
#include "wiring_private.h" 

#include <WiiChuck.h>
Accessory nunchuck1;

uint8_t rele1 = A1;
uint8_t rele2 = A0;

uint8_t Zvalue;
uint8_t Cvalue;
uint8_t Zvalue_old = 0;
uint8_t Cvalue_old = 0;
uint8_t Ztag, Ctag;

void resetBoard()
{
  //Serial.println("reset");
  NVIC_SystemReset();       // esta funcion en teoria si funciona en SAMD
}



void setup() {

  //Dynamixel.setID(1,2); // Solo para programar las IDs
  Dynamixel.begin(1000000,5);  // Inicialize the servo at 1Mbps and Pin Control 5
  Dynamixel.setCSlope(254, 64, 64);
  Dynamixel.setCMargin(254, 3, 3);
  
  Serial.begin(512000);
  //delay(4000);
  //while (!Serial) {
    //; // wait for serial port to connect. Needed for Leonardo only
  //}

  pinMode(rele1, OUTPUT);
  pinMode(rele2, OUTPUT);
  
  nunchuck1.begin();
  if (nunchuck1.type == Unknown_) { // Modificado en la libreria para que no de error con la Ethernet.h
    nunchuck1.type = NUNCHUCK;
  }
  
}


void loop() {

  nunchuck1.readData();    // Read inputs and update maps
  uint8_t joystickValueX = nunchuck1.values[0];
  uint8_t joystickValueY = nunchuck1.values[1];
  Zvalue = nunchuck1.values[10];
  Cvalue = nunchuck1.values[11];

//  Serial.print("X: ");
//  Serial.print(joystickValueX);
//  Serial.print(" Y: ");
//  Serial.print(joystickValueY);
//  Serial.print(" Z: ");
//  Serial.print(Zvalue);  
//  Serial.print(" C: ");
//  Serial.println(Cvalue);  
  
  if(Zvalue != Zvalue_old)
  {
    Zvalue_old = Zvalue;
    if(Zvalue)
    {
      Serial.println("Z->ON");
      if(Ztag)
      {
         Dynamixel.moveSpeed(2, 1023, 512);
         Ztag=0;
      }
      else
      {
         Dynamixel.moveSpeed(2, 0, 512); 
         Ztag=1;
      }
    } 
  }

  if(Cvalue != Cvalue_old)
  {
    Cvalue_old = Cvalue;
    if(Cvalue)
    {
      Serial.println("C->ON");
      if(Ctag)
      {
         Dynamixel.moveSpeed(1, 1023, 1023);
         Ctag=0;
      }
      else
      {
         Dynamixel.moveSpeed(1, 0, 1023); 
         Ctag=1;
      }
    } 

  }


  
}