#include "Superquadratics.h"

Superquadratics::Superquadratics()
{
}

Superquadratics::~Superquadratics()
{
}

//Initialise the static vector
vector<QVector3D> Superquadratics::OriginalData_;

void  Superquadratics::fitSuperquadratic(const vector<QVector3D> &originalData, vector<QVector3D> &outputData, int LOD)
{
	OriginalData_ = originalData;

    real_1d_array x = "[1,1,1,0.1,0.1]"; //[a1, a2, a3, e1, e2]- arguments of the 5 dimensional inside out function
    double epsg = 0.0000000001;
    double epsf = 0;
    double epsx = 0;
    double diffstep = 1.0e-6;
    ae_int_t maxits = 0;
    mincgstate state;
    mincgreport rep;

    mincgcreatef(x, diffstep, state); //let ALGLIB calculate the derivatives. Otherwise use mincgcreate(...) and change function1_grad / staticFunction1_grad
    mincgsetcond(state, epsg, epsf, epsx, maxits);
    alglib::mincgoptimize(state, staticFunction1_grad);
    mincgresults(state, x, rep);

	cout<<"CG Results for a1 a2 a3 e1 e2:"<<x[0]<<" "<<x[1]<<" "<<x[2]<<" "<<x[3]<<" "<<x[4]<<endl;
  	
	createSuperquadratic(x[0], x[1], x[2], LOD, x[3], x[4], outputData);
}

void Superquadratics::createSuperquadratic(double a1, double a2, double a3, int LOD, 
					double e1, double e2, vector<QVector3D> &data)
{
	QVector3D point;
	for(double n=-PI/2; n<=PI/2; n=n+(PI/LOD))
	{
		for(double w=-PI; w<PI; w=w+((2*PI)/LOD))
		{
			point.setX(a1 * pow(cos(n), e1) * pow(cos(w), e2));
			point.setY(a2 * pow(cos(n), e1) * pow(sin(w), e2));
			point.setZ(a3 * pow(sin(n), e1));

			data.push_back(point);
		}
	}
}

double Superquadratics::insideOutsideFunction(double a1, double a2, double a3, double e1,
											  double e2, double x, double y, double z)
{
	double tempA, tempB, tempC, tempD;

	tempA = pow((x/a1),(2/e2)) + pow((y/a2),(2/e2));
	tempB = pow(tempA, (e2/e1));
	tempC = tempB + pow((z/a3),(2/e1));
	tempD = pow(tempC, (e1/2));

	return tempD;
}

void Superquadratics::function1_grad(const real_1d_array &x, double &func) 
{
    // this callback calculates the inside Out function 

	double sum = 0;
	for(int i=0; i<OriginalData_.size(); i++)
		sum = sum + pow((1-insideOutsideFunction(x[0], x[1], x[2], x[3], x[4], OriginalData_[i].x(), OriginalData_[i].y(), OriginalData_[i].z())), 2);

    func = sum; 

	////Calculate the derivative in respect of a1 / x[0]
	//sum = 0;
	//for(int i=0; i<OriginalData_.size(); i++)
	//	sum = sum + (2 * (pow(OriginalData_[i].x(),2)/pow(x[0],3)) * (1-insideOutsideFunction(x[0], x[1], x[2], 1, 1, OriginalData_[i].x(), OriginalData_[i].y(), OriginalData_[i].z())));

	//grad[0] = sum;
	//
	////Calculate the derivative in respect of a2 / x[1]
	//sum = 0;
	//for(int i=0; i<OriginalData_.size(); i++)
	//	sum = sum + (2 * (pow(OriginalData_[i].y(),2)/pow(x[1],3)) * (1-insideOutsideFunction(x[0], x[1], x[2], 1, 1, OriginalData_[i].x(), OriginalData_[i].y(), OriginalData_[i].z())));

	//grad[1] = sum;
	//
	////Calculate the derivative in respect of a3 / x[2]
	//sum = 0;
	//for(int i=0; i<OriginalData_.size(); i++)
	//	sum = sum + (2 * (pow(OriginalData_[i].z(),2)/pow(x[2],3)) * (1-insideOutsideFunction(x[0], x[1], x[2], 1, 1, OriginalData_[i].x(), OriginalData_[i].y(), OriginalData_[i].z())));
	//    
	//grad[2] = sum;

}

void Superquadratics::staticFunction1_grad(const real_1d_array &x, double &func, void *ptr) 
{
	Superquadratics *object = (Superquadratics *)ptr;
	object->function1_grad(x, func);
}