#define BLYNK_TEMPLATE_ID "TMPL6TSIth64N"
#define BLYNK_TEMPLATE_NAME "ROBOX Sky  "
#define BLYNK_AUTH_TOKEN "9Vwu6rwn6OO_3vFIKsK__i_uB-eZcCDA"
#define BLYNK_PRINT Serial

#include <WiFi.h>
#include <WiFiClient.h>
#include <BlynkSimpleEsp32.h>
#include <ESP32Servo.h>

#include <LiquidCrystal_I2C.h>
#include "DHT.h"

#define DHTPIN 5 // Digital pin connected to the DHT sensor
#define DHTTYPE DHT22 // DHT 22 (AM2302), AM2321
const int trigPin = 19;
const int echoPin = 18;

const int dataPin = 2;   /* DS */
const int clockPin = 16; /* SHCP */
const int latchPin = 17; /* STCP */

const int Pin_Arm = 25;
const int Pin_Left = 26;
Servo servo_Arm;
Servo servo_Left;
int pos_Arm = 0;
int pos_Left = 0;

char auth[] = BLYNK_AUTH_TOKEN;

// Your WiFi credentials.
// Set password to "" for open networks.
char ssid[] = "Wokwi-GUEST";
char pass[] = "";

BlynkTimer timer;

int sw1_state = 0;
int sw2_state = 0;
int sw3_state = 0;
int sw4_state = 0;

#define button1_vpin    V0
#define button2_vpin    V1
#define button3_vpin    V2
#define button4_vpin    V3


//the binary numbers of 0-9 are initialized in an array
int dataArray[15]=
{0B00000000,// bit[0] stop
0B00000100, // bit[1] M1 forward
0B00001000, // bit [2] M1 reward
0B00000010, // bit[3] M2 forward
0B00010000, // bit [4] M2 reward
0B00000001, // bit[5] M4 forward
0B01000000, // bit [6] M4 reward
0B00100000, // bit[7] M3 forward
0B10000000, // bit [8] M3 reward
0B00100111,  // bit [9] Robot forward
0B11011000,  // bit [10] Robot rerward
0B00000110,  // bit [11] Robot turn right forward M1, M2 forward
0B00100001,  // bit [12] Robot turn left forward M3, M4 forward
0B00011000,  // bit [13] Robot turn right reward M1, M2 reward
0B11000000  // bit [14] Robot turn left reward M3, M4 reward
};

#define Sound_Speed 0.034
#define CM_TO_INCH 0.393701

long duration;
int distanceCm;
float distanceInch;

DHT dht(DHTPIN, DHTTYPE);
LiquidCrystal_I2C LCD = LiquidCrystal_I2C(0x27, 20, 4);

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  Serial.println("Hello, Paramat Kainaw");
  LCD.init();
  LCD.backlight();
  LCD.setCursor(6, 0);
  LCD.print("--Boss--");
  pinMode(trigPin, OUTPUT); // Sets the trigPin as an Output
  pinMode(echoPin, INPUT); // Sets the echoPin as an Input
  pinMode(dataPin,OUTPUT);
  pinMode(latchPin,OUTPUT);
  pinMode(clockPin,OUTPUT);
  dht.begin();
  //////////////////////////////////////
    delay(500);
    LCD.clear();
  servo_Arm.attach(Pin_Arm, 500, 2400);
  servo_Left.attach(Pin_Left, 500, 2400);
  servo_Arm.write(pos_Arm);
  servo_Left.write(pos_Left);
  delay(500);
  servo_Arm.write(pos_Arm+90);
  servo_Left.write(pos_Left+90);
  delay(500);
  Blynk.begin(auth, ssid, pass);

}
void loop() {
  // put your main code here, to run repeatedly:
  ultrasonic_sensor();
  DHT22_sensor();
  delay(10); // this speeds up the simulation
    for(int index=0;index<15;index++)
  {
    digitalWrite(latchPin,LOW);
    shiftOut(dataPin,clockPin,MSBFIRST,dataArray[index]);
    digitalWrite(latchPin,HIGH);
    display(index);
  }
  Blynk.run();
  timer.run();

  if (sw1_state==HIGH){
    servo_Arm_Select();
  }
    else if (sw2_state==HIGH){
    servo_Arm_Unselect();
  }
  if (sw3_state==HIGH){
    servo_Left_UP();
  }
    else if (sw4_state==HIGH){
    servo_Left_Down();
  }

  // put your main code here, to run repeatedly:
  delay(10); // this speeds up the simulation
}
void  ultrasonic_sensor(){
  // Claes the trigPin
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  // Sets the trigPin on HIGH state for 10 micro seconds
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  //Reads the echoPin, returns the sound wave travel time in microseconds
  duration = pulseIn(echoPin, HIGH);
  // Calculate the distance
  distanceCm = duration * Sound_Speed/2;
  
  // Convert to inches
  distanceInch = distanceCm * CM_TO_INCH;
  
  // Prints the distance in the Serial Monitor
  Serial.print("Distance (cm): ");
  Serial.println(distanceCm);
  LCD.setCursor(1, 3);
  LCD.print("Distance (cm): ");
  LCD.println(distanceCm);
  // Serial.print("Distance (inch): ");
  // Serial.println(distanceInch);
  delay(900);
}
  void DHT22_sensor(){
    // Wait a few seconds between measurements.
     delay(2000);
 
    // Reading temperature or humidity takes about 250 milliseconds!
    // Sensor readings may also be up to 2 seconds 'old' (its a very slow sensor)
   int h = dht.readHumidity();
   // Read temperature as Celsius (the default)
   int t = dht.readTemperature();
   // Read temperature as Fahrenheit (isFahrenheit = true)
   float f = dht.readTemperature(true);
 
   // Check if any reads failed and exit early (to try again).
   if (isnan(h) || isnan(t) || isnan(f)) {
  Serial.println(F("Failed to read from DHT sensor!"));
  return;
}
 
   // Compute heat index in Fahrenheit (the default)
   float hif = dht.computeHeatIndex(f, h);
   // Compute heat index in Celsius (isFahreheit = false)
   float hic = dht.computeHeatIndex(t, h, false);
 
   Serial.print(F("Humidity: "));
   Serial.print(h);
   Serial.print(F("% Temperature: "));
   Serial.print(t);
   Serial.print(F(" C "));
   Serial.print(f);
   Serial.print(F(" F Heat index: "));
   Serial.print(hic);
   Serial.print(F(" C "));
   Serial.print(hif);
   Serial.println(F(" F"));

   LCD.setCursor(17, 0);
   LCD.print(t);
   LCD.setCursor(19, 0);
   LCD.print(F("C"));

   LCD.setCursor(17, 1);
   LCD.print(h);
   LCD.setCursor(19 , 1);
   LCD.print(F("%"));

  }
     void display(int b){
    if(b==0){
      Serial.print(b);
      Serial.println("-->>0B00000000");
      LCD.setCursor(0, 1);
      LCD.println("--Stop--    ");
      LCD.setCursor(0, 2);
      LCD.println("                ");
      delay(300);
      
    }
    else if(b==1){
      Serial.print(b);
      Serial.println("-->>0B00000100");
      LCD.setCursor(0, 1);
      LCD.println("--M1forward-");
      delay(300);
    }
    else if(b==2){
      Serial.print(b);
      Serial.println("-->>0B00001000");
      LCD.setCursor(0, 1);
      LCD.println("--M1reward-");
      delay(300);
    }
    else if(b==3){
      Serial.print(b);
      Serial.println("-->>0B00000010");
      LCD.setCursor(0, 1);
      LCD.println("--M2forward-");
      delay(300);
    }
    else if(b==4){
      Serial.print(b);
      Serial.println("-->>0B00010000");
      LCD.setCursor(0, 1);
      LCD.println("--M2reward-");
      delay(300);
    }
    else if(b==5){
      Serial.print(b);
      Serial.println("-->>0B00000001");
      LCD.setCursor(0, 1);
      LCD.println("--M4forward-");
      delay(300);
    }
    else if(b==6){
      Serial.print(b);
      Serial.println("-->>0B01000000");
      LCD.setCursor(0, 1);
      LCD.println("--M4reward-");
      delay(300);
    }
    else if(b==7){
      Serial.print(b);
      Serial.println("-->>0B00100000");
      LCD.setCursor(0, 1);
      LCD.println("--M3forward-");
      delay(300);
    }
    else if(b==8){
      Serial.print(b);
      Serial.println("-->>0B10000000");
      LCD.setCursor(0, 1);
      LCD.println("--M3reward-");
      delay(300);
    }
    else if(b==9){
      Serial.print(b);
      Serial.println("-->>0B00100111");
      LCD.setCursor(0, 1);
      LCD.println("-Robotforward-");
      delay(300);
    }
    else if(b==10){
      Serial.print(b);
      Serial.println("-->>0B11011000");
      LCD.setCursor(0, 1);
      LCD.println("-Robotrerward-");
      delay(300);
    }
    else if(b==11){
      Serial.print(b);
      Serial.println("-->>0B00000110");
      LCD.setCursor(0, 1);
      LCD.println(" Robotturnright");
      LCD.setCursor(0, 2);
      LCD.println("forwardM1 M2forward");
      delay(300);
    }
    else if(b==12){
      Serial.print(b);
      Serial.println("-->>0B00100001");
      LCD.setCursor(0, 1);
      LCD.println(" Robotturnleft");
      LCD.setCursor(0, 2);
      LCD.println("forwardM3 M4forward");
      delay(300);
    }
    else if(b==13){
      Serial.print(b);
      Serial.println("-->>0B00011000");
      LCD.setCursor(0, 1);
      LCD.println(" Robotturnright");
      LCD.setCursor(0, 2);
      LCD.println("rewardM1 M2reward");
      delay(300);
    }
    else if(b==14){
      Serial.print(b);
      Serial.println("-->>0B11000000");
      LCD.setCursor(0, 1);
      LCD.println(" Robotturnleft");
      LCD.setCursor(0, 2);
      LCD.println("rewardM3 M4reward");
      delay(300);
    }
    delay(2000); 

  }
void servo_Arm_Select(){
    if (pos_Arm<180){
    servo_Arm.write(pos_Arm);
    delay(10);
    pos_Arm+=1; 
    }
}
void servo_Arm_Unselect(){
    if (pos_Arm>0){
    servo_Arm.write(pos_Arm);
    delay(10);
    pos_Arm-=1; 
    }
}
void servo_Left_UP(){
    if (pos_Left<180){
    servo_Left.write(pos_Left);
    delay(10);
    pos_Left+=1; 
    }
}
void servo_Left_Down(){
    if (pos_Left>0){
    servo_Left.write(pos_Left);
    delay(10);
    pos_Left-=1; 
    }
}