float d1, d2, Base=9;						// d1, d2 (dist (raw) to object), Base (distance between sensors)
float diff, d1_tot, d2_tot;			// totals for 5 distance readings
float d1_inch, d2_inch, Median;	// dist (inch) to object and Median measure
float AngC, AngM;								// acute angle at object and angle from sensor line to Median
int i=10;												// Number of samples to read and average

// #define ENABLE_EASE_QUADRATIC
#define ENABLE_EASE_CUBIC_IN_OUT

#include <Servo.h>
#include "ServoEasing.hpp"

// Servo arm; 
ServoEasing Arm1;								// Create a "Servo" object called "Arm1"

float pos = 90.0; // Initial position of the servo arm (in degrees)

long readUltrasonicDistance(int triggerPin, int echoPin)
{
	pinMode(triggerPin, OUTPUT);  // Clear the trigger
	digitalWrite(triggerPin, LOW);
	delayMicroseconds(2);
	// Sets the trigger pin to HIGH state for 10 microseconds
	digitalWrite(triggerPin, HIGH);
	delayMicroseconds(10);
	digitalWrite(triggerPin, LOW);
	pinMode(echoPin, INPUT);
	// Reads the echo pin, and returns
	// the sound wave travel time in microseconds
	return pulseIn(echoPin, HIGH);
}

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

//   arm.attach(7); 			// Attach the arm to the pin 7
//   arm.write(pos); 			// Initialize the arm's position to 90 (center)

 	Arm1.attach(7, pos);		// Attach servo to pin 7 and initialize to 'pos'
}

void loop()
{
	d1_tot=0; d2_tot=0;
	for (int x = 0; x < i; x++) {			// read the distance 'i' times to total and average the readings 

  	d1 = readUltrasonicDistance(3, 2);
		d2 = readUltrasonicDistance(5, 4);
		d1_tot = d1_tot + d1;
		d2_tot = d2_tot + d2;
		// Serial.print(x); Serial.print(", "); Serial.print(d1, 0); Serial.print(", "); Serial.println(d2, 0);
		delay(50);
	}
	d1_inch = d1_tot * .0135 / i*2;		// average the 5 readings and convert to inches
	d2_inch = d2_tot * .0135 / i*2;
	// d1_inch = d1 * .0135 / 2;
	// d2_inch = d2 * .0135 / 2;

	//	calculate the angle where the sensor lines meet at the object (law of Cosines)
	AngC = acos((sq(d1_inch) + sq(d2_inch) - sq(Base)) / (2 * d1_inch * d2_inch)) * 57296 / 1000;
	//  calculate the Median length (vertex at object to center of Base)
	Median = sqrt((2*sq(d1_inch)+2*sq(d2_inch)-sq(Base))/4);
	//  calculate angle from horizontal line to Median
	AngM = acos((sq(Base/2) + sq(Median) - sq(d1_inch)) / (2 * (Base/2) * Median)) * 57296 / 1000;
	// pos = 90 + (90 - (AngC / 2));

	if ((d1_inch-d2_inch) >= Base) AngM = 178;	// if off-scale to right, set angle to 178°
	if ((d1_inch-d2_inch) <= -Base) AngM = 2;		// if off-scale to left, set angle to 2°

	Serial.print("d1_tot= ");
	Serial.print(d1_tot, 2);
	Serial.print(", d2_tot= ");
	Serial.print(d2_tot, 2);
	Serial.print(", d1= ");
	Serial.print(d1_inch, 2);
	Serial.print(", d2= ");
	Serial.print(d2_inch, 2);
	Serial.print(", Med= ");
	Serial.print(Median, 2);
	Serial.print(", Top Angle = ");
	Serial.print(AngC, 2);
	Serial.print(", Med Angle = ");
	Serial.println(AngM, 2);
	// Serial.print(", pos= ");
	// Serial.println(pos, 0);

  // arm.write(AngM);
	Arm1.startEaseTo(AngM, 40);				// move the servo arm to AngM using speed of 40

	delay(200);
}