/*

  Investigation I - Creative Project: RME 2023 Spring
  Author: Hongfei Ji
  Andrew ID: hongfeij

*/
#include <Servo.h>
#include <Arduino_LPS22HB.h>

float mapFloat(float x, float in_min, float in_max, float out_min, float out_max)
{
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

Servo myservo;
int pos;
boolean isCounterwise = false;
boolean isHere;
float peakTemp = 0;
float sootheVal = 0;

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

  if (!BARO.begin()) {
    Serial.println("Failed to initialize pressure sensor!");
    while (1);
  }

  myservo.attach(D9);

}

void loop() {

  float temperature = BARO.readTemperature();

  Serial.print("Temperature = ");

  float valMapped = constrain(temperature, 26, 30);
  float mappedTime = mapFloat(valMapped, 26, 30, 490, 10);
  Serial.print(valMapped);
  Serial.println(" C");
  Serial.println();

  Serial.print("Frequency: ");
  Serial.print(1000 / mappedTime);
  Serial.println(" times/s.");

  if( Serial.available() ){
    // Judge if the user is in front of the screen
    String incoming = Serial.readString();
    Serial.print("Received Data => ");
    Serial.println(incoming);
    
    if( incoming.startsWith( "Human Detected" ) > 0) {
      isHere = true;
    } else {
      isHere = false;
    }
  }
  
  if (isHere == true) {
    // Red Signal for warning
    digitalWrite(LEDR, LOW);
    delay(10);
    digitalWrite(LEDR, HIGH);
    delay(10);
    peakTemp = max(valMapped, peakTemp);
    sootheVal = peakTemp - valMapped;
    Serial.println(sootheVal);

    // Making noise when temperature surpasses the threshold
    if (valMapped >= 28 && sootheVal < 0.25) {
        myservo.write(pos); 
        if (pos == 180 || pos == 0) {
          isCounterwise = !isCounterwise;
        }
        if (pos >= 0 && pos <= 180 && isCounterwise == true) {
          pos = pos + 6;
        }
        if (pos >= 0 && pos <= 180 && isCounterwise == false) {
          pos = pos - 6;
        }
    }
  } else {
    // Peace state or being soothe
    digitalWrite(LEDG, LOW);
    delay(5);
    digitalWrite(LEDG, HIGH);
    delay(5);
  }
  delay(mappedTime);
}