// This value will store the last time we published an event
long lastPublishedAt = -1;
// this is the time delay before we should publish a new event
// from this device
int publishAfter = 1000;

// Setup inputs and outputs
int ledPin = D3;
int buttonPin = D2;
int buttonState = -1;

int remotePosition = -1;
int convertedToPosition = -1;

int servoPin = A4;
Servo servo;
bool updateServo = true;

void setup()
{

  Serial.begin( 9600 );

  pinMode(ledPin, OUTPUT );

  pinMode(buttonPin, INPUT_PULLUP);

  blinkLED( 10 );
  
  servo.attach( servoPin );

  //testServo();

  Particle.variable( "remPosition", remotePosition );
  Particle.variable( "cPos", convertedToPosition );
    
  Particle.subscribe( "naturallic/paired/", handleServoToggleMessage );
}

void testServo(){
    //  Test the servo is working and connected
  servo.write( 90 );
  delay(1000);
  servo.write( 5 );
  delay(1000);
  servo.write( 170 );
  delay(1000);
}

void loop()
{
	int value = digitalRead( buttonPin );
	
	if( value == LOW && buttonState != value ){
	    
	    // there's been a change.
	    remotePosition = -remotePosition;

        updateServo = true;

        blinkLED( 3 );
        
	    // publish my event
	    // you'll want some more complex stuff here
    	publishMyEvent(  );
	        
	    
	}
	// store the value for the next press.
	buttonState = value;
	
	if( updateServo ){
	    
	    if( remotePosition == -1 ){
	        
	        Serial.println( "setting zero to -1 ");
	        
	        servo.write( 5 );
	    }else{
	        
	        Serial.println( "setting zero to +1 ");

	        servo.write( 175 );
	    }
	    
	    
	    updateServo = false;
	}
	

    // delay for a bit
    delay(100);
}

void blinkLED( int times ){
    
    for( int i = 0; i< times; i++){
        digitalWrite( ledPin, HIGH );
        delay(100);
        digitalWrite( ledPin, LOW );
        delay(100);
    }

}

void publishMyEvent()
{
  // Remember that a device can publish at rate of about 1 event/sec,
  // with bursts of up to 4 allowed in 1 second.
  // Back to back burst of 4 messages will take 4 seconds to recover.
  // So we want to limit the amount of publish events that happen.

  // check that it's been 10 secondds since our last publish
  //Particle.publish( "test" , String( remotePosition ) );


  if( lastPublishedAt + publishAfter < millis() )
  {
      String eventName = "naturallic/paired/" + System.deviceID();

      Particle.publish( eventName, String( remotePosition ) );

      // we just pubished so capture this.
      lastPublishedAt = millis();
  }

}


void handleServoToggleMessage( const char * event, const char *data )
{
    String eventStr = String( event );
    String dataStr = String( data );
    String deviceID = System.deviceID();
    
    int position = dataStr.toInt();
    
    // if (eventStr.indexOf( deviceID ) != -1) {
    //     return;
    // }
    // else {
        if( position == -1 or position == 1){
           remotePosition =  position;
           updateServo = true;
        }
    
        Serial.print( "handleServoToggleMessage " );
        Serial.print( data );
        Serial.print( "from Device: " );
        Serial.println( eventStr );
                
    // }
}