#define BLYNK_TEMPLATE_ID "TMPL67-9cuOVS"
#define BLYNK_TEMPLATE_NAME "ROBOT"
#define BLYNK_AUTH_TOKEN "1iZot5tFjgPT0Ynf6cbSMg55sgsbsNK9"
#define BLYNK_PRINT Serial


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

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

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

 
#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;


#define SERVO_LIFT 25 // ESP32 pin GPIO26 connected to servo motor
#define SERVO_PIN 26 // ESP32 pin GPIO26 connected to servo motor


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

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


BLYNK_CONNECTED() {
  Blynk.syncVirtual(button1_vpin);
  Blynk.syncVirtual(button2_vpin);
   Blynk.syncVirtual(button3_vpin);
  Blynk.syncVirtual(button4_vpin);
}

BLYNK_WRITE(button1_vpin) {
  sw1_state = param.asInt();
  Serial.print("sw1_state-->>");
  Serial.println(sw1_state);
  
}
//--------------------------------------------------------------------------
BLYNK_WRITE(button2_vpin) {
  sw2_state = param.asInt();
  Serial.print("sw2_state-->>");
  Serial.println(sw2_state);
}
BLYNK_WRITE(button3_vpin) {
  sw1_state = param.asInt();
  Serial.print("sw3_state-->>");
  Serial.println(sw3_state);
  
}
//--------------------------------------------------------------------------
BLYNK_WRITE(button4_vpin) {
  sw2_state = param.asInt();
  Serial.print("sw4_state-->>");
  Serial.println(sw4_state);
}


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.print("KITTISAK");
  LCD.init();
  LCD.backlight();
  LCD.setCursor(3, 0);
  LCD.print("   KITTISAK    ");
  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(2000);
    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:

  void loop() {
  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_Unselect();
  }

  // put your main code here, to run repeatedly:
  delay(10); // this speeds up the simulation
}


  ultrasonic_sensor();
  dht22_sensor();
    /*for(int index=0;index<15;index++)
  {
    digitalWrite(latchPin,LOW);
    shiftOut(dataPin,clockPin,MSBFIRST,dataArray[index]);
    digitalWrite(latchPin,HIGH);
    display(index);
  }*/

  delay(10); // this speeds up the simulation
}

void display(int b){
    if(b==0){
      Serial.print(b);
      Serial.println("-->>0B00000000");
      LCD.setCursor(1, 1);
      LCD.println("--Stop--");
      delay(500);
    }
    if(b==1){
      Serial.print(b);
      Serial.println("-->>0B00000001");
      LCD.setCursor(1, 1);
      LCD.println(" M1 forward");
      delay(500);
    }
    if(b==2){
      Serial.print(b);
      Serial.println("-->>0B00000010");
      LCD.setCursor(1, 1);
      LCD.println(" M1 reward");
      delay(500);
    }
    if(b==3){
      Serial.print(b);
      Serial.println("-->>0B00000011");
      LCD.setCursor(1, 1);
      LCD.println(" M2 forward");
      delay(500);
    }
    if(b==4){
      Serial.print(b);
      Serial.println("-->>0B00000100");
      LCD.setCursor(1, 1);
      LCD.println(" M2 reward");
      delay(500);
    }
    if(b==5){
      Serial.print(b);
      Serial.println("-->>0B00000101");
      LCD.setCursor(1, 1);
      LCD.println(" M4 forward");
      delay(500);
    }
    if(b==6){
      Serial.print(b);
      Serial.println("-->>0B00000110");
      LCD.setCursor(1, 1);
      LCD.println(" M4 reward");
      delay(500);
    }
    if(b==7){
      Serial.print(b);
      Serial.println("-->>0B00000111");
      LCD.setCursor(1, 1);
      LCD.println(" M3 forward");
      delay(500);
    }
    if(b==8){
      Serial.print(b);
      Serial.println("-->>0B00001000");
      LCD.setCursor(1, 1);
      LCD.println(" M3 reward");
      delay(500);
    }
    if(b==9){
      Serial.print(b);
      Serial.println("-->>0B11111111");
      LCD.setCursor(1, 1);
      LCD.println("Robot forward");
      delay(500);
    }
    delay(1000);
}

void dht22_sensor(){
// Wait a few seconds between measurements.
delay(100);
 
// 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(15, 0);
LCD.print(t);
LCD.print(F(" C "));
LCD.setCursor(15, 1);
LCD.print(h);
LCD.print(F(" % "));
}

void ultrasonic_sensor(){
// Clears 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(100);
}
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_Unselect(){
    if (pos_Left>0){
    servo_Left.write(pos_Left);
    delay(10);
    pos_Left-=1; 
    }
}