// Euler_Verlet.cpp : Defines the entry point for the console application.
//

//#include "stdafx.h"
#include <stdexcept>
#include <iostream>
#include <fstream>
using namespace std;

//const int N = 2;
//const int N = 8;
const int N = 12;

//funkcja, która oblicza przyspieszenie
double rzut(int i, double* y, double t)
{
	static const double g = 9.81; //m i s

	double wynik = 0;
	switch(i)
	{
	case 0:
		wynik = y[1];		
		break;
	case 1:
		wynik = g;
		break;

	default:
		throw runtime_error("Zły numer równania");
	}

	return wynik;
}

double oscylator(int i, double* y, double t)
{
	static const double k = 1;

	double wynik = 0;
	switch (i)
	{
	case 0:
		wynik = y[1];
		break;
	case 1:
		wynik = -k*y[0];
		break;

	default:
		throw runtime_error("Zły numer równania");
	}

	return wynik;
}

const double jednostkiMasy = 5.97219E24;
const double jednostkaOdległości = 384403000;
const double jednostkaCzasu = 24 * 60 * 60;

#define SQR(x)((x)*(x))

double grawitacja2(int i, double* y, double t)
//y[0] = x1, y[1] = vx1, y[2] = y1, y[3] = vy1
//y[4] = x2, y[5] = vx2, y[6] = y2, y[7] = vy2
{
	static const double G = 6.6742867E-11; //m^3/kg/s^2
	static const double M = jednostkiMasy;
	static const double m = 0.0123*jednostkiMasy;

	double odległość_x = y[4] - y[0];
	double odległość_y = y[6] - y[2];
	double kwadrat_odległości = SQR(odległość_x) + SQR(odległość_y);
	double odległość = sqrt(kwadrat_odległości);
	double siła = G*M*m / kwadrat_odległości;
	double _x = odległość_x / odległość;
	double _y = odległość_y / odległość;

	double wynik = 0;
	switch (i)
	{
	//Ziemia
	case 0:
		wynik = y[1];
		break;
	case 1:
		wynik = siła *_x / M;
		break;
	case 2:
		wynik = y[3];
		break;
	case 3:
		wynik = siła *_y / M;
		break;

	//Księżyc
	case 4:
		wynik = y[5];
		break;
	case 5:
		wynik = -siła *_x / m;
		break;
	case 6:
		wynik = y[7];
		break;
	case 7:
		wynik = -siła *_y / m;
		break;

	default:
		throw runtime_error("Nieprawidlowy numer rownania");
	}

	return wynik;
}

double grawitacja3(int i, double* y, double t) //zagadnienie 3 ciał
//y[0] = x1, y[1] = vx1, y[2] = y1, y[3] = vy1 Ziemia
//y[4] = x2, y[5] = vx2, y[6] = y2, y[7] = vy2 Księżyc
//y[8] = x3, y[9] = vx3, y[10] = y3, y[11] = vy3 rakieta
{
	static const double G = 6.6742867E-11; //m^3/kg/s^2
	static const double M = jednostkiMasy;
	static const double m = 0.0123*jednostkiMasy;
	static const double mr = 1E-18*jednostkiMasy;

	//Ziemia-Księżyc
	double odległość_x_ZK = y[4] - y[0];
	double odległość_y_ZK = y[6] - y[2];
	double kwadrat_odległości_ZK = SQR(odległość_x_ZK) + SQR(odległość_y_ZK);
	double odległość_ZK = sqrt(kwadrat_odległości_ZK);
	double siła_ZK = G*M*m / kwadrat_odległości_ZK;
	double _x_ZK = odległość_x_ZK / odległość_ZK;
	double _y_ZK = odległość_y_ZK / odległość_ZK;

	//Ziemia-rakieta
	double odległość_x_Zr = y[8] - y[0];
	double odległość_y_Zr = y[10] - y[2];
	double kwadrat_odległości_Zr = SQR(odległość_x_Zr) + SQR(odległość_y_Zr);
	double odległość_Zr = sqrt(kwadrat_odległości_Zr);
	double siła_Zr = G*M*mr / kwadrat_odległości_Zr;
	double _x_Zr = odległość_x_Zr / odległość_Zr;
	double _y_Zr = odległość_y_Zr / odległość_Zr;

	//Księżyc-rakieta
	double odległość_x_Kr = y[8] - y[4];
	double odległość_y_Kr = y[10] - y[6];
	double kwadrat_odległości_Kr = SQR(odległość_x_Kr) + SQR(odległość_y_Kr);
	double odległość_Kr = sqrt(kwadrat_odległości_Kr);
	double siła_Kr = G*m*mr / kwadrat_odległości_Kr;
	double _x_Kr = odległość_x_Kr / odległość_Kr;
	double _y_Kr = odległość_y_Kr / odległość_Kr;

	const double wpływRakiety = 1;
	const double przyspieszenieCiąguRakiety = 0.1;

	double wynik = 0;
	switch (i)
	{
	//Ziemia
	case 0:
		wynik = y[1];
		break;
	case 1:
		wynik = (siła_ZK *_x_ZK + wpływRakiety * siła_Zr * _x_Zr) / M;
		break;
	case 2:
		wynik = y[3];
		break;
	case 3:
		wynik = (siła_ZK *_y_ZK + wpływRakiety * siła_Zr * _y_Zr) / M;
		break;

	//Księżyc
	case 4:
		wynik = y[5];
		break;
	case 5:
		wynik = (-siła_ZK *_x_ZK + wpływRakiety * siła_Kr * _x_Kr) / m;
		break;
	case 6:
		wynik = y[7];
		break;
	case 7:
		wynik = (-siła_ZK *_y_ZK + wpływRakiety * siła_Kr * _y_Kr) / m;
		break;

	//rakieta
	case 8:
		wynik = y[9];
		break;
	case 9:
		wynik = (-siła_Zr * _x_Zr - siła_Kr * _x_Kr) / mr;
		wynik += (-_x_Kr) * przyspieszenieCiąguRakiety;
		wynik *= wpływRakiety;
		break;
	case 10:
		wynik = y[11];
		break;
	case 11:
		wynik = (-siła_Zr * _y_Zr - siła_Kr * _y_Kr) / mr;
		wynik += (-_y_Kr) * przyspieszenieCiąguRakiety;
		wynik *= wpływRakiety;
		break;

	default:
		throw runtime_error("Nieprawidlowy numer rownania");
	}

	return wynik;
}


double* odeint_Euler(int N, double(*f)(int i, double* y, double t), double* y, double t, double h, double* y_nast)
{
	for (int i = 0; i < N; i++) y_nast[i] = y[i] + h*f(i, y, t);
	return y_nast;
}

double* odeint_MidPoint(int N, double(*f)(int i, double* y, double t), double* y, double t, double h, double* y_nast)
{
	double* y_tmp = new double[N];
	double k1, k2;

	for (int i = 0; i < N; ++i)
	{
		k1 = h*f(i, y, t);
		y_tmp[i] = y[i] + 0.5*k1;
	}

	for (int i = 0; i < N; ++i)
	{
		k2 = h*f(i, y_tmp, t + 0.5*h);
		y_nast[i] = y[i] + k2;
	}

	delete[] y_tmp;

	return y_nast;
}

double* odeint_RK4(int N, double(*f)(int i, double* y, double t), double* y, double t, double h, double* y_nast)
{
	double* y_tmp = new double[N];
	double* k1 = new double[N];
	double* k2 = new double[N];
	double* k3 = new double[N];
	double* k4 = new double[N];

	for (int i = 0; i < N; ++i)
	{
		k1[i] = h*f(i, y, t);
		y_tmp[i] = y[i] + 0.5*k1[i];
	}
	for (int i = 0; i < N; ++i)
	{
		k2[i] = h*f(i, y_tmp, t + 0.5*h);
		y_nast[i] = y[i] + 0.5*k2[i];
	}
	for (int i = 0; i < N; ++i)
	{
		k3[i] = h*f(i, y_nast, t + 0.5*h);
		y_tmp[i] = y[i] + k3[i];
	}
	for (int i = 0; i < N; ++i)
	{
		k4[i] = h*f(i, y_tmp, t + h);
		y_nast[i] = y[i] + (k1[i] + 2.0 * k2[i] + 2.0*k3[i] + k4[i]) / 6.0;
	}

	delete[] y_tmp;
	delete[] k1;
	delete[] k2;
	delete[] k3;
	delete[] k4;

	return y_nast;
}

const double błąd_min = 1E4;
const double błąd_max = 1E5;

double* odeint_RKF45(int N, double(*f)(int i, double* y, double t), double* y, double t, double& h, double* y_nast)
{
	static const double b31 = 3.0 / 32.0; 
	static const double b32 = 9.0 / 32.0; 
	static const double a3 = 3.0 / 8.0;

	static const double b41 = 1932.0 / 2197.0; 
	static const double b42 = -7200.0 / 2197.0; 
	static const double b43 = 7296.0 / 2197.0; 
	static const double a4 = 12.0 / 13.0;

	static const double b51 = 439.0 / 216.0; 
	static const double b53 = 3680.0 / 513.0; 
	static const double b54 = -845.0 / 4104.0;

	static const double w41 = 25.0 / 216.0; 
	static const double w43 = 1408.0 / 2565.0; 
	static const double w44 = 2197.0 / 4104.0;

	static const double b61 = -8.0 / 27.0; 
	static const double b63 = -3544.0 / 2565.0; 
	static const double b64 = 1859.0 / 4104.0; 
	static const double b65 = -11.0 / 40.0;
	
	static const double w51 = 16.0 / 135.0; 
	static const double w53 = 6656.0 / 12825.0; 
	static const double w54 = 28561.0 / 56430.0; 
	static const double w55 = -9.0 / 50.0; 
	static const double w56 = 2.0 / 55.0;

	double* y_tmp = new double[N];
	double* y4_nast = new double[N];
	double* k1 = new double[N];
	double* k2 = new double[N];
	double* k3 = new double[N];
	double* k4 = new double[N];
	double* k5 = new double[N];
	double* k6 = new double[N];

	double błąd = 0;
	do
	{
		for (int i = 0; i < N; ++i)
		{
			k1[i] = h*f(i, y, t);
			y_tmp[i] = y[i] + 0.25*k1[i];
		}
		for (int i = 0; i < N; ++i)
		{
			k2[i] = h*f(i, y_tmp, t + 0.25*h);
			y_nast[i] = y[i] + b31*k1[i] + b32*k2[i];
		}
		for (int i = 0; i < N; ++i)
		{
			k3[i] = h*f(i, y_nast, t + a3*h);
			y_tmp[i] = y[i] + b41*k1[i] + b42*k2[i] + b43*k3[i];
		}
		for (int i = 0; i < N; ++i)
		{
			k4[i] = h*f(i, y_tmp, t + a4*h);
			y_nast[i] = y[i] + b51*k1[i] - 8.0*k2[i] + b53*k3[i] + b54*k4[i];
		}
		for (int i = 0; i < N; ++i)
		{
			k5[i] = h*f(i, y_nast, t + h);
			y4_nast[i] = y[i] + w41*k1[i] + w43*k3[i] + w44*k4[i] + 0.2*k5[i];
			y_tmp[i] = y[i] + b61*k1[i] - 2.0*k2[i] + b63*k3[i] + b64*k4[i] + b65*k5[i];
		}
		for (int i = 0; i < N; ++i)
		{
			k6[i] = h*f(i, y_tmp, t + 0.5*h);
			y_nast[i] = y[i] + w51*k1[i] + w53*k3[i] + w55*k5[i] + w56*k6[i];
			błąd += fabs(y_nast[i] - y4_nast[i]);			
		}
		błąd /= N;
		//cout << "blad: " << błąd << "\n";
		//cout << "h=" << h << "\n";
		if (błąd < błąd_min)
		{			
			h *= 2;
			cout << "Zwiększam krok dwukrotnie: h=" << h << "\n";
		}
		if (błąd > błąd_max)
		{
			h /= 2;
			cout << "Zmniejszam krok dwukrotnie: h=" << h << "\n";
		}
	} while (błąd < błąd_min || błąd>błąd_max);

	delete[] y_tmp;
	delete[] k1;
	delete[] k2;
	delete[] k3;
	delete[] k4;
	delete[] k5;
	delete[] k6;

	return y_nast;
}

/*
int _tmain(int argc, _TCHAR* argv[])
{
	return 0;
}
*/

int main(int argc, char* argv)
{
	double* y = new double[N];
	double* y_nast = new double[N];

	for (int i = 0; i < N; ++i) y[i] = 0;

	//y[0] = 1;

	//warunki początkowe Księżyca
	y[6] = jednostkaOdległości;
	y[5] = 1.022E3; //vx2 m/s
	//warunki początkowe rakiety
	y[10] = 0.05 * jednostkaOdległości;
	y[11] = 1.1*7.91E3; //110% pierwszej prędkości kosmicznej

	//przejście do układu środka masy Ziemi i Księżyca
	static const double M = jednostkiMasy;
	static const double m = 0.0123*jednostkiMasy;
	double Vsm_x = (M*y[1] + m*y[5]) / (m + M);
	double Vsm_y = (M*y[3] + m*y[7]) / (m + M);
	y[1] -= Vsm_x;
	y[5] -= Vsm_x;
	y[3] -= Vsm_y;
	y[7] -= Vsm_y;

	double Rsm_x = (M*y[0] + m*y[4]) / (m + M);
	double Rsm_y = (M*y[2] + m*y[6]) / (m + M);
	y[0] -= Rsm_x;
	y[4] -= Rsm_x;
	y[2] -= Rsm_y;
	y[6] -= Rsm_y;

	double h = 1E-3 * jednostkaCzasu;
	const double rok = 365 * jednostkaCzasu;
	const double miesiąc = 27.3 * jednostkaCzasu;
	const double doba = jednostkaCzasu;
	const double tmax = 5 * miesiąc;

	ofstream plik_wy("..\\Debug\\wyniki.dat");		

	for (double t = 0; t < tmax; t += h)
	{
		//odeint_Euler(N, rzut, y, t, h, y_nast);
		//odeint_Euler(N, oscylator, y, t, h, y_nast);
		//odeint_MidPoint(N, oscylator, y, t, h, y_nast);
		//odeint_RK4(N, oscylator, y, t, h, y_nast);
		//odeint_RK4(N, grawitacja2, y, t, h, y_nast);
		//odeint_RK4(N, grawitacja3, y, t, h, y_nast);
		odeint_RKF45(N, grawitacja3, y, t, h, y_nast);

		//cout << t << "\t";
		plik_wy << t << "\t";
		for (int i = 0; i < N; ++i)
		{
			y[i] = y_nast[i];
			//cout << y[i] << "\t";
			plik_wy << y[i] << "\t";
		}
		//cout << "\n";
		plik_wy << h << "\n";		
	}

	plik_wy.close();

	delete[] y;
	delete[] y_nast;

	cout << "OK.\n\n";

	return 0;
}
