int armServoPin = D2;
int headServoPin = D3;
//int ledPin = D2;
int buttonPin = D0;
int electroMagPin = D1;
//int buttonPinArmDown = D1 ;
Servo pandaArmServo;
Servo pandaHeadServo;
int buttonState = LOW;
int servoPos = 50;
int magstrength = 255;
//
bool shouldDoNod = false;
bool isArmUp = false;

void setup(){
  Serial.begin(9600);
  pandaArmServo.attach(armServoPin); //Attaching Arm servo pin to Arm servo
  pandaHeadServo.attach(headServoPin); // Attaching Head servo pin to Head servo
  pinMode(buttonPin , INPUT_PULLUP); //defining pin Mode
  pinMode(electroMagPin, OUTPUT); // defining electro mag output pin
  /*Particle.function("servo", servoControl);*/
  /*Particle.variable( "Nod" , &Nod , INT );*/
  Particle.subscribe( "diot2017/mashedpotatoes/panda/nod", Nodding );
}

void loop(){

  if( shouldDoNod )
  {
    doNod( );
    shouldDoNod = false;
  }

  int currentButtonState = digitalRead( buttonPin );

  if( buttonState == HIGH && buttonState != currentButtonState ) // Enter only if the button is pushed
  {
      Serial.println("Pressed");
      isArmUp = !isArmUp;
  }

  buttonState = currentButtonState;

  if( isArmUp && servoPos < 100 ){
    // Check if arm is up or down
    servoPos = 100 ; // if arm down, make the new servo position 150 (up)
    pandaArmServo.write( servoPos ); // update servo position
    Serial.println("Arm DOWN");
    analogWrite(electroMagPin, magstrength); // switch the electro magnet ON

    delay( 1000 );
  }else if( !isArmUp && servoPos > 50 ){
    servoPos = 50 ; // if arm up, make the new servo position 50 (down)
    pandaArmServo.write( servoPos ); // update servo position
    Serial.println("Arm UP");
    Particle.publish("diot2017/mashedpotatoes/panda/LedOn");
    digitalWrite(electroMagPin, 0); // switch OFF the electro magnet
    delay( 1000 );


  }

  delay( 50 );
}

void doNod( )
{
  for (int i=0; i< 5; i++ ) // loop to nod head 5 times
  {
    Serial.println("Head UP");
    pandaHeadServo.write(50);
    delay(200);
    Serial.println("Head DOWN");
    pandaHeadServo.write (15);
    delay(200);
  }
}

/*int servoControl( String command ) // Panda head nodding function
{
  int n = command.toInt();
  Serial.println("DO NOD");
  if (n == 1){
    for (int i=0; i< 5; i++ ) // loop to nod head 5 times
    {
      Serial.println("Head UP");
      pandaHeadServo.write (90);
      delay(2000);
      Serial.println("Head DOWN");
      pandaHeadServo.write (15);
      delay(1000);
    }
  }
  return 1;
}*/

int Nodding( const char *event, const char *data ) // Panda head nodding function
{
  Serial.println("DO NOD");
  shouldDoNod = true;
  return 1;
}