#include "Superquadratics.h"

Superquadratics::Superquadratics()
{
}

Superquadratics::~Superquadratics()
{
}

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

    real_1d_array x = "[1,1,1]";
    double epsg = 0.0000000001;
    double epsf = 0;
    double epsx = 0;
    double stpmax = 0.1;
    ae_int_t maxits = 0;
    mincgstate state;
    mincgreport rep;

	// first run
    mincgcreate(x, state);
    mincgsetcond(state, epsg, epsf, epsx, maxits);
    mincgsetstpmax(state, stpmax);
	//This where the error is thwron by MVS 2010
	alglib::minlbfgsoptimize(state, staticFunction1_grad);
    mincgresults(state, x, rep);

    printf("%s\n", x.tostring(2).c_str()); 

    //createSuperquadratic(a1, a2, a3, LOD, e1, e2, 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;

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

void Superquadratics::function1_grad(const real_1d_array &x, double &func, real_1d_array &grad) 
{
    // this callback calculates the inside Out function and its derivatives with e1 = e2 = 1 (because we already know that we will use a sphere)
	// In order to let the algorithm find the best values for e1 and e2 we need to calculate the derivatives in respect of e 1 and e2, too. -> Difficult

	double sum = 0;
	for(int i=0; i<OriginalData_.size(); i++)
		sum = sum + pow((1-insideOutsideFunction(x[0], x[1], x[2], 1, 1, 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, real_1d_array &grad, void *ptr) 
{
	Superquadratics *object = (Superquadratics *)ptr;
	object->function1_grad(x, func, grad);
}