// C++ code
//
#include <DHT.h>
#include <Adafruit_MLX90614.h>

#define echoPin 2
#define trigPin 3
#define DHTPIN 8
#define DHTTYPE DHT22
#define feedbackPin 5
#define inputTypePin 11
#define proximityLED 10
#define temperatureLED 6
#define MIN_TEMP 27
#define MAX_TEMP 35
#define MAX_DIST 80
#define MIN_DIST 5

// Global variables
DHT dht(DHTPIN, DHTTYPE);
Adafruit_MLX90614 mlx = Adafruit_MLX90614();
float duration, distance, celsius, temp, hum, speed_of_sound = 343;
unsigned char twoSecondsPassed = 2, isProximitySelected = 0;

// Receive distance from ultrasonic proximity sensor
void ultrasonic_proximity()
{
  // Send pulse
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  // Receive back pulse
  duration = pulseIn(echoPin, HIGH);

  // Measure distance
  distance = (duration / 2) * speed_of_sound / 10000;
}

// Configure Timer1 to stop every 2.8s
void configure_timer1()
{
  // Clear registers
  TCCR1A = 0;
  TCCR1B = 0;
  // Clear internal counter
  TCNT1 = 0;
  // Set top value (CTC mode will compare OCR1A)
  OCR1A = 0xAE63;
  // CTC mode
  TCCR1B |= (1 << WGM12);
  // 1024 prescaler
  TCCR1B |= (1 << CS12) | (1 << CS10);
}

// Enable timer compare interrupt
void init_timer1() {
  TIMSK1 |= (1 << OCIE1A);  
}

// Reads temperature & humidity every 5.6s using Timer1 intrerupt
ISR(TIMER1_COMPA_vect) {
  // If 5 seconds have elapsed -> read temp & humidity
  if (twoSecondsPassed >= 2) {
    temp = dht.readTemperature();
    hum = dht.readHumidity();
    Serial.print("temp: ");
    Serial.println(temp);
    Serial.print("hum: ");
    Serial.println(hum);

    // Recompute speed of sound
    speed_of_sound = 331.4f + (0.606f * temp) + (0.0124f * hum);
    
    twoSecondsPassed = 0;
  } else {
    ++twoSecondsPassed;
  }
}

// Configures devices and GPIO pins
void setup()
{
  // Disable intreruptions
  cli();
  
  Serial.begin(9600);
  pinMode(LED_BUILTIN, OUTPUT);
  pinMode(proximityLED, OUTPUT);
  pinMode(temperatureLED, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(trigPin, OUTPUT);
  pinMode(feedbackPin, OUTPUT);
  pinMode(inputTypePin, INPUT);
  dht.begin();

  // 2.8s seconds timer initialization
  configure_timer1();
  init_timer1();

  // Enable intreruptions
  sei();

  // I2C
  mlx.begin();
}

// Sends output signal as a PWM signal to the output source
void send_output_signal(int high, int low) {
  for (int i = 0; i < 100; i++) { // make a sound
      digitalWrite(feedbackPin, HIGH); // send high signal to output
      delay(high); // delay 1ms
      digitalWrite(feedbackPin, LOW); // send low signal to output
      delay(low);
  }
}

// Runs continuously the Blind Assist Algorithm
void loop()
{
  isProximitySelected = digitalRead(inputTypePin);

  // Select which LED to light based on the slideswitch
  if (isProximitySelected) {
    digitalWrite(proximityLED, HIGH);
    digitalWrite(temperatureLED, LOW);
  } else {
    digitalWrite(temperatureLED, HIGH);
    digitalWrite(proximityLED, LOW);
  }

  if (isProximitySelected) {
    // Read distance
    ultrasonic_proximity();

    // Check if distance is valid
    Serial.print("Distance=");
    if (distance >= 400 || distance <= 2) {
      Serial.print("Out of range.\n");
    } else {
      Serial.print(distance);
      Serial.print(" cm.\n");

      // Check if distance is too close to the user
      if (distance <= MAX_DIST) {
        // Create variable frequency based on signal
        int high = map(distance, MIN_DIST, MAX_DIST, 2, 10);
        int low = map(distance, MIN_DIST, MAX_DIST, 1, 3);

        // Send signal to output
        send_output_signal(high, low);
      }
    }
  } else {
    // Read IR temperature
    float tempIR = mlx.readObjectTempC();
    
    Serial.print("Ambient = "); 
    Serial.print(mlx.readAmbientTempC()); 
    Serial.print("*C\tObject = "); 
    Serial.print(tempIR); Serial.println("*C");

    // Check if temperature is too hot for the user
    if (tempIR >= MIN_TEMP)  {
      // Create variable frequency based on signal
      int high = map(tempIR, MIN_TEMP, MAX_TEMP, 10, 2);
      int low = map(tempIR, MIN_TEMP, MAX_TEMP, 3, 1);

      // Send signal to output
      send_output_signal(high, low);
    }
  }

  // Wait 100 miliseconds before polling data again
  delay(100);
}