#include <CurieBLE.h>


// BLE Services & Characteristics UUIDs

//IMU NODE
#define IMU_SERVICE_UUID "3147ac9e-fc38-28e5-52aa-5e5517507c66"
#define AX_CHAR_UUID "3147af14-fc38-11e5-86aa-5e5517507c66"
#define AY_CHAR_UUID "3147b090-fc38-11e5-86aa-5e5517507c66"
#define AZ_CHAR_UUID "3147b180-fc38-11e5-86aa-5e5517507c66"
#define GX_CHAR_UUID "3147b252-fc38-11e5-86aa-5e5517507c66"
#define GY_CHAR_UUID "3147b5ae-fc38-11e5-86aa-5e5517507c66"
#define GZ_CHAR_UUID "3147b694-fc38-11e5-86aa-5e5517507c66"
//MAGNETOMETER NODE
#define MAG_SERVICE_UUID "3147ac9e-fc38-28b5-52aa-5e5517507a66"
#define MX_CHAR_UUID "3147af14-fc38-11b5-86aa-5e5517507a66"
#define MY_CHAR_UUID "3147b090-fc38-11b5-86aa-5e5517507a66"
#define MZ_CHAR_UUID "3147b180-fc38-11b5-86aa-5e5517507a66"
//GPS NODE
#define GPS_SERVICE_UUID "3147ac9e-fb38-28e5-52aa-5e5517507b66"
#define LAT_CHAR_UUID "3147af14-fa38-11e5-86aa-5e5517507b66"
#define LON_CHAR_UUID "3147b090-fa38-11e5-86aa-5e5517507b66"
#define ALT_CHAR_UUID "3147b180-fa38-11e5-86aa-5e5517507b66"
#define NOS_CHAR_UUID "3147b252-fa38-11e5-86aa-5e5517507b66"
// BLE Services & Characteristics UUIDs
#define SENSOR_SERVICE_UUID "3197ac9e-fc38-28e5-52aa-5e5517507c66"
#define TS_CHAR_UUID "3197af14-ac38-11e5-86aa-5e5517507c66"
#define PS_CHAR_UUID "3197b090-ac38-11e5-86aa-5e5517507c66"
#define HS_CHAR_UUID "3197b180-ac38-11e5-86aa-5e5517507c66"
#define FS_CHAR_UUID "3197b252-ac38-11e5-86aa-5e5517507c66"
#define SS_CHAR_UUID "3197b5ae-ac38-11e5-86aa-5e5517507c66"
#define AS_CHAR_UUID "3197b694-ac38-11e5-86aa-5e5517507c66"


// RGB Status LED Pin definition
#define RED_LED_PIN 5       //  1
#define GREEN_LED_PIN 9     //  2
#define BLUE_LED_PIN 6      //  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() {
 // put your setup code here, to run once:
 
  Serial.begin(9600);

  // initialize the BLE hardware
  BLE.begin();

  Serial.println("BLE Central - Peripheral Explorer");
  
 
  
  //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,3,200); //led,count,blink interval
//  blink_led(2,3,200); //led,count,blink interval
//  blink_led(3,3,200); //led,count,blink interval

    // start scanning for peripherals
  BLE.scan();
  
 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() {
  // put your main code here, to run repeatedly:
 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

 // check if a peripheral has been discovered
  BLEDevice peripheral = BLE.available();

  if (peripheral) {
    // discovered a peripheral, print out address, local name, and advertised service
    Serial.print("Found ");
    Serial.print(peripheral.address());
    Serial.print(" '");
    Serial.print(peripheral.localName());
    Serial.print("' ");
    Serial.print(peripheral.advertisedServiceUuid());
    Serial.println();

    // see if peripheral is a LED
    if (peripheral.localName() == "LED") {
      // stop scanning
      BLE.stopScan();

      explorerPeripheral(peripheral);

      // peripheral disconnected, we are done
      while (1) {
        // do nothing
      }
    }
  }
}

void explorerPeripheral(BLEDevice peripheral) {
  // connect to the peripheral
  Serial.println("Connecting ...");

  if (peripheral.connect()) {
    Serial.println("Connected");
  } else {
    Serial.println("Failed to connect!");
    return;
  }

  // discover peripheral attributes
  Serial.println("Discovering attributes ...");
  if (peripheral.discoverAttributes()) {
    Serial.println("Attributes discovered");
  } else {
    Serial.println("Attribute discovery failed!");
    peripheral.disconnect();
    return;
  }

  // read and print device name of peripheral
  Serial.println();
  Serial.print("Device name: ");
  Serial.println(peripheral.deviceName());

  // loop the services of the peripheral and explore each
  for (int i = 0; i < peripheral.serviceCount(); i++) {
    BLEService service = peripheral.service(i);

    exploreService(service);
  }

  Serial.println();

  // we are done exploring, disconnect
  Serial.println("Disconnecting ...");
  peripheral.disconnect();
  Serial.println("Disconnected");
}

void exploreService(BLEService service) {
  // print the UUID of the service
  Serial.print("Service ");
  Serial.println(service.uuid());

  // loop the characteristics of the service and explore each
  for (int i = 0; i < service.characteristicCount(); i++) {
    BLECharacteristic characteristic = service.characteristic(i);

    exploreCharacteristic(characteristic);
  }
}

void exploreCharacteristic(BLECharacteristic characteristic) {
  // print the UUID and properies of the characteristic
  Serial.print("\tCharacteristic ");
  Serial.print(characteristic.uuid());
  Serial.print(", properties 0x");
  Serial.print(characteristic.properties());

  // check if the characteristic is readable
  if (characteristic.canRead()) {
    // read the characteristic value
    characteristic.read();

    if (characteristic.valueLength() > 0)
    {
      // print out the value of the characteristic
      Serial.print(", value 0x");
      printData(characteristic.value(), characteristic.valueLength());
    }
  }
  Serial.println();

  // loop the descriptors of the characteristic and explore each
  for (int i = 0; i < characteristic.descriptorCount(); i++) {
    BLEDescriptor descriptor = characteristic.descriptor(i);

    exploreDescriptor(descriptor);
  }
}

void exploreDescriptor(BLEDescriptor descriptor) {
  // print the UUID of the descriptor
  Serial.print("\t\tDescriptor ");
  Serial.print(descriptor.uuid());

  // read the descriptor value
  descriptor.read();

  // print out the value of the descriptor
  Serial.print(", value 0x");
  printData(descriptor.value(), descriptor.valueLength());

  Serial.println();
}

void printData(const unsigned char data[], int length) {
  for (int i = 0; i < length; i++) {
    unsigned char b = data[i];

    if (b < 16) {
      Serial.print("0");
    }

    Serial.print(b, HEX);
  }
}


//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)
      {
          analogWrite(GREEN_LED_PIN, fade_brightness);
          delay(fade_delay );
      }
      for(fade_brightness = 255; fade_brightness > 0; fade_brightness -= fade_amount)
      {
          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);
    }
  } 
}