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

uint8_t rele1 = A1;
uint8_t rele2 = A0;
uint8_t interruptPin = A2;

unsigned long previousMillis = 0;
long interval = 1000;
unsigned long timerMillis = 0;
long randomInterval = 1000;

volatile bool bocaOpenFlag, bocaCloseFlag;

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



void boca() 
{ // toas las variables adentro han de ser volatil 


  if((digitalRead(interruptPin) == 1) && (bocaCloseFlag == 1) )
  {
      Serial.println("Cierra Boca");
      cierraBoca(); 
      delay(20);
      bocaCloseFlag = 0;
  }
  else if((digitalRead(interruptPin) == 0) && (bocaCloseFlag == 0) )
  {
      Serial.println("Abre Boca");
      abreBoca();
      delay(20);
      bocaCloseFlag = 1;
  }

}

void setup() {

  //Dynamixel.setID(1,2); // Solo para programar las IDs
  Dynamixel.begin(1000000, 5); // Inicialize the servo at 1Mbps and Pin Control 5

  Serial.begin(512000);
  //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;
  // }
  
  pinMode(interruptPin, INPUT_PULLUP);
  //attachInterrupt(digitalPinToInterrupt(interruptPin), ISR_boca, CHANGE);

  randomSeed(42);   

}


void abreBoca() // id 1
{
  //Dynamixel.torqueStatus(2,1);
  Dynamixel.moveSpeed(2, 100, 1000); // BOCA
  delay(100);
  //Dynamixel.torqueStatus(2,0);

}

void cierraBoca() // id 1
{
  //Dynamixel.torqueStatus(2,1);
  Dynamixel.moveSpeed(2, 420, 1000); // BOCA
  //Dynamixel.torqueStatus(2,0);
  //delay(100);
}

void abreOjo() // id 2  INVERTIDO MOTOR
{
  Dynamixel.torqueStatus(1, 1);
  Dynamixel.moveSpeed(1, 350, 1023); // OJOS
  delay(250);
  Dynamixel.torqueStatus(1, 0);
}

void cierraOjo() // id 2  INVERTIDO MOTOR
{
  Dynamixel.torqueStatus(1, 1);
  Dynamixel.moveSpeed(1, 700, 1023); // OJOS OLD
  delay(250);
  Dynamixel.torqueStatus(1, 0);
}


void loop()
{

  boca();

  unsigned long currentMillis = millis();

  if (currentMillis - previousMillis >= interval) 
  {
    // save the last time you blinked the LED
    previousMillis = currentMillis;
    interval = random(500, 5000);
    
    abreOjo();
    delay(10);
    cierraOjo();
  }


}