//Sensor 
//Tuindeuren woonkamer
//Ventilatie rooster

// Enable debug prints to serial monitor
#define MY_DEBUG 

// Enable and select radio type attached
#define MY_RADIO_NRF24
//#define MY_RADIO_RFM69

// Enable repeater functionality for this node
#define MY_REPEATER_FEATURE

#include <MySensors.h>
#include <SPI.h>
#include <Servo.h> 
#include <Bounce2.h>

#define d1_BUTTON_PIN  4  // Arduino Digital I/O pin for button/reed switch
#define d2_BUTTON_PIN  5  // Arduino Digital I/O pin for button/reed switch

#define SERVO_DIGITAL_OUT_PIN 3
#define SERVO_MIN 0 // Fine tune your servos min. 0-180
#define SERVO_MAX 360  // Fine tune your servos max. 0-180
#define DETACH_DELAY 900 // Tune this to let your movement finish before detaching the servo
#define CHILD_ID 10   // Id of the sensor child

#define d1_CHILD_ID 11   // Id of the sensor child
#define d2_CHILD_ID 12   // Id of the sensor child

//MySensor gw;
MyMessage msg(CHILD_ID, V_DIMMER);
Servo myservo;  // create servo object to control a servo 
                // a maximum of eight servo objects can be created Sensor gw(9,10);
unsigned long timeOfLastChange = 0;
bool attachedServo = false;
//          
Bounce d1_debouncer = Bounce(); 
int d1_value;
int d1_oldValue=-1;
Bounce d2_debouncer = Bounce(); 
int d2_value;
int d2_oldValue=-1;

// Change to V_LIGHT if you use S_LIGHT in presentation below
MyMessage d1_msg(d1_CHILD_ID,V_TRIPPED);
MyMessage d2_msg(d2_CHILD_ID,V_TRIPPED);

//          
            
void setup() 
{ 
   // Setup the button
  pinMode(d1_BUTTON_PIN,INPUT);
  // Activate internal pull-up
  digitalWrite(d1_BUTTON_PIN,HIGH);
  
  // After setting up the button, setup debouncer
  d1_debouncer.attach(d1_BUTTON_PIN);
  d1_debouncer.interval(5);
 
   // Setup the button
  pinMode(d2_BUTTON_PIN,INPUT);
  // Activate internal pull-up
  digitalWrite(d2_BUTTON_PIN,HIGH);
  
  // After setting up the button, setup debouncer
  d2_debouncer.attach(d2_BUTTON_PIN);
  d2_debouncer.interval(5);
} 

void presentation() 
{
  // Send the sketch version information to the gateway and Controller  
  sendSketchInfo("Servo", "2.0");

  // Register all sensors to gw (they will be created as child devices)
  present(CHILD_ID, S_DIMMER);
  // Request last servo state at startup
  request(CHILD_ID, V_DIMMER);  
  send(msg.set(10));

  present(d1_CHILD_ID, S_DOOR);  
  present(d2_CHILD_ID, S_DOOR);  
}

void loop() 
{ 
  if (attachedServo && millis() - timeOfLastChange > DETACH_DELAY) {
     myservo.detach();
     attachedServo = false;
  }

  d1_debouncer.update();
  // Get the update value
  d1_value = d1_debouncer.read();
 
  if (d1_value != d1_oldValue) {
     // Send in the new value
     send(d1_msg.set(d1_value==HIGH ? 1 : 0));
     wait(500);
    d1_oldValue = d1_value;
  }

  d2_debouncer.update();
  // Get the update value
  d2_value = d2_debouncer.read();
 
  if (d2_value != d2_oldValue) {
     // Send in the new value
     send(d2_msg.set(d2_value==HIGH ? 1 : 0));
     wait(500);     
     d2_oldValue = d2_value;
  }
} 

void receive(const MyMessage &message)
{

  Serial.print(".");
  if (message.isAck()) 
  {
     Serial.println("This is an ack from gateway");
  
      //Check if Ack is from d1
      if(d1_oldValue != d1_value)
      {
        d1_oldValue = d1_value; 
      }

      //Check if Ack is from d2
      if(d2_oldValue != d2_value)
      {
        d2_oldValue = d2_value; 
      }
  }
  
  if (message.type==V_DIMMER) 
  { // This could be M_ACK_VARIABLE or M_SET_VARIABLE
    myservo.attach(SERVO_DIGITAL_OUT_PIN);   
    attachedServo = true;    
     int val = message.getInt();
     myservo.write(SERVO_MAX + (SERVO_MIN-SERVO_MAX)/100 * val); // sets the servo position 0-180
     myservo.write(SERVO_MAX/100 * val); // sets the servo position 0-180
     // Write some debug info
     Serial.print("Servo changed. new state: ");
     Serial.println(val);        
   } else if (message.type==V_UP) {
     Serial.println("Servo UP command");
     myservo.write(SERVO_MIN);
     send(msg.set(100));
   } else if (message.type==V_DOWN) {
     Serial.println("Servo DOWN command");
     myservo.write(SERVO_MAX); 
     send(msg.set(0));
   } else if (message.type==V_STOP) {
     Serial.println("Servo STOP command");
     myservo.detach();
     attachedServo = false;

   }
   timeOfLastChange = millis();


}