#include <CurieBLE.h>

BLEService actuatorService("19B10000-E8F2-537E-4F6C-D104768A1214"); // BLE LED Service


BLEUnsignedCharCharacteristic servoxCharacteristic("18B10001-E8F2-537E-4F6C-D104768A1214", BLERead | BLEWrite);
BLEUnsignedCharCharacteristic servoyCharacteristic("18B10001-E8F2-537E-4F6C-D104768A1314", BLERead | BLEWrite);


#include <Servo.h>

Servo xservo;
Servo yservo;

// RGB Status LED Pin definition
#define RED_LED_PIN 6        //  1
#define GREEN_LED_PIN 5      //  2
#define BLUE_LED_PIN 9       //  3

//RGB LED blink function variables
byte led_num = 0;
int blink_count = 0;
int blink_interval = 0;
int blink_count_var = 0;

//RGB LED fade function variables
int fade_brightness = 0;  // how bright the LED is
int fade_amount = 0;     // how many points to fade the LED by
int fade_delay = 0;     //dimming effect
int fade_count = 0;    //number of counts
int fade_count_var = 0;


void setup() {
  Serial.begin(9600);

//set RGB LED pins as output
  pinMode(RED_LED_PIN, OUTPUT);
  pinMode(GREEN_LED_PIN, OUTPUT);
  pinMode(BLUE_LED_PIN, OUTPUT);
  
  //initially turn-off LEDs
  digitalWrite(RED_LED_PIN, LOW);
  digitalWrite(GREEN_LED_PIN, LOW);
  digitalWrite(BLUE_LED_PIN, LOW);
  
  //system reset/initialize indication
  blink_led(1,2,100);//led,count,blink interval
  blink_led(2,2,200);//led,count,blink interval
  blink_led(3,2,200);//led,count,blink interval
  
   xservo.attach(5);
   yservo.attach(6);
  fade_led(1,3,10,10); //led,fade_count,fade_amount,fade_delay
  // begin initialization
  BLE.begin();
  fade_led(2,3,10,10); //led,fade_count,fade_amount,fade_delay
  // set advertised local name and service UUID:
  BLE.setLocalName("Actuator_WSN");
  BLE.setAdvertisedService(actuatorService);

  // add the characteristic to the service
  actuatorService.addCharacteristic(servoxCharacteristic);
  actuatorService.addCharacteristic(servoyCharacteristic);

  // add service
  BLE.addService(actuatorService);

  // set the initial value for the characeristic:
  servoxCharacteristic.setValue(45);
  servoyCharacteristic.setValue(45);

  // start advertising
  BLE.advertise();

  Serial.println("BLE Actuator Peripheral");

      // final system ready indication
 fade_led(1,2,10,10); //led,fade_count,fade_amount,fade_delay
 fade_led(2,2,10,10); //led,fade_count,fade_amount,fade_delay
 fade_led(3,2,10,10); //led,fade_count,fade_amount,fade_delay
 
}

void loop() {

  // system stand-by indication
  fade_led(2,2,10,10); //led,fade_count,fade_amount,fade_delay
  
  // listen for BLE peripherals to connect:
  BLEDevice central = BLE.central();

  // if a central is connected to peripheral:
  if (central) {
    Serial.print("Connected to central: ");
    // print the central's MAC address:
    Serial.println(central.address());
    blink_led(3,5,100);
    // while the central is still connected to peripheral:
    while (central.connected()) {
      // if the remote device wrote to the characteristic,
      // use the value to control the LED:
      if (servoxCharacteristic.written()) {
         xservo.write(servoxCharacteristic.value() ); 
      }
      if (servoyCharacteristic.written()) {
         yservo.write(servoyCharacteristic.value() ); 
      }
    }

    // when the central disconnects, print it out:
    Serial.print(F("Disconnected from central: "));
    Serial.println(central.address());
    blink_led(1,5,100);
  }
}

// RGB Led blink Function
void blink_led(int led_num, int blink_count, int blink_interval)
{
  if( led_num == 1)
  {
    for( blink_count_var = 1; blink_count_var <= blink_count; blink_count_var++ )
    {
      digitalWrite(RED_LED_PIN, HIGH);   
      delay(blink_interval);                       
      digitalWrite(RED_LED_PIN, LOW);    
      delay(blink_interval); 
    }
  }
  if( led_num == 2)
  {
    for( blink_count_var = 1; blink_count_var <= blink_count; blink_count_var++ )
    {
      digitalWrite(GREEN_LED_PIN, HIGH);   
      delay(blink_interval);                       
      digitalWrite(GREEN_LED_PIN, LOW);    
      delay(blink_interval); 
    }
  }
  if( led_num == 3)
  {
    for( blink_count_var = 1; blink_count_var <= blink_count; blink_count_var++ )
    {
      digitalWrite(BLUE_LED_PIN, HIGH);   
      delay(blink_interval);                       
      digitalWrite(BLUE_LED_PIN, LOW);    
      delay(blink_interval); 
    }
  }
}
// RGB Led Fade Function
void fade_led(int led_num, int fade_count, int fade_amount, int fade_delay  ) 
{
  if( led_num == 1)
  {
    for(fade_count_var = 1; fade_count_var <= fade_count; fade_count_var++ )
    {
      for(fade_brightness = 0; fade_brightness < 255; fade_brightness += fade_amount)
      {
          analogWrite(RED_LED_PIN, fade_brightness);
          delay(fade_delay );
      }
      for(fade_brightness = 255; fade_brightness > 0; fade_brightness -= fade_amount)
      {
          analogWrite(RED_LED_PIN, fade_brightness);
          delay(fade_delay );
      }
      digitalWrite(RED_LED_PIN, LOW);
    }
  }
 if( led_num == 2)
  {
    for(fade_count_var = 1; fade_count_var <= fade_count; fade_count_var++ )
    {
      for(fade_brightness = 0; fade_brightness < 255; fade_brightness += fade_amount)
      {
            blink_led(1,1,20); //led,count,blink interval
          analogWrite(GREEN_LED_PIN, fade_brightness);
          delay(fade_delay );
      }
      for(fade_brightness = 255; fade_brightness > 0; fade_brightness -= fade_amount)
      {

        blink_led(2,1,20); //led,count,blink interval
          analogWrite(GREEN_LED_PIN, fade_brightness);
          delay(fade_delay );
      }
      digitalWrite(GREEN_LED_PIN, LOW);
    }
  } 
 if( led_num == 3)
  {
    for(fade_count_var = 1; fade_count_var <= fade_count; fade_count_var++ )
    {
      for(fade_brightness = 0; fade_brightness < 255; fade_brightness += fade_amount)
      {
          analogWrite(BLUE_LED_PIN, fade_brightness);
          delay(fade_delay );
      }
      for(fade_brightness = 255; fade_brightness > 0; fade_brightness -= fade_amount)
      {
          analogWrite(BLUE_LED_PIN, fade_brightness);
          delay(fade_delay );
      }
      digitalWrite(BLUE_LED_PIN, LOW);
    }
  } 
}