#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;
uint8_t timerTag;

unsigned long previousMillis = 0;    
const long interval = 1000;          

unsigned long timerMillis = 0;  
long randomInterval = 1000;          


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(2, 0, 0);
  //Dynamixel.setCMargin(2, 1, 1);
  
  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];

//  unsigned long currentMillis = millis();
//  if (currentMillis - previousMillis >= interval) 
//  {
//    Dynamixel.torqueStatus(2,0);
//    Dynamixel.torqueStatus(1,0);
//    delay(30);
//  }

  unsigned long currentMillis2 = millis();
  if (currentMillis2 - timerMillis >= randomInterval) 
  {
    timerMillis = millis();
    randomInterval = random(200, 2000);
    Dynamixel.torqueStatus(1,1);
    //Dynamixel.moveSpeed(2, 760, 1000); // OJOS OLD
    //Dynamixel.moveSpeed(1, 550, 1000); // BOCA OLD
    Dynamixel.moveSpeed(2, 760, 1000); // OJOS OLD
    Dynamixel.moveSpeed(1, 420, 1000); // BOCA OLD
    delay(1000);
    //Dynamixel.moveSpeed(1, 720, 1000); // BOCA OLD
    Dynamixel.moveSpeed(2, 300, 1000); // OJOS
    Dynamixel.moveSpeed(1, 100, 1000); // BOCA 
    delay(1000);
    delay(1);
    Serial.println("random");
    delay(30);
    
  }

  
  if(Zvalue != Zvalue_old)
  {
    Zvalue_old = Zvalue;
    previousMillis = millis();
    if(Zvalue)
    {
      Serial.println("Z->ON");
      Dynamixel.torqueStatus(2,1);
      Dynamixel.moveSpeed(2, 760, 1000); // OJOS
      delay(1); 
    }
    else
    {
      Serial.println("Z->OFF");
      Dynamixel.torqueStatus(2,1);
      Dynamixel.moveSpeed(2, 300, 1000);
      delay(1); 
    }
  }

  if(Cvalue != Cvalue_old)
  {
    Cvalue_old = Cvalue;
    previousMillis = millis();
    if(Cvalue)
    {
      Serial.println("C->ON");
      Dynamixel.torqueStatus(1,1);
      Dynamixel.moveSpeed(1, 550, 1000); // BOCA
      delay(1); 
    }
    else
    {
      Serial.println("C->OFF");
      Dynamixel.torqueStatus(1,1);
      Dynamixel.moveSpeed(1, 720, 1000); 
      delay(1); 
    }
  }
  
}