#include "Motorz.h";
#include "tankdrive.h";
#include <NewPing.h>
#include <DHT.h>

#define DHTPIN 2
#define iterations 3
#define SONAR_L_PIN 3
#define SONAR_M_PIN 4
#define SONAR_R_PIN 5
#define MAX_DISTANCE 400

NewPing sonarL(SONAR_L_PIN,SONAR_L_PIN, MAX_DISTANCE);
NewPing sonarM(SONAR_M_PIN,SONAR_M_PIN, MAX_DISTANCE);
NewPing sonarR(SONAR_R_PIN,SONAR_R_PIN, MAX_DISTANCE);
DHT dht(DHTPIN, DHT11); 

float soundcm;
float temp=20.0;
float hum=50.0;
float duration;

unsigned long temp_startcnt;
unsigned long temp_currcnt;
int i;
int dist[3];
int enr=10;
int in1=11;
int in2=12;
int directr=1;
int enl=9;
int in3=7;
int in4=8;
int directl=0;

Motorz motorR(enr,in1,in2,directr);
Motorz motorL(enl,in3,in4,directl);
TankDrive mytank(&motorR,directr,&motorL,directl);

void setup(){
  temp_startcnt=millis();
  dht.begin();
  Serial.begin(115200);
  Serial.println("Setup");
  motorR.SetSpeed(200);
  motorL.SetSpeed(60);
  soundcm = 331.4 + (0.606 * temp) + (0.0124 * hum) ;
  soundcm=soundcm/10000;

  }


void loop()
{
  temp_currcnt=millis();
  if((temp_currcnt-temp_startcnt)>=30000){
     GetTemps();
     Serial.println();
     Serial.println();
     Serial.print("temps aquaried ");
     Serial.print(millis());
     Serial.println();
     Serial.println();
  }

  GetDistances();
  if(dist[1]>5){
  Serial.print("Moving forward\n");
  mytank.MoveForward();
  }
  else{
    Serial.print("Object detected\n");
    mytank.Stop();
  }

  /*
  mytank.MoveForward();
    delay(5000);
  mytank.Stop();
  delay(2000);
  mytank.TurnRight();
  delay(5000);
  mytank.Stop();
  delay(2000);
  mytank.MoveBack();
  delay(5000);
   mytank.Stop();
  delay(2000);
  */
}

void GetDistances(){
  /*duration = sonarL.ping_median(iterations);
  dist[0]=(duration / 2) * soundcm;
  duration = sonarM.ping_median(iterations);
  dist[1]=(duration / 2) * soundcm;
  duration = sonarR.ping_median(iterations);
  dist[2]=(duration / 2) * soundcm;  */
  duration = sonarL.ping();
  dist[0]=(duration / 2) * soundcm;
  delay(100);
  duration = sonarM.ping();
  dist[1]=(duration / 2) * soundcm;
  delay(100);
  duration = sonarR.ping();
  dist[2]=(duration / 2) * soundcm;  
  delay(100);
  Serial.print("Distance L:");
  Serial.print(dist[0]);
  Serial.print(" M:");
  Serial.print(dist[1]);
  Serial.print(" R:");
  Serial.print(dist[2]);
  Serial.print(" Temp:");
  Serial.print(temp);
   Serial.print(" Humidity:");
  Serial.print(hum);
  Serial.println();
}

void GetTemps(){
  hum = dht.readHumidity();  // Get Humidity value
  temp= dht.readTemperature(); // Get Temp value
  temp_startcnt=temp_currcnt;
  
}