#include <iostream>
#include <cmath>

using namespace std;

int main()
{
	const double L1 = 16.0;
	const double L2 = 32.0;
	const double L3 = 2.0;
	const double L4 = 1.2;
	const double L5 = 10.5;
	const double DEG_TO_RAD = 3.14159265 / 180.0 ;

	double z, zprime, r, rprime, rdprime;
	double theta, thetaprime;
	double w, a, b, c, v, u, phiprime, rhoprime;

	// Get the desired hand coordinates
	cout << "What is theta? ";
	cin >> theta;
	cout << "What is r? ";
	cin >> r;
	cout << "What is z? ";
	cin >> z;

	cout << "Desired point is (" << r << ", " << theta << ", " << z << ")\n";

	// Change theta from degrees to radians
	theta = theta * DEG_TO_RAD;
	// Adjust for the robot geometry
	zprime = z - L5;
//	rdprime = sqrt(r*r - L4 * L4);
	rdprime = sqrt(pow(r, 2.0) - pow(L4, 2.0));
	cout << "rdprime is " << rdprime << endl;
	rprime  = rdprime + L3;
	thetaprime = theta + acos(rdprime/r);

	cout << "thetaprime is " << thetaprime << endl;

	w = (L1 * L1 + rprime * rprime + zprime * zprime - L2 * L2) / 2;
	a = rprime * rprime + zprime * zprime;
	b = -2.0 * zprime * w;
	c = w * w - rprime * rprime * L1 * L1;
	v = (-b + sqrt(b * b - 4 * a * c)) / 2 * a;

	cout << "w is " << w << endl;
	cout << "a is " << a << endl;
	cout << "b is " << b << endl;
	cout << "c is " << c << endl;
	cout << "v is " << v << endl;

	u = sqrt(L1 * L1 - v * v);
	phiprime = acos(v / L1);
	rhoprime = asin((v - zprime) / L2) - phiprime;

	cout << "u is " << u << endl;
	cout << "phi is " << phiprime << endl;
	cout << "rho is " << rhoprime << endl;
}