#include <Servo.h>
//#include <ArduinoBLE.h> 

#define PIN_TRIG 13
#define PIN_ECHO 12
#define PIN_SERVO 11

const float BETA = 3950;
Servo myServo;  // Create servo object to control a servo

// int predictiveLogic(int distance, float temperature) {
//   return map(distance, 0, 200, 0, 180);
// }

// Placeholder for TerrainMap class or data structure
// class TerrainMap {
//   public:
//     void init() {
//       // Initialization code here
//     }
//
//     void update(int distance, float temp) {
//       // Update the map with new sensor readings
//     }
//
//     int predictiveServoAngle() {
//       return 90; 
//     }
// };

// TerrainMap terrainMap;
//BLEService terrainService("1234");
//BLEStringCharacteristic terrainMapCharacteristic("2345", BLERead | BLENotify, sizeof(TerrainMap)); 



void setup() {
  Serial.begin(115200);
  pinMode(PIN_TRIG, OUTPUT);
  pinMode(PIN_ECHO, INPUT);

  myServo.attach(PIN_SERVO); // Attaches the servo on pin 11

  // Begin BLE initialization
  /* if (!BLE.begin()) {
    Serial.println("starting BLE failed!");
    while (1);
  }

  BLE.setLocalName("TerrainMapDevice");
  BLE.setAdvertisedService(terrainService);
  terrainService.addCharacteristic(terrainMapCharacteristic);
  BLE.addService(terrainService);
  BLE.advertise();
  Serial.println("Bluetooth device active, waiting for connections..."); */
}

void loop() {
  // Start a new measurement:
  digitalWrite(PIN_TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(PIN_TRIG, LOW);

  // Read the result:
  long duration = pulseIn(PIN_ECHO, HIGH);
  int distance_cm = duration / 58;
  int distance_in = duration / 148;

  Serial.print("Distance in CM: ");
  Serial.println(distance_cm);

  int servoPos = map(distance_cm, 0, 200, 0, 180);
  servoPos = constrain(servoPos, 0, 180);
  myServo.write(servoPos); 
  
  int analogValue = analogRead(A0);
  float celsius = 1 / (log(1 / (1023. / analogValue - 1)) / BETA + 1.0 / 298.15) - 273.15;
  Serial.print("Temperature: ");
  Serial.print(celsius);
  Serial.println(" ℃");

  // Update the terrain map with the new readings
  // terrainMap.update(distance_cm, celsius);

  // Retrieve predictive logic from the terrain map
  // int predictiveAngle = terrainMap.predictiveServoAngle();

  // Placeholder for predictive logic function (you will need to implement this logic)
  // int servoPos = predictiveLogic(distance_cm, celsius);
  
  // Use the predictive logic to move the servo
  // servoPos = constrain(servoPos, 0, 180); // Ensure the angle is within servo's range
  // myServo.write(servoPos);

  // terrainMapCharacteristic.writeValue(terrainMap);
  }