// by N M D Weerasinghe COTMd
//code to simulate the rotation of robot cart in an angle propotional to error in distance from wall 
//distance sensor assumed to be kept always perpendicular to wall by some other means
//codes for uv sensor copied from wokwi examples(docs)
//


#define trigPin_1   8 
#define echoPin_1   9 
#define RmotorA_pin   3 
#define RmotorB_Pin   4 
#define RmotorPWM_pin 6
#define LmotorA_pin   2 
#define LmotorB_Pin   7 
#define LmotorPWM_pin 5

#define MAX_DISTANCE 400 //distance from wall, in cm
#define MAX_SPEED 80
#define base_speed  50 // speed used when forward(direct) motion
#define rotate_speed 30 //this is just a wild guess, need to change experimentally or otherwise if required.
#define Req_Distance 170 //required distance from wall, in cm, practically shoud  less than half of MAX_DISTANCE
//#define k1 0.5  // propotional factor between error and heading angle
#define k2 0.3// propotional factor between delay time and rotate angle ,need to adjust experimentally
#define Max_Heading_Angle 80 //in degrees to avoid over rotating for large errors
#define offset 7 // to make Lmotorwheel speed = RmotorWheel Speed , to adjust experimentally
#define rotate_step 3 // in degrees, amount of rotation at a time. rotate delay amount depends on this
#define direct_move_step 5 // in mm , amount of directmotion at a time, direct move delay amount depends on this 

int distance = 0;

int dist_error = 0; // error = Req_Distance - distance 
int prv_dist_error = 0; 
int heading_angle = 0; // theta marked in the diagram
int output_heading_angle = 0; // theta achieved
int theta = 0; //amount of rotation, parameter passed to rotate procedure
int movement = 0; // total amount of (direct) motion , for simulation purpose only

bool Heading_angle_ok = false;

float k1 = 0.0;


int read_UV_distance(int trig, int echo);
void rotate_robot(int angle, bool rotate_CW);
void direct_move_robot(int move_step, bool move_forward);

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  for (int i=2; i<8; i++) // to put pins 2 to 7 in to output mode
  {
    pinMode(i, OUTPUT);   
  }
  pinMode(trigPin_1, OUTPUT);
  pinMode(echoPin_1, INPUT);

  int max_Error = max(Req_Distance,MAX_DISTANCE-Req_Distance);
  k1 = float(Max_Heading_Angle)/float(max_Error); // make k1 so that heading angle  not exceed Max_Heading_Angle
  Serial.print("k1 : ");
  Serial.println(k1);


}

void loop() {
  // put your main code here, to run repeatedly:
//Serial.println();
distance = read_UV_distance(trigPin_1, echoPin_1);
prv_dist_error = dist_error; // should be before define/assign dist_error
dist_error = Req_Distance - distance;

//int delta_dist_error = dist_error - prv_dist_error;

heading_angle = k1*dist_error;

Serial.print("Error in distance : ");
Serial.print(dist_error);

//Serial.print("Heading angle : ");
//Serial.println(heading_angle);

Serial.print("   Output Heading angle : ");
Serial.println(output_heading_angle);

//Serial.print("Total movement : ");
//Serial.println(movement);

Heading_angle_ok = false ;

delay(50); // be carefull when introducing delays. large delay values may significantly lag other operations and hence cause faulty responses
            // if need large delay values use some other methods (such as using millis())

while(not Heading_angle_ok) // rotate untill heading angle = k1 *  dist_error // this is for simulation purpose only
{
  if(abs(heading_angle - output_heading_angle) <= rotate_step) //this is for simulation purposes only. actual situation angle can't measure need to find solution for it 
    {
      Heading_angle_ok = true ; //@@@@@@@@@@@@@@@@@@@@@@@@@
      break;
    }
  
  bool rotate_clockwise = heading_angle > output_heading_angle;
  rotate_robot(rotate_step,rotate_clockwise);
  
}

direct_move_robot(direct_move_step,true);


//heading_angle += theta; // new heading angle is previous heading angle + amount of rotation
//theta = 0; // 

}


int read_UV_distance(int trig, int echo)
{
  digitalWrite(trigPin_1, HIGH);
  delayMicroseconds(10); // be carefull when introducing delays. large delay values may significantly lag other operations and hence cause faulty responses
  digitalWrite(trigPin_1, LOW);

  // Read the result:
  int duration = pulseIn(echoPin_1, HIGH);
  int distance_cm = duration / 58;
  //Serial.print("Distance in CM: ");
  //Serial.println(distance_cm);
  return distance_cm;
  //Serial.print("Distance in inches: ");
  //Serial.println(duration / 148);
}

void rotate_robot(int step_angle, bool rotate_CW)// 
{
  //theta = step_angle;
  if(rotate_CW)
  {
    output_heading_angle += step_angle; //for simulation purpose only
    //implement the code here to rotate step angle  to suitable direction clockwise (say)
  }
  else
  {
    output_heading_angle -= step_angle;//for simulation purpose only
    //implement the code here to rotate step angle  to suitable direction clockwise (say)
  }
  
  //Serial.print("Output Heading angle : ");
  //Serial.println(output_heading_angle);

}


void direct_move_robot(int move_step, bool move_forward) //
{
  movement += move_step;  // for simulation purpose only. 
  // implement code here to move the (adjustable) step size called by parameter passed in.
}