#include <BLEDevice.h>
#include <BLEServer.h>
#include <BLEUtils.h>
#include <BLE2902.h>
#include <ESP32_Servo.h>

#ifdef __cplusplus
extern "C" {
#endif
uint8_t temprature_sens_read();
#ifdef __cplusplus
}
#endif
uint8_t temprature_sens_read();

#define DIR1_CHANNEL     2
#define DIR2_CHANNEL     3

// use 13 bit precission for LEDC timer
#define LEDC_TIMER_13_BIT  13

// use 5000 Hz as a LEDC base frequency
#define LEDC_BASE_FREQ     5000


#define dir1Pin        14      // GPIO pin used to Dir1
#define dir2Pin        12      // GPIO pin used to Dir2
#define servoPin       15      // GPIO pin used to connect the servo control (digital out)
#define lightPin       34

BLECharacteristic *pCharacteristic1;
BLECharacteristic *pCharacteristic2;
BLECharacteristic *pCharacteristic3;

bool deviceConnected = false;
int lightValue = 0;

int servo = 0;
int motor = 0;

float temperature=0.0;

Servo myservo;

// See the following for generating UUIDs:
// https://www.uuidgenerator.net/

#define SERVICE_UUID          "4fafc201-1fb5-459e-8fcc-c5c9c331914b"
#define CHARACTERISTIC_UUID1  "beb5483e-36e1-4688-b7f5-ea07361b26a8"
#define CHARACTERISTIC_UUID2  "beb5483e-36e1-4688-b7f5-ea07361b26a9"
#define CHARACTERISTIC_UUID3  "beb5483e-36e1-4688-b7f5-ea07361b26aA"

class MyServerCallbacks: public BLEServerCallbacks {
    void onConnect(BLEServer* pServer) {
      deviceConnected = true;
    };

    void onDisconnect(BLEServer* pServer) {
      deviceConnected = false;
    }
};

class MyCallbacks: public BLECharacteristicCallbacks {
    void onWrite(BLECharacteristic *pCharacteristic) {
      std::string rxValue = pCharacteristic->getValue();
      String cadena = rxValue.c_str();
      if (rxValue.length() > 0) {
        int intcadena = cadena.toInt();
        motor = intcadena / 1000;
        servo = (intcadena - motor * 1000) ;
        Serial.print("Motor: ");
        Serial.print(motor);
        Serial.print(" Servo: ");
        Serial.println(servo);
        if (motor == 50) {
          ledcWrite(DIR1_CHANNEL, 0);
          ledcWrite(DIR2_CHANNEL, 0);
        }
        else if (motor < 50) {
          ledcWrite(DIR1_CHANNEL, 0);
          ledcWrite(DIR2_CHANNEL, map(motor, 50, 100, 0, 8191));
        }
        else if (motor > 50) {
          ledcWrite(DIR1_CHANNEL, map(motor, 50, 0, 0, 8191));
          ledcWrite(DIR2_CHANNEL, 0);
        }
      myservo.write(map(servo, 0, 100, 50, 130));

      }
    }
};


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


  Serial.println( myservo.attach(servoPin, 500, 2400));
  
  //Setup PWM Channel for Dir1
  ledcSetup(DIR1_CHANNEL, LEDC_BASE_FREQ, LEDC_TIMER_13_BIT);
  ledcAttachPin(dir1Pin, DIR1_CHANNEL);

  //Setup PWM Channel for Dir2
  ledcSetup(DIR2_CHANNEL, LEDC_BASE_FREQ, LEDC_TIMER_13_BIT);
  ledcAttachPin(dir2Pin, DIR2_CHANNEL);

  //Setup Servo
  

  //Stop Motors and Center
  ledcWrite(DIR1_CHANNEL, 0);
  ledcWrite(DIR2_CHANNEL, 0);
  myservo.write(90);

  // Create the BLE Device
  BLEDevice::init("DeustoAstroBot");

  // Create the BLE Server
  BLEServer *pServer = BLEDevice::createServer();
  pServer->setCallbacks(new MyServerCallbacks());

  // Create the BLE Service
  BLEService *pService = pServer->createService(SERVICE_UUID);

  // Create a BLE Characteristic
  pCharacteristic1 = pService->createCharacteristic(
                       CHARACTERISTIC_UUID1,
                       BLECharacteristic::PROPERTY_READ
                     );
  pCharacteristic2 = pService->createCharacteristic(
                       CHARACTERISTIC_UUID2,
                       BLECharacteristic::PROPERTY_WRITE |
                       BLECharacteristic::PROPERTY_READ
                     );
  pCharacteristic3 = pService->createCharacteristic(
                       CHARACTERISTIC_UUID3,
                       BLECharacteristic::PROPERTY_READ   |
                       BLECharacteristic::PROPERTY_NOTIFY |
                       BLECharacteristic::PROPERTY_INDICATE
                     );

  // https://www.bluetooth.com/specifications/gatt/viewer?attributeXmlFile=org.bluetooth.descriptor.gatt.client_characteristic_configuration.xml
  // Create a BLE Descriptor
  pCharacteristic1->addDescriptor(new BLE2902());
  pCharacteristic2->addDescriptor(new BLE2902());
  pCharacteristic3->addDescriptor(new BLE2902());

  pCharacteristic2->setCallbacks(new MyCallbacks());
  // Start the service
  pService->start();

  // Start advertising
  pServer->getAdvertising()->start();
  Serial.println("Waiting a client connection to notify...");
}

void loop() {

  if (deviceConnected) {
    lightValue=analogRead(lightPin);
    Serial.printf("*** NOTIFY: %d ***\n", lightValue);
    String valueTexto = (String) lightValue;
    temperature=(temprature_sens_read() - 32) / 1.8;
    String tempTexto=(String)temperature;
    String texto=tempTexto+";"+valueTexto;
    pCharacteristic3->setValue(texto.c_str());
    pCharacteristic3->notify();

  }
  delay(2000);
}