#include <WiFi.h>
#include <PubSubClient.h>
 #include <FastLED.h>
 #include <LiquidCrystal.h>
 LiquidCrystal lcd(43, 45, 47, 49, 51, 53);
#define NUM_LEDS  18
#define LED_PIN   9
CRGB leds[NUM_LEDS];
 #include "Wire.h"
 #include <MPU6050_light.h>
 MPU6050 mpu(Wire);
 unsigned long timer = 0;
const int Motor_pin=50;
bool Direction= 52;
String ESPData;
const int OUT_PIN = 8;
const int SAMPLE_TIME = 10;
unsigned long millisCurrent;
unsigned long millisLast = 0;
unsigned long millisElapsed = 0;
int sampleBufferValue = 0;
int i=0;
int pir=A2;
int TiltSwitch=3;
int Count =0;


void setup() {
  Serial.begin(9600);
  Serial1.begin(9600);
  pinMode(A0, INPUT);
FastLED.addLeds<WS2812B, LED_PIN, GRB>(leds, NUM_LEDS);
FastLED.setBrightness(50);
pinMode(LED_PIN,OUTPUT);
fill_solid(leds, NUM_LEDS, CRGB::Black);
     FastLED.show();
    lcd.begin(16, 2);
       Wire.begin();
 byte status = mpu.begin();
pinMode(Motor_pin,OUTPUT);
pinMode(Direction,OUTPUT);
pinMode(pir,INPUT);
 pinMode(TiltSwitch,INPUT);
}


void Tilt() {
    long StartMillis;
long  CurrentMillis;
StartMillis=millis();
 CurrentMillis = millis(); //get the current "time" (actually the number of milliseconds since the program started)
 long ElapsedTime= CurrentMillis- StartMillis;
 while (ElapsedTime <= 10000)  //test whether the period has elapsed
  {
int val=digitalRead(TiltSwitch);
Serial.println(ElapsedTime);
  if (val==1){
    Count++;
    int LED_out=map(Count,0,1000,0,18);
   for (int i=0; i<18; i++){
  if (i<7){
    leds[i] = CRGB::Red;
  }else if(i<14){
    leds[i] = CRGB::Orange; 
  }else {
    leds[i] = CRGB::White;
  }
  }
 for (int i=LED_out; i<18;i++){
  leds[i] = CRGB::Black;
 }
 FastLED.show(); 
  }
  CurrentMillis=millis();
ElapsedTime= CurrentMillis- StartMillis;
}
   lcd.clear();
lcd.print("DONE");
Serial.println("Done");
 fill_solid(leds, NUM_LEDS, CRGB::Black);
  FastLED.show();
}
void ForceSensor()
{
  long StartMillis;
long  CurrentMillis; 
StartMillis=millis();
 CurrentMillis = millis();//get the current "time" (actually the number of milliseconds since the program started)
 long ElapsedTime= CurrentMillis- StartMillis;
 Serial.println(ElapsedTime);
  while (ElapsedTime <= 10000)  //test whether the period has elapsed
  {
  int ForceSensor1 =analogRead(A0);
   int LED_out=map(ForceSensor1,0,800,0,18);
    LEDBarGraph(LED_out); 
    CurrentMillis=millis();
   ElapsedTime= CurrentMillis- StartMillis;
}
 lcd.clear();
lcd.print("DONE");
  fill_solid(leds, NUM_LEDS, CRGB::Black);
     FastLED.show();
Serial.print("Done");
}


void SoundSensor() {
  long StartMillis;
long  CurrentMillis; 
StartMillis=millis();
 CurrentMillis = millis(); //get the current "time" (actually the number of milliseconds since the program started)
 long ElapsedTime= CurrentMillis- StartMillis;
 Serial.println(ElapsedTime);
  while (ElapsedTime <= 10000)  //test whether the period has elapsed
  {
  millisCurrent = millis();
  millisElapsed = millisCurrent - millisLast;
  if (digitalRead(OUT_PIN) == LOW) {
    sampleBufferValue++;
  }
  if (millisElapsed > SAMPLE_TIME) {
    Serial.println(sampleBufferValue);
     int LED_out=map(sampleBufferValue,0,600,0,18);
    sampleBufferValue = 0;
    millisLast = millisCurrent;
 LEDBarGraph(LED_out);
  }
CurrentMillis=millis();
ElapsedTime= CurrentMillis- StartMillis;
}
Serial.print("Done");
}


void Ultrasonic() {
long StartMillis;
long  CurrentMillis; //the value is a number of milliseconds
int trigPin = 11;    // Trigger
int echoPin = 12;    // Echo
long duration, cm, inches;
StartMillis=millis();
 CurrentMillis = millis(); //get the current "time" (actually the number of milliseconds since the program started)
 long ElapsedTime= CurrentMillis- StartMillis;
 Serial.println(ElapsedTime);
  while (ElapsedTime <= 10000)  //test whether the period has elapsed
  {
   //Define inputs and outputs
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  // The sensor is triggered by a HIGH pulse of 10 or more microseconds.
  // Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
  digitalWrite(trigPin, LOW);
  delayMicroseconds(5);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  // Read the signal from the sensor: a HIGH pulse whose
  // duration is the time (in microseconds) from the sending
  // of the ping to the reception of its echo off of an object.
  pinMode(echoPin, INPUT);
  duration = pulseIn(echoPin, HIGH);
   // Convert the time into a distance
  inches = (duration/2) / 74;   // Divide by 74 or multiply by 0.0135
 RGBControl(inches); 
 CurrentMillis=millis();
   ElapsedTime= CurrentMillis- StartMillis;
 Serial.println(ElapsedTime);
  }
     fill_solid(leds, NUM_LEDS, CRGB::Black);
     FastLED.show();
Serial.print("Done");
lcd.clear();
lcd.print("Done");
}


void RGBControl(int inches)
{
  switch (inches) {
  case 10:
    //do something when var equals 1
  fill_solid(leds, NUM_LEDS, CRGB::Blue);
  FastLED.show();
    break; 
  case 6:
    //do something when var equals 2
    fill_solid(leds, NUM_LEDS, CRGB::Red);
  FastLED.show();
    break;
    case 2:
    //do something when var equals 2
     fill_solid(leds, NUM_LEDS, CRGB::Green);
  FastLED.show();
    break;
    FastLED.show();
}
}


void LEDBarGraph(int LED_out)
{
 for (int i=0; i<18; i++){
  if (i<7){
    leds[i] = CRGB::Red;
  }else if(i<14){
    leds[i] = CRGB::Orange; 
  }else {
    leds[i] = CRGB::White;
  }
  }
 for (int i=LED_out; i<18;i++){
  leds[i] = CRGB::Black;
 }
 FastLED.show();
}


void PrintToLCD() {  
    if (Serial1.available()) {        
      ESPData = Serial1.readString();
      Serial.print(ESPData);

      if (ESPData.indexOf("L") >= 0) {
        lcd.clear();
lcd.print("LEFT");
      }   
       if (ESPData.indexOf("R") >= 0) 
     {
     lcd.clear();
lcd.print("RIGHT");

    }   
      if (ESPData.indexOf("D") >= 0) 
     {
      lcd.clear();
lcd.print("DOWN");

}      
       if (ESPData.indexOf("U") >= 0) 
     {
      lcd.clear();
lcd.print("UP");
     }
      if (ESPData.indexOf("sensor1") >= 0) {
        lcd.clear();
lcd.print("ForceSensor");
delay(10);
        //Use Force Sensor
    ForceSensor();
   }
   
 if (ESPData.indexOf("sensor2") >= 0) {
            //Use Ultrasonic Sensor
            Serial.println("Ultrasonic");
              lcd.clear();
lcd.print("UltrasonicSensor");
delay(10);
    Ultrasonic();
     }
 if  (ESPData.indexOf("sensor3") >= 0) {
      lcd.clear();
lcd.print("MPU");
delay(10);
                 // Use Accelerometer 
    MPU();
       }
 if  (ESPData.indexOf("sensor4") >= 0) {
      lcd.clear();
lcd.print("SoundSensor");
delay(10);
  // Use Sound Sensor 
    SoundSensor();
         
         }
 if  (ESPData.indexOf("sensor5") >= 0) { 
      lcd.clear();
lcd.print("     PIR");
delay(10);
      // Use Motion Sensor
    PIR();
           }
   
 if  (ESPData.indexOf("sensor6") >= 0) {
      lcd.clear();
lcd.print("Tilt Switch");
delay(10);
// Use tilt Switch
    Tilt();
               }
//      if (ESPData.indexOf("sensor2") >= 0) {
//            //Use Ultrasonic Sensor
//            Serial.println("Ultrasonic");
//              lcd.clear();
//lcd.print("UltrasonicSensor");
//delay(10);
//    Ultrasonic();
//     }
     
        }
}




 void MPU() {
 long StartMillis;
long  CurrentMillis;  //get the current "time" (actually the number of milliseconds since the program started)

 StartMillis=millis();
 CurrentMillis = millis(); 
  long ElapsedTime= CurrentMillis- StartMillis;
 Serial.println(ElapsedTime);
  while (ElapsedTime <= 10000)  //test whether the period has elapsed
  {
   mpu.update();
 if ((millis() - timer) > 10) { //use data every 10ms
    int yaw =mpu.getAngleY();
    Serial.println(yaw);
     timer = millis();
     int PWM = map(yaw,0,100,0,255);
     if (yaw<10 && yaw>-10){
      digitalWrite(Motor_pin,LOW);
     }
  if (yaw>10){
   digitalWrite(Direction,HIGH);
digitalWrite(Motor_pin,PWM);
   }
   if (yaw<-10){
    digitalWrite(Direction,LOW);
    digitalWrite(Motor_pin,PWM);
  }
 }
  CurrentMillis=millis();
   ElapsedTime= CurrentMillis- StartMillis;
 }
 digitalWrite(Motor_pin,LOW);
  lcd.clear();
lcd.print("DONE");
 Serial.print("Done");
 }

 
 void PIR() {
long StartMillis;
long  CurrentMillis;
StartMillis=millis();
 CurrentMillis = millis(); //get the current "time" (actually the number of milliseconds since the program started)
 long ElapsedTime= CurrentMillis- StartMillis;
 Serial.println(ElapsedTime);
  while (ElapsedTime <= 10000)  //test whether the period has elapsed
  {
  // put your main code here, to run repeatedly:
   if ((millis() - timer) > 10) { //use data every 10ms
int val=analogRead(pir);
Serial.println(val);
timer = millis();
digitalWrite(Motor_pin,LOW);
if (val>700){
digitalWrite(Motor_pin,HIGH);
digitalWrite(Direction, HIGH);
Serial.println(val);
}}
CurrentMillis=millis();
ElapsedTime= CurrentMillis- StartMillis;
 }
 Serial.print("Done");
}


void SensorDistrubution(){
if (Serial1.available()) {        
      ESPData = Serial1.readString();
      Serial.print(ESPData);  
   if (ESPData.indexOf("sensor1") >= 0) {
        lcd.clear();
lcd.print("ForceSensor");
delay(10);
        //Use Force Sensor
    ForceSensor();
   }
   
 if (ESPData.indexOf("sensor2") >= 0) {
            //Use Ultrasonic Sensor
            Serial.println("Ultrasonic");
              lcd.clear();
lcd.print("UltrasonicSensor");
delay(10);
    Ultrasonic();
     }
 if  (ESPData.indexOf("sensor3") >= 0) {
      lcd.clear();
lcd.print("MPU");
delay(10);
                 // Use Accelerometer 
    MPU();
       }
 if  (ESPData.indexOf("sensor4") >= 0) {
      lcd.clear();
lcd.print("SoundSensor");
delay(10);
  // Use Sound Sensor 
    SoundSensor();
         
         }
 if  (ESPData.indexOf("sensor5") >= 0) { 
      lcd.clear();
lcd.print("     PIR");
delay(10);
      // Use Motion Sensor
    PIR();
           }
   
 if  (ESPData.indexOf("sensor6") >= 0) {
      lcd.clear();
lcd.print("Tilt Switch");
delay(10);
// Use tilt Switch
    Tilt();
               }
}
}

void loop() {
PrintToLCD();
//SensorDistrubution();
}