#include "Superquadratics.h"

Superquadratics::Superquadratics()
{
}

Superquadratics::~Superquadratics()
{
}

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

template< typename T>
inline int sgn(T value)
{
	if (value<0)
        return -1;

    if (value>0)
        return 1;

    if(value==0)
        return 0;
}

void  Superquadratics::fitSuperquadratic(const vector<QVector3D> &originalData, vector<QVector3D> &outputData, int LOD)
{
	//Fit superquadratic using ALGLIB optimization tools
	//Set the static variable OriginalData_
	OriginalData_ = originalData;

	//Find the best fit with the IMPROVED LEVENBERG-MARQUARDT METHOD FOR NON-LINEAR LEAST SQUARES OPTIMIZATION
	real_1d_array x = "[1,1,1,0.1,0.1]";
    real_1d_array bndl = "[+0.000001,+0.000001,+0.000001,+0.000001,+0.000001]";
    real_1d_array bndu = "[+100,+100,+100,+100,+100]";
    double epsg = 0.0000000001;
    double epsf = 0;
    double epsx = 0;
    ae_int_t maxits = 0;
    minlmstate state;
    minlmreport rep;

    minlmcreatev(OriginalData_.size(), x, 0.0001, state);
    minlmsetbc(state, bndl, bndu);
    minlmsetcond(state, epsg, epsf, epsx, maxits);
    alglib::minlmoptimize(state, staticFunction1_grad);
    minlmresults(state, x, rep);

	cout<<"CG Results for a1 a2 a3 e1 e2:"<<x[0]<<" "<<x[1]<<" "<<x[2]<<" "<<x[3]<<" "<<x[4]<<endl;
  	
	//Calclulate the data for the superquadratic with the parameters found for the best fit
	createSuperquadratic(x[0], x[1], x[2], x[3], x[4], LOD, outputData);
}

void Superquadratics::createSuperquadratic(double a1, double a2, double a3, 
					double e1, double e2,  int LOD, 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 * (sgn(cos(n)) * pow(abs(cos(n)), e1)) * (sgn(cos(w)) * pow(abs(cos(w)), e2)));
			point.setY(a2 * (sgn(cos(n)) * pow(abs(cos(n)), e1)) * (sgn(sin(w)) * pow(abs(sin(w)), e2)));
			point.setZ(a3 * (sgn(sin(n)) * pow(abs(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, real_1d_array &fi) 
{
    // this callback calculates the inside Out function 

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

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