#include<ESP32Servo.h>

#define SYS_PIN 14
#define SERVOPIN 12
#define TRIGPIN 18
#define ECHOPIN 5
#define CAM_SERVO_HOR 26
#define CAM_SERVO_VER 27
#define FMOT_PWM1 23
#define FMOT_EN1 24
#define RMOT_PWM1 25
#define RMOT_EN1 22
#define FMOT_PWM2 16
#define FMOT_EN2 17
#define RMOT_PWM2 18
#define RMOT_EN2 15
#define RAND_ANALOG_PIN 33
int randnum;
/* Lidar System using Ultrasonic Sensor and Servo.....
int randNum;

void setup() {
  Serial.begin(9600);

  // Seed randomness using floating analog pin
  randomSeed(analogRead(A0));
}

void loop() {
  // Random int between 0 and 99
  randNum = random(100);
  Serial.println(randNum);

  delay(300);
}

*/

class ObstacleAvoidance{
  public:
    ObstacleAvoidance(int trigpin, int echopin, int servopin){
      this->trigpin = trigpin;
      this->echopin = echopin;
      this->servopin = servopin;

      pinMode(this->trigpin, OUTPUT);
      pinMode(this->echopin, INPUT);
      myservo.attach(this->servopin);
    }
  private:
    int trigpin;
    int echopin;
    int servopin;
    Servo myservo;
};

class Sprinkler{
  public:
  private:
};

class Camera{
  public:
    Camera(int camservopinhor, int camservopinver){
      this->camservopinhor = camservopinhor;
      this->camservopinver = camservopinver;

      myservo.attach(this->camservopinhor);
      myservo1.attach(this->camservopinver);
    }
    int camprocess(){
      count = 0;
      for(int i = 30;i<=150;i=i+20){
        myservo.write(i);
        for(int j = 0;j<=180;j=j+9){
          myservo1.write(j);
          delay(400);
          if(j==45){
            j=j+90;
          }
          if(j%4==0){
            Serial.println("Cam Snap");
            count = (count +1)%36;
          }
        }
      }
      Serial.println("Scanning Complete......... Rotation Complete....");
      return count;
    }
  private:
    int caminputs[180];
    //int dangerinputs[5] = {2,3,5,30,13};
    int camservopinhor;
    int camservopinver;
    Servo myservo;
    Servo myservo1;
    int count = 0;

};

class Motion{
  public:
    Motion(int fmotpwm, int fmoten, int rmotpwm, int rmoten){
      this->fmotpwm = fmotpwm;
      this->fmoten = fmoten;
      this->rmotpwm = rmotpwm;
      this->rmoten = rmoten;

      pinMode(this->fmotpwm, OUTPUT);
      pinMode(this->fmoten, OUTPUT);
      pinMode(this->rmotpwm, OUTPUT);
      pinMode(this->rmoten, OUTPUT);
    }
  private:
    int fmotpwm;
    int fmoten;
    int rmotpwm;
    int rmoten;
};

class UGV{
  public:
    UGV(int trigpin, int echopin, int servopin, int camservopinhor, int camservopinver, int fmotpwm1,int fmoten1,int rmotpwm1,int rmoten1,int fmotpwm2,int fmoten2,int rmotpwm2,int rmoten2):oasystem(trigpin,echopin,servopin),sprinkle(),cam(camservopinhor,camservopinver),motion1(fmotpwm1,fmoten1,rmotpwm1,rmoten1),motion2(fmotpwm2,fmoten2,rmotpwm2,rmoten2){
      Serial.println("The Pesticide Sprinkling System is Initialized.......");
    }
    void PlantMonitoring(){
      snaps = cam.camprocess();
      Serial.println(snaps);
    }
    void PlantLocDetect(int randnum){
      int ver_angle = 30 + (randnum/7)*20;
      int hor_angle;
    }
  private:
    ObstacleAvoidance oasystem;
    Sprinkler sprinkle;
    Camera cam;
    Motion motion1;
    Motion motion2;
    int snaps = 0;
};

void setup(){
  Serial.begin(115200);
  Serial.println("System Initialized!!!");
  pinMode(SYS_PIN, OUTPUT);
  digitalWrite(SYS_PIN, HIGH);
  delay(500);
  digitalWrite(SYS_PIN, LOW);
  randomSeed(analogRead(RAND_ANALOG_PIN));
  //UGV MyUGV(TRIGPIN, ECHOPIN, SERVOPIN, CAM_SERVO_HOR, CAM_SERVO_VER);
}
UGV MyUGV(TRIGPIN, ECHOPIN, SERVOPIN, CAM_SERVO_HOR, CAM_SERVO_VER, FMOT_PWM1, FMOT_EN1, RMOT_PWM1, RMOT_EN1, FMOT_PWM2, FMOT_EN2, RMOT_PWM2, RMOT_EN2);
//UGV MyUGV(TRIGPIN, ECHOPIN, SERVOPIN, CAM_SERVO_HOR, CAM_SERVO_VER);

void loop(){
   MyUGV.PlantMonitoring();
   randnum = random(28);
   Serial.println(randnum);
}