#define trigPin 2
#define echoPin 4
#define ledPin 18
#include <iostream>
#include <Wire.h> 
#include <LiquidCrystal_I2C.h>
float duration;
int distance;
 
LiquidCrystal_I2C lcd(0x27,16,2); 

//explicit list (const allocator_type& alloc = allocator_type());
int numOfCupsRotatedBy = 0;
int numRevolutions = 0;

float previous_revolution_time;
float previous_distance = 100000;

float anemometer_factor = 2.2; // Need to calculate this value emperically by don't have the tools to execute this currently

void setup() {
  // put your setup code here, to run once:
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(ledPin, OUTPUT);

  //Serial.begin(9600);
  //Serial.println(" ");

  // initialize LCD
  lcd.init();
  // turn on LCD backlight                      
  lcd.backlight();
 
}

void reset_sensor(){
  // Clear the trigPin by setting it LOW:
  digitalWrite(trigPin, LOW);
  delayMicroseconds(5);
 
  // Trigger the sensor by setting the trigPin high for 10 microseconds:
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
}
 
void loop() {
  bool endOfCupRotating = false;
  bool endOfRevolution = false;

  delay(30);
  // put your main code here, to run repeatedly:
  // Clear thrigger pin and trigger sensor
  previous_revolution_time = micros();
  reset_sensor();
  // Read the echoPin, pulseIn() returns the duration (length of the pulse) in microseconds:
  float duration = pulseIn(echoPin, HIGH);
    float curr_revolution_time = micros();

  float current_distance = duration/58; // in cm
  
  if(previous_distance > current_distance){ 
    delay(8); // Because of this delay the accurateness of wind speeds measured fall within a certain range
    reset_sensor();

  // Read the echoPin, pulseIn() returns the duration (length of the pulse) in microseconds:
   duration = pulseIn(echoPin, HIGH);
       curr_revolution_time = micros();

   current_distance = duration/58; // in cm
    if (previous_distance > current_distance){
        numOfCupsRotatedBy += 1;
        endOfCupRotating = true;
    }
  }
  previous_distance = current_distance;

  if (numOfCupsRotatedBy % 3 == 1 && endOfCupRotating){
    numRevolutions += 1;
    endOfCupRotating = false;
    endOfRevolution = true;
  }

  // every 10 revolutions, update velocity
  if(endOfRevolution && numOfCupsRotatedBy >= 10){
    float avg_velocity = anemometer_factor * (numRevolutions/((curr_revolution_time - previous_revolution_time) * 10E-6));
    // LED Feedback
    if (avg_velocity > 80){
      // 80 m/s is a sample, random value if gone over then alerts us)
      digitalWrite(ledPin, HIGH);
      delay(3);
    }
    else{
      digitalWrite(ledPin, LOW);
    }

    // Print velocity to LCD
    lcd.clear();
    lcd.setCursor(0,0);
    lcd.print("Average Velocity: ");
    lcd.setCursor(0,1);
    lcd.print(avg_velocity);
    lcd.print(" m/s");
    delay(20);

    numRevolutions = 0;
    numOfCupsRotatedBy = 0;
    endOfRevolution = false;
  }
}