// PhysicsSim Shuttle Orientation.cpp : Defines the entry point for the console application.
//

#include "stdafx.h"
#include <cmath>
#include <iostream>
#include <math.h>

using namespace std;

int main()
{
	double rotinput;
	cout << "Nose rotation: ";
	cin >> rotinput;

	double const pi = 3.14159265359;
	double rotangle = rotinput *pi/180; // in radians. For zero rotation, craft is pointing with nose in +y direction.


	double p1[3] = { 2.5 * (cos(pi / 2 + rotangle)), 2.5 * (sin(pi / 2 + rotangle)), 3 }; // X, Y, and mass

	double p2[3] = { 1.552417 * (cos(1.310194 + rotangle)), 1.552417 * (sin(1.310194 + rotangle)), 5 };
//	double p2[3] = { 0.4, 1.5, 5 };

	double p3[3] = { 1.552417 * (cos(pi-1.310194 + rotangle)), 1.552417 * (sin(pi-1.310194 + rotangle)), 5 };

	double p4[3] = { 1.581139*(cos(-0.321751 + rotangle)), 1.581139*(sin(-0.321751 + rotangle)), 6 };
//	double p4[3] = { 1.5, -0.5, 6 };

	double p5[3] = { 1.581139*(cos(pi+0.321751  + rotangle)), 1.581139*(sin(pi+0.321751 + rotangle)), 6 };

	double p6[3] = { 1.5 * (cos(-pi / 2 + rotangle)), 1.5 * (sin(-pi / 2 + rotangle)), 11 };

	double points[6][3] = { { p1[0], p1[1], p1[2] }, { p2[0], p2[1], p2[2] }, { p3[0], p3[1], p3[2] }, { p4[0], p4[1], p4[2] }, { p5[0], p5[1], p5[2] }, { p6[0], p6[1], p6[2] } };
	// single massive 2-dim array allows for these values to be cycled through in the for loops for force and torque

	double y_zero = 6600000; // "zero point" for y-coordinate. Distance to Earth's center, meters.

	double m_Earth = (5.972 * pow(10,24));
	double G = (6.67 * pow(10, -11));

	double localg = -G*m_Earth / (y_zero*y_zero);

	// double velx = sqrt(abs(localg)*y_zero); // velocity required for a circular orbit at this point.
	/*
	cout << p1[0] << "   " << p1[1] << "   " << p1[2] << endl;
	cout << p2[0] << "   " << p2[1] << "   " << p2[2] << endl;
	cout << p3[0] << "   " << p3[1] << "   " << p3[2] << endl;
	cout << p4[0] << "   " << p4[1] << "   " << p4[2] << endl;
	cout << p5[0] << "   " << p5[1] << "   " << p5[2] << endl;
	cout << p6[0] << "   " << p6[1] << "   " << p6[2] << endl;
	*/
	cout << endl;

	double torquesum = 0;

	for (int i = 0; i <= 5; i++)
	{
		cout << i << endl;
		cout << points[i][0] << "   " << points[i][1] << "   " << points[i][2] << endl;

		double p_y = points[i][1] + y_zero;
		double p_dir = atan(points[i][1] / points[i][0]);
		double p_dist = sqrt(points[i][0] * points[i][0] + points[i][1] * points[i][1]);

		double g_dir = atan(p_y / points[i][0]);
		double dist = sqrt(p_y * p_y + points[i][0] * points[i][0]);
		double g_mag = G*m_Earth / (dist*dist);

		double f_mag = g_mag*points[i][2];

		double torque = p_dist*f_mag*sin(p_dir - g_dir);
		torquesum = torquesum + torque;

	}

	cout << endl << endl << "Torque Sum: " << torquesum*1000 << " * 10^-3" << endl;

	system("pause");
	return 0;
}