#include <math.h>
#include <Servo.h>  // Include the Servo library

#define PI 3.1415926535897932384626433832795

Servo myServo;  // Create a Servo object
int angle;

long duration_a, inches_a, cm_a;
long duration_b, inches_b, cm_b;

const int pingPinA = 10; // Trigger Pin of Ultrasonic Sensor 
const int echoPinA = 11; // EchoPin of Ultrasonic Sensor

const int pingPinB = 7;
const int echoPinB = 8;

void setup() { 
	Serial.begin(9600); // Starting Serial Terminal 
     
        myServo.attach(9);  // Attach the servo to pin 9

	pinMode(pingPinA, OUTPUT);
	pinMode(echoPinA, INPUT);

        pinMode(pingPinB, OUTPUT);
        pinMode(echoPinB, INPUT);
	
}

void loop() {
   digitalWrite(pingPinA, LOW);delayMicroseconds(2);
   digitalWrite(pingPinA, HIGH);delayMicroseconds(10);
   digitalWrite(pingPinA, LOW);

   duration_a = pulseIn(echoPinA, HIGH);
   inches_a = microsecondsToInches(duration_a);
   cm_a = microsecondsToCentimeters(duration_a);

   digitalWrite(pingPinB, LOW);delayMicroseconds(2);
   digitalWrite(pingPinB, HIGH);delayMicroseconds(10);
   digitalWrite(pingPinB, LOW);

   duration_b = pulseIn(echoPinB, HIGH);
   inches_b = microsecondsToInches(duration_b);
   cm_b = microsecondsToCentimeters(duration_b);

   Serial.print(cm_a);Serial.print("  ");
   Serial.print("cm_a");Serial.print("  ");

   Serial.print(cm_b);Serial.print("  ");
   Serial.print("cm_b");Serial.print("  ");

   angle = findServoAngle(cm_a);
   Serial.print(angle);Serial.print("  ");
   Serial.print("angle");Serial.print("  ");
   Serial.println();

   myServo.write(angle);

   delay(2000);
}

long microsecondsToInches(long microseconds) {
   return microseconds / 74 / 2;
}

long microsecondsToCentimeters(long microseconds) {
   return microseconds / 29 / 2;
}

int findServoAngle(long object_distance_from_a){
    long x = object_distance_from_a;
    double angle  = asin((x*sin(34*PI/180))/sqrt((x*x)+(81)-(18*x*cos(34*PI/180))));
    angle = (angle * 180 / PI)+34;
    return angle;
}

/*
NOTE
left side ultrasonic server is A, and on right is refered as B.
A and B face each other in distance of 15 cm
servo is 5cm perpendicular from the centre of line joining A and B sensor.
The zero degree of Servo motor is horizontal to the same AB line and point to left side. So that servo rotate from the side of A to B.
REQUIREMENT
1 - servo library is required #include <Servo.h>
*/