#ifndef FMAIN
#include "Chimera.h"
#include <Console.h>
#endif

#include <cmath>
#include <iomanip>
#include <iostream>
#include <fstream>
#include <string>
#include <cstdlib>

using namespace std;

// given constants
double rho = 1.225;  //kg/m^3
double S = 1.2;      //m^2
double TM = 30.;     //N
double VT[4] = { 0.,11.,22.,33. };
double TT[4] = { 40.,30.,25.,22. };
double VTT[3] = { 39.85,-1.0136,0.0145 };	// ciag smigla marszowego jako funkcja predkosci lotu
double m = 25.;      //kg
double g = 9.81;     //m/s^2

// weight
double Q = m * g;

double d = 19. * 0.0254;	//VTOL propeller diameter [m]
double A = 0.25 * M_PI * d * d * 4;

double qq, vel = 0., acc = 0.;
double drag, lift, thrust, force, power;
double alpha, alphaj;
double Cx = 0;
double Cz = 0;
//double aa = 0.0, bb = 0.0, cc = 0.0, dd = 4.0*M_PI/180.;			// const
//double aa = -0.000003, bb = 0.00025, cc = 0.0011, dd = -0.1046;	// Init
//double aa = 0.000009, bb = 0.000271, cc = 0.0004, dd = -0.10464;		// Annealing updated
double aa = 0.000006, bb = 0.000159, cc = 0.0004, dd = -0.040786;		// Monte-Carlo 
//double aa = 0.000008, bb = 0.000165, cc = 0.0004, dd = -0.065649;		// PSO updated

//double aa = 		-0.0000074,  bb = 	0.0000522, cc =  	0.0004000, dd =   	-0.1968957;




void FFFF(double* xp, double* x, int nn, double t)
{
	vel = x[0];
	qq = 0.5 * rho * vel * vel;
    //alphaj = -6./180.*M_PI;
    alphaj = aa * vel * vel * vel + bb * vel * vel + cc * vel + dd;

	Cx = 1.0792 * alphaj * alphaj + 0.2131 * alphaj + 0.0431;
	Cz = 6.1069 * alphaj + 0.5776;
	drag = qq * S * Cx;
	lift = qq * S * Cz;
	double TV = VTT[0] + VTT[1] * vel + VTT[2] * vel * vel;		// ciag marszowy
	thrust = ((-lift + Q - TV * sin(alphaj)) / cos(alphaj));	// VTOL thrust
	if (thrust < 0.)thrust = 0.;
	force = -drag - thrust * sin(alphaj) + TV * cos(alphaj);	//force (sumX) 
	acc = force / m;				//acceleration
	//double VTOL = sqrt(thrust / A / rho);	// predkosc przeplywu przez smiglo VTOL - zalozono dwukrotne zwiekszenie predkosci na smigle
	//power = 0.5 * A * rho * VTOL * VTOL * VTOL;	// VTOL power - moc na napedach VTOL
	power = pow(thrust,1.5)/sqrt(2*rho*A);

#if defined(FMAIN)
	printf("alpha %f Cx %f Cz %f\n", alphaj*180./M_PI, Cx, Cz);
	printf("vel %f thrust %f drag %f lift %f \n", vel, thrust, drag, lift);
#endif

	xp[0] = acc;
	xp[1] = x[0];
	xp[2] = power;
}

void RKG4(void(*Diffun)(double* xp, double* x, int nn, double t),
	double dX0[], double dXP[], double dX[], int n,
	double dDelTime, double dT0)
{
	const double A[4] = { 0.5,0.292893219,1.707106781,0.1666666667 };
	const double B[4] = { 0.5,0.292893219,1.707106781,0.33333333 };
	const double C[4] = { 0.5,0.292893219,1.707106781,0.5 };
	const double TT[4] = { 0.,0.166666667,0.33333333,0.5 };

	double* dQ = new double[n];

	double dT, dY, dR;
	int i, j;

	for (i = 0; i < n; i++)
	{
		dQ[i] = 0.;
		dX[i] = dX0[i];
	}

	for (j = 0; j < 4; j++)
	{
		dT = dT0 + dDelTime * TT[j];
		/**/		Diffun(dXP, dX, n, dT);
		for (i = 0; i < n; i++)
		{
			dY = dDelTime * dXP[i];
			dR = A[j] * dY - B[j] * dQ[i];
			dX[i] += dR;
			dQ[i] += 3. * dR - C[j] * dY;
		}
	}

if (dQ) {delete[] dQ; dQ = 0;};
}

void calk( double *xwyn )
{
		double x[3], xp[3], x0[3];
		double t = 0., dt = 0.05;

		x0[0] = 0.;
		x0[1] = 0.;
		x0[2] = 0.;
		t = 0.;

		for (int i = 0; i < 800; i++)
		{
			RKG4(FFFF, x0, xp, x, 3, dt, t);

#if defined(FMAIN)
			printf("t %f a %f v %f x %f w %f Q %f lift %f\n", t, xp[0], x[0], x[1], x[2], Q, lift);
#endif
			if (x[0] > 25. || lift >= Q || i == 799)
			//if ( lift >= Q )
			{
				//printf("t %f a %f v %f x %f w %f\n", t, xp[0], x[0], x[1], x[2]);
				break;
			}
			t += dt;
			x0[0] = x[0];
			x0[1] = x[1];
			x0[2] = x[2];
		}
		
	for (int i = 0; i < 3; i++)xwyn[i] = x[i];

}



#if defined(FMAIN)

int main(void)
{
	double x[3];
	calk( x );
	
	printf("\n alpha %f v %f x %f w %f\n", alphaj*180/M_PI, x[0], x[1], x[2]);

}

#else

double OBJ_F(int current_iter, int thread_id, OptiM_Param *Par, double *XX, double *Gain, double *C, double *F)
{  
	double x[3];
	
	aa = XX[0];
	bb = XX[1];
	cc = XX[2];
	dd = XX[3];
	
	calk( x );
	F[0] = x[2];
	
	vel = x[0];
	qq = 0.5 * rho * vel * vel;
	alphaj = aa * vel * vel * vel + bb * vel * vel + cc * vel + dd;
	
	C[0] = Penalty(0, Gain, alphaj, '<', 0.15);
    C[1] = Penalty(1, Gain, alphaj, '>', -0.15);
	
	F[1] = alphaj*180/M_PI;
	F[2] = vel;

    return F[0]+C[0]+C[1];

}

#if defined(WIN32)

extern "C" BOOL DllMain(HINSTANCE hModule, DWORD reason, LPVOID reserved)
{
    switch (reason)
	{
		case 1://DLL_PROCESS_ATTACH:
		case 2://DLL_THREAD_ATTACH:
		case 3://DLL_THREAD_DETACH:
		case 0://DLL_PROCESS_DETACH:
		break;
    }
    return true;
}

#endif

#endif