#include "engine/math/hcMatrix.h"
#include "engine/hcImageFITS.h"
//#include "engine/math/coordTransform.h"

#include "src/pfss.h"

//#include "extern/soil/SOIL.h"

#include <stdio.h>
#include <string.h>
#include <fstream>

#include <boost/regex.hpp>
#include "boost/lexical_cast.hpp"
#include <fstream>

using namespace std;
using namespace boost;

/*! returns the magnetic field vector at given position (spherical coordinates) for a dipole
 *  at the origin oriented along the positive z-axis. Return value given in spherical coordinates.
 *  mathematics found at http://www.physicsinsights.org/dipole_field_1.html
 */
Vec3D getDipoleField(Vec3D pos, hcFloat dipole, bool debug)
{
	hcFloat r		= pos[0] / r_sol;
	hcFloat theta	= pos[1];
	hcFloat stheta	= sin(theta);
	hcFloat ctheta	= cos(theta);
	hcFloat fac		= dipole / (r * r * r);
	hcFloat Br		= fac * 2 * ctheta;
	hcFloat Bt		= fac * stheta;

	Vec3D retval((hcFloat)Br, (hcFloat)Bt, (hcFloat)0.0f);

	if(debug)
		printf("r: %E, t: %E, fac: %E, Br: %E, Bt: %E\n", r, theta, fac, Br, Bt);

	return retval;
}

/*! mathematics found in "The Model Magnetic Configuration of the Extended Corona
in the Solar Wind Formation Region - Igor Veselovsky and Olga Panasenco"
 */
Vec3D getDipoleCurrentField(Vec3D pos, hcFloat dipole, bool debug)
{
	hcFloat r		= pos[0] / r_sol;
	hcFloat theta	= pos[1];
	hcFloat stheta	= sin(theta);
	hcFloat ctheta	= cos(theta);
	hcFloat fac		= dipole / (r * r * r);

	hcFloat mu0		= 4*PI*1E-7;
	hcFloat mu		= mu0 * dipole;
	hcFloat r0		= 2.5;
	hcFloat B0		= -1E-8;
	hcFloat Ba		= (B0*r0*r0) * (B0*r0*r0) * (B0*r0*r0) / (4 * mu * mu);
	hcFloat a		= 2 * mu / (B0*r0*r0);
	hcFloat rho		= r / a;

	hcFloat Br		= Ba / (rho * rho) * (ctheta / rho + (theta - PI/2.0 < 0 ? -1.0 : 1.0));
	hcFloat Bt		= 1 / (2 * rho * rho * rho) * Ba * stheta;

	if(debug)
	{
		printf("r: %E, t: %E, Ba: %E, a: %E, rho: %E, Br: %E, Bt: %E\n", r, theta, Ba, a, rho, Br, Bt);
		//printf("(B0*r0*r0): %E, ^3: %E, denom: %E, ba: %E\n", (B0*r0*r0), (B0*r0*r0) * (B0*r0*r0) * (B0*r0*r0), (4 * dipole * dipole), Ba);
	}

	Vec3D retval((hcFloat)Br, (hcFloat)Bt, (hcFloat)0.0f);

	return retval;
}

PFSSsolution_SHC::PFSSsolution_SHC(const char *filename, bool WSOfile, hcFloat r_ss)
{
	initNULL();
	importCoefficients(filename, WSOfile, r_ss);
}

PFSSsolution_SHC::PFSSsolution_SHC()
{
    initNULL();
}

PFSSsolution_SHC::~PFSSsolution_SHC()
{
    clear();
}

PFSSsolution_SHC::PFSSsolution_SHC(const PFSSsolution_SHC &other)
{
	printf("PFSS_magfield: cpy constructor not implemented!\n");
	initNULL();
}

PFSSsolution_SHC &PFSSsolution_SHC::operator=(const PFSSsolution_SHC &other)
{
	if(this == &other)
		return *this;

	printf("PFSSsolution_SHC: assignment operator not implemented!\n");

	return *this;
}

void PFSSsolution_SHC::initNULL()
{
	g 					= NULL;
	h 					= NULL;
	assLegPoly 			= NULL;
	coeffOrder 			= 0;
}

void PFSSsolution_SHC::clear()
{
	if(g!=NULL)
	{
		for(uint i=0;i<coeffOrder;++i)
		{
			delete [] g[i];
			delete [] h[i];
			delete [] assLegPoly[i];
		}
		delete [] g;
		delete [] h;
		delete [] assLegPoly;
	}

	initNULL();
}

/*! computes spherical harmonic coefficients from synoptic magnetogram
 *
 * 	@param 	order			maximum order of coefficients to be computed
 * 	@param	r_ss			heliocentric position of source surface (in m)
 * 	@param	photMagfield	data structure containing synoptic photospheric (LOS) magnetic field strength
 */
void PFSSsolution_SHC::determineCoefficientsFromPhotMagfield(uint order, hcFloat r_ss, SynPhotMagfield &photMagfield)
{
    init(order, r_ss);

    uint width 			= photMagfield.width;
    uint height 		= photMagfield.height;
    hcFloat	maxSinLat	= photMagfield.synInfo.maxSinLat;
    hcFloat minSinLat	= -photMagfield.synInfo.maxSinLat;
    hcFloat numerator 	= 1.0 / (width * height);

    for(uint l=0;l<=coeffOrder;++l)
        for(uint m=0;m<=l;++m)
        {
            g[l][m] = 0.0;
            h[l][m] = 0.0;
            for(uint y=0;y<height;++y)
            {
            	hcFloat dSinTheta 	= (maxSinLat - minSinLat) / (height-1);
                hcFloat theta		= PI / 2.0 - asin(maxSinLat - (height - 1 - y) * dSinTheta);
                hcFloat legPolyEval = assLegPoly[l][m].operator ()(cos(theta));
                hcFloat cosecans	= 1.0 / sin(theta);

                for(uint x=0;x<width;++x)
                {
                    hcFloat phi		= m * (x+1.0/2.0) * 2.0 * PI / width;
					hcFloat D_xy 	= photMagfield(x,y) * cosecans;
					hcFloat gm		= D_xy * cos(phi) * legPolyEval;
					hcFloat hm		= D_xy * sin(phi) * legPolyEval;
                    g[l][m] 		+= gm;
                    h[l][m] 		+= hm;
                }
            }

            /*//according to Altschuler & Newkirk for source surf at inf
            g[l][m] *= (2.0*l + 1) / (l + 1) * numerator;
            h[l][m] *= (2.0*l + 1) / (l + 1) * numerator;

            /*/

            // according to Xudong Sun:
            g[l][m] *= (2.0*l + 1) * numerator;
            h[l][m] *= (2.0*l + 1) * numerator;//*/
        }
}

bool PFSSsolution_SHC::importCoefficients(const char *filename, bool WSOfile, hcFloat r_ss)
{
    FILE* coeffFile;
    char str[1000];
    char tempstr[100];
    uint k,z,order;
    int highestOrder = 0;

	if (!doesFileExist(filename))
	{
		printf("ERROR! PFSSsolution_SHC::load input file '%s' does not exist\n", filename);
		init(0, 2.5*r_sol);
		return false;
	}

    coeffFile = fopen(filename, "r");

    uint i = 0;
    while(strcmp(fgets(str, 1000, coeffFile), "\n"))
    {
        while(str[i++] != ' ');
        strncpy(tempstr, str, --i);
        tempstr[i] = '\0';
        if (atoi(tempstr) > highestOrder)
            highestOrder = atoi(tempstr);
    }

    init((uint)highestOrder, r_ss);

    fseek(coeffFile,0, SEEK_SET);
    uint j=0;
    while (fgets(str, 1000, coeffFile) != NULL)
    {
        if(j==0)        // load g's
        {
            if(str[0] == '\n')
                ++j;
            else
            {
                i = 0;

                tempstr[0] = '\0';
                while(str[i++] != ' ');
                z = i;
                strncpy(tempstr, str, i-1);
                tempstr[i-1] = '\0';
                order=(uint)atoi(tempstr);
                k=0;

                while(k<=order)
                {
                    while(str[i] == ' ' || str[i] == '\t')
                        ++i;
                    z=i;
                    while(str[i] != ' ' && str[i++] != '\n');
                    strncpy(tempstr, str+z, i-1);
                    tempstr[i-z] = '\0';
                    g[order][k++] = strtof(tempstr, NULL);
                    tempstr[0] = '\0';
                    z=i;
                }
                ++order;
            }
        }
        else            // load h's
        {
            if(str[0] == '\n')
                break;
            else
            {
                i = 0;

                tempstr[0] = '\0';
                while(str[i++] != ' ');
                z = i;
                strncpy(tempstr, str, i-1);
                tempstr[i-1] = '\0';
                order=(uint)atoi(tempstr);
                k=0;

                while(k<=order)
                {
                    while(str[i] == ' ' || str[i] == '\t')
                        ++i;
                    z=i;
                    while(str[i] != ' ' && str[i++] != '\n');
                    strncpy(tempstr, str+z, i-1);
                    tempstr[i-1-z] = '\0';
                    h[order][k++] = strtof(tempstr, NULL);
                    tempstr[0] = '\0';
                    z=i;
                }

                ++order;
            }
        }

    }
    fclose(coeffFile);

    if(WSOfile)				// WSO files employ microTesla as units
    {
    	for(uint l=0; l<=coeffOrder; ++l)
    		for(uint m=0; m<=l; ++m)
    		{
    			g[l][m] /= 100;
    			h[l][m] /= 100;
    		}
    }

    return true;
}

void PFSSsolution_SHC::exportCoefficients(const char *filename)
{
    char tempstr[10000];
    char tmp[100];

    FILE *outfile = fopen(filename, "w");
    for(uint l=0;l<=coeffOrder;++l)
    {
        tempstr[0] = '\0';
        sprintf(tempstr, "%u ", l);
        for(uint m=0;m<=l;++m)
        {
            tmp[0] = '\0';
            sprintf(tmp, "%f ", g[l][m]);
            strcat(tempstr, tmp);
        }
        if(l==0)
            strcat(tempstr, "\t\t g's");
        strcat(tempstr, "\n");
        fprintf(outfile, "%s", tempstr);
    }
    fprintf(outfile, "\n");
    for(uint l=0;l<=coeffOrder;++l)
    {
        tempstr[0] = '\0';
        sprintf(tempstr, "%u ", l);
        for(uint m=0;m<=l;++m)
        {
            tmp[0] = '\0';
            sprintf(tmp, "%f ", h[l][m]);
            strcat(tempstr, tmp);
        }
        if(l==0)
            strcat(tempstr, "\t\t h's");
        strcat(tempstr, "\n");
        fprintf(outfile, "%s", tempstr);
    }
    fclose(outfile);
}

//---------------------------------------------------------------------------------------------------------------
//---   PFSSsolution_SHC_sun
//---------------------------------------------------------------------------------------------------------------

PFSSsolution_SHC_sun::PFSSsolution_SHC_sun()
{
	initNULL();
}

PFSSsolution_SHC_sun::PFSSsolution_SHC_sun(const PFSSsolution_SHC_sun &other)
{
	initNULL();
	operator =(other);
}

PFSSsolution_SHC_sun::~PFSSsolution_SHC_sun()
{
	clear();
}

PFSSsolution_SHC_sun &PFSSsolution_SHC_sun::operator=(const PFSSsolution_SHC_sun &other)
{
	if(this == &other)
		return *this;

	init(other.coeffOrder, other.sourceSurfaceFactor*r_sol);

	for(uint l=0; l<=coeffOrder; ++l)
		for(uint m=0; m<=l; ++m)
		{
			g[l][m]	= other.g[l][m];
			h[l][m]	= other.h[l][m];
		}

	return *this;
}

PFSSsolution_SHC_sun PFSSsolution_SHC_sun::operator-(const PFSSsolution_SHC_sun &other)
{
	PFSSsolution_SHC_sun retval;

	if(this->coeffOrder != other.coeffOrder || this->sourceSurfaceFactor != other.sourceSurfaceFactor)
	{
		printf("PFSS_magfield::operator- order does not match (%u != %u) or r_ss is different.\n", this->coeffOrder, other.coeffOrder);
		retval.init(0, 2.5*r_sol);
	}
	else
	{
		retval.init(this->coeffOrder, this->sourceSurfaceFactor);
		for(uint l=0; l<=coeffOrder; ++l)
			for(uint m=0; m<=l; ++m)
			{
				hcFloat g1 		= fabs(this->g[l][m] - other.g[l][m]) / fabs(this->g[l][m]);
				hcFloat g2 		= fabs(this->g[l][m] - other.g[l][m]) / fabs(other.g[l][m]);
				hcFloat h1 		= fabs(this->h[l][m] - other.h[l][m]) / fabs(this->h[l][m]);
				hcFloat h2 		= fabs(this->h[l][m] - other.h[l][m]) / fabs(other.h[l][m]);

				retval.g[l][m] 	= this->g[l][m]==0 ? 0.0 : g1 < g2 ? g1 : g2;
				retval.h[l][m] 	= this->h[l][m]==0 ? 0.0 : h1 < h2 ? h1 : h2;
			}
	}

	return retval;

}

void PFSSsolution_SHC_sun::init(uint order, hcFloat r_ss)
{
	clear();
	coeffOrder 	= order;
	sourceSurfaceFactor = r_ss/r_sol;
	g 			= new hcFloat*[coeffOrder + 1];
	h 			= new hcFloat*[coeffOrder + 1];
	assLegPoly 	= new AssocLegendrePoly_sun*[coeffOrder + 1];

	for(uint i=0;i<=coeffOrder;++i)
	{
		g[i] 			= new hcFloat[i+1];
		h[i] 			= new hcFloat[i+1];
		assLegPoly[i] 	= new AssocLegendrePoly_sun[i+1];

		for(uint j=0;j<=i;++j)
		{
			AssocLegendrePoly_sun temp(i, j);
			assLegPoly[i][j] = temp;
		}
	}
}

void PFSSsolution_SHC_sun::eval(const Vec3D &pos, Vec3D &result)
{
	hcFloat r			= pos[0];
	hcFloat theta		= pos[1];
	hcFloat phi			= pos[2];

	hcFloat r_ss 		= sourceSurfaceFactor * r_sol;     // source surface
	hcFloat retval_r 	= 0.0;
	hcFloat retval_t 	= 0.0;
	hcFloat retval_p 	= 0.0;

	for(uint l=0;l<=coeffOrder;++l)
		for(uint m=0;m<=l;++m)
		{   // equation 16 from Notes on PFSS Extrapolation, Xudong Sun
			retval_r += assLegPoly[l][m](cos(theta))
					  * ( g[l][m] * cos(m*phi) + h[l][m] * sin(m*phi) )
					  * pow(r_sol / r, (hcFloat)(l+2))
					  * (l+1+l*pow(r / r_ss, (hcFloat)(2*l+1))) / (l+1+l*pow(r_sol / r_ss, (hcFloat)(2*l+1)));

			// equation 17
			retval_t += -assLegPoly[l][m].getDeriv(theta)
					  * ( g[l][m] * cos(m*phi) + h[l][m] * sin(m*phi) )
					  * pow(r_sol / r, (hcFloat)(l+2))
					  * (1 - pow(r / r_ss, (hcFloat)(2*l+1))) / (l+1+l*pow(r_sol / r_ss, (hcFloat)(2*l+1)));

			// equation 18
			retval_p += assLegPoly[l][m](cos(theta))
					  * ( g[l][m] * sin(m*phi) - h[l][m] * cos(m*phi) )
					  * pow(r_sol / r, (hcFloat)(l+2))
					  * (1 - pow(r / r_ss, (hcFloat)(2*l+1))) / (l+1+l*pow(r_sol / r_ss, (hcFloat)(2*l+1)));
		}

	result(0) = retval_r;
	result(1) = retval_t;
	result(2) = retval_p;
}

//---------------------------------------------------------------------------------------------------------------
//---   PFSSsolution_SHC_hoek
//---------------------------------------------------------------------------------------------------------------

PFSSsolution_SHC_hoek::PFSSsolution_SHC_hoek()
{
	initNULL();
}

PFSSsolution_SHC_hoek::PFSSsolution_SHC_hoek(const PFSSsolution_SHC_hoek &other)
{
	initNULL();
	operator =(other);
}

PFSSsolution_SHC_hoek::~PFSSsolution_SHC_hoek()
{
	clear();
}

PFSSsolution_SHC_hoek &PFSSsolution_SHC_hoek::operator=(const PFSSsolution_SHC_hoek &other)
{
	if(this == &other)
		return *this;

	printf("PFSSsolution_SHC: assignment operator not implemented!\n");

	return *this;
}

PFSSsolution_SHC_hoek PFSSsolution_SHC_hoek::operator-(const PFSSsolution_SHC_hoek &other)
{
	PFSSsolution_SHC_hoek retval;

	if(this->coeffOrder != other.coeffOrder)
	{
		printf("PFSS_magfield::operator- order does not match (%u != %u)\n", this->coeffOrder, other.coeffOrder);
		retval.init(0, 2.5*r_sol);
	}
	else
	{
		retval.init(this->coeffOrder, this->sourceSurfaceFactor);
		for(uint l=0; l<=coeffOrder; ++l)
			for(uint m=0; m<=l; ++m)
			{
				retval.g[l][m] = this->g[l][m]==0 ? 0.0 : (this->g[l][m] - other.g[l][m]) / this->g[l][m];
				retval.h[l][m] = this->h[l][m]==0 ? 0.0 : (this->h[l][m] - other.h[l][m]) / this->h[l][m];
			}
	}

	return retval;

}

void PFSSsolution_SHC_hoek::init(uint order, hcFloat r_ss)
{
	clear();
	coeffOrder 	= order;
	sourceSurfaceFactor = r_ss / r_sol;
	g 			= new hcFloat*[coeffOrder + 1];
	h 			= new hcFloat*[coeffOrder + 1];
	assLegPoly 	= new AssocLegendrePoly_sun*[coeffOrder + 1];

	for(uint i=0;i<=coeffOrder;++i)
	{
		g[i] 			= new hcFloat[i+1];
		h[i] 			= new hcFloat[i+1];
		assLegPoly[i] 	= new AssocLegendrePoly_sun[i+1];

		for(uint j=0;j<=i;++j)
		{
			AssocLegendrePoly_sun temp(i, j);
			assLegPoly[i][j] = temp;
		}
	}
}

void PFSSsolution_SHC_hoek::eval(const Vec3D &pos, Vec3D &result)
{
	hcFloat r			= pos[0];
	hcFloat theta		= pos[1];
	hcFloat phi			= pos[2];

	hcFloat r_ss 		= sourceSurfaceFactor * r_sol;     // source surface
	hcFloat retval_r 	= 0.0;
	hcFloat retval_t 	= 0.0;
	hcFloat retval_p 	= 0.0;

	for(uint l=0;l<=coeffOrder;++l)
		for(uint m=0;m<=l;++m)
		{
			hcFloat c	= -pow(r_sol / r_ss, (hcFloat)(l+2));

			retval_r += assLegPoly[l][m](cos(theta))
					  * ( g[l][m] * cos(m*phi) + h[l][m] * sin(m*phi) )
					  * ((l+1) * pow(r_sol / r, (hcFloat)(l+2)) - c * l * pow(r / r_ss, (hcFloat)((int)l-1)));

			retval_t += -assLegPoly[l][m].getDeriv(theta)
					  * ( g[l][m] * cos(m*phi) + h[l][m] * sin(m*phi) )
					  * (pow(r_sol / r, (hcFloat)(l+2)) + c * pow(r / r_ss, (hcFloat)((int)l-1)));

			retval_p += assLegPoly[l][m](cos(theta))
					  * ( g[l][m] * sin(m*phi) - h[l][m] * cos(m*phi) )
					  * (pow(r_sol / r, (hcFloat)(l+2)) + c * pow(r / r_ss, (hcFloat)((int)l-1))) * m / sin(theta);
		}

	result(0) = retval_r;
	result(1) = retval_t;
	result(2) = retval_p;
}

//---------------------------------------------------------------------------------------------------------------
//---   CSSS
//---------------------------------------------------------------------------------------------------------------

CSSS_magfield::CSSS_magfield(const char *filename)
{
	initNULL();
    if(!load(filename))
    	printf("ERROR! CSSS_magfield constructor could not load file '%s'\n", filename);
}

CSSS_magfield::CSSS_magfield()
{
	initNULL();
    printf("ERROR! CSSS_magfield std constructor not implemented!\n");
}

CSSS_magfield::CSSS_magfield(const CSSS_magfield &other)
{
	initNULL();
	printf("ERROR! CSSS_magfield cpy constructor not implemented!\n");
}

CSSS_magfield::~CSSS_magfield()
{
    clear();
}

void CSSS_magfield::initNULL()
{
	g 					= NULL;
	h 					= NULL;
	assLegPoly 			= NULL;
	sourceSurfaceFactor = 2.5;
	order 			= 0;
}

void CSSS_magfield::clear()
{
	if(g != NULL)
	{
		for(uint i=0;i<=order;++i)
		{
			delete [] g[i];
			delete [] h[i];
			delete [] assLegPoly[i];
		}
		delete [] g;
		delete [] h;
		delete assLegPoly;
	}

	initNULL();
}

void CSSS_magfield::init(uint order)
{
	clear();

    this->order	= order;
    g 			= new double*[order + 1];
    h 			= new double*[order + 1];
    assLegPoly 	= new AssocLegendrePoly_sun*[order + 1];

    for(uint l=0;l<=order;++l)
    {
        g[l] 			= new double[l+1];
        h[l] 			= new double[l+1];
        assLegPoly[l] 	= new AssocLegendrePoly_sun[l+1];
        for(uint m=0;m<=l;++m)
        {
            AssocLegendrePoly_sun temp(l, m);
            assLegPoly[l][m] 	= temp;
            g[l][m]				= 0.0;
            h[l][m]				= 0.0;
        }
    }
}

void CSSS_magfield::exportCoefficients(const char *filename)
{
    char tempstr[10000];
    char tmp[100];
    FILE *outfile = fopen(filename, "w");
    for(uint l=0;l<=order;++l)
    {
        tempstr[0] = '\0';
        sprintf(tempstr, "%u ", l);
        for(uint m=0;m<=l;++m)
        {
            tmp[0] = '\0';
            sprintf(tmp, "%f ", g[l][m]);
            strcat(tempstr, tmp);
        }
        if(l==0)
            strcat(tempstr, "\t\t g's");
        strcat(tempstr, "\n");
        fprintf(outfile, "%s", tempstr);
    }
    fprintf(outfile, "\n");
    for(uint l=0;l<=order;++l)
    {
        tempstr[0] = '\0';
        sprintf(tempstr, "%u ", l);
        for(uint m=0;m<=l;++m)
        {
            tmp[0] = '\0';
            sprintf(tmp, "%f ", h[l][m]);
            strcat(tempstr, tmp);
        }
        if(l==0)
            strcat(tempstr, "\t\t h's");
        strcat(tempstr, "\n");
        fprintf(outfile, "%s", tempstr);
    }
    fclose(outfile);
}

/*
void CSSS_magfield::determineCoefficientsFromFITSsineLat(uint order, const char *FITS_filename,
		double maxSinLat, double minSinLat, bool accountForMissingFlux)
{

    uint x, y, l, m;
    initArrays(order);
    hcImageFITS fitsIMG(FITS_filename);
    uint width 	= fitsIMG.width;
    uint height = fitsIMG.height;

    double dTheta = PI / (2 * height);
    double dSinTheta = (maxSinLat - minSinLat) / (height-1);
    double dPhi = 2*PI/(width);

    // compute missing-flux-term
    double delta = 0.0;
    double tempB;
    double theta;
    double cosecans;
    double numerator = 0.0;

    for(y=0;y<height;++y)
    {
        tempB = 0.0;
        // this is for equally spaced theta
        //theta = (2.0*(height - 1 - y) + 1 ) * dTheta;
        // this is for equally spaced sin(latitude) -- theta = 180° - (lat + 90°)
        theta = PI / 2 - asin(maxSinLat - (height - 1- y) * dSinTheta);

        cosecans = 1.0 / sin(theta);
        numerator += cosecans;

        for(x=0;x<width;++x)
            if(!isnan(fitsIMG(x,y)) && !(fitsIMG(x,y) == -32768.0))
            {
                tempB += fitsIMG(x,y);
            }
        delta += tempB * cosecans;
    }

    delta /= (numerator * width);
    if(accountForMissingFlux == false)
        delta = 0;

    // now compute the coefficients
    numerator = 1.0 / (width * height);
    double legPolyEval;
    double phi;
    double D_xy;

    for(l=0;l<=coeffOrder;++l)
        for(m=0;m<=l;++m)
        {
            g[l][m] = 0.0;
            h[l][m] = 0.0;
            for(y=0;y<height;++y)
            {
                // this is for equally spaced theta
                //theta = (2*(height - 1 - y) + 1 ) * dTheta;
                //this is for equally spaced sin(latitude)
                theta 		= PI / 2 - asin(maxSinLat - (height - 1- y) * dSinTheta);

                legPolyEval = assLegPoly[l][m].operator ()(cos(theta));
                cosecans 	= 1 / sin(theta);

                for(x=0;x<width;++x)
                {
                    phi = m * x * dPhi;
                    if(!isnan(fitsIMG(x,y)) && !(fitsIMG(x,y) == -32768.0))
                        D_xy = fitsIMG(x,y) - delta;
                    else
                        D_xy = -delta;
                    g[l][m] += D_xy * cos(phi) * cosecans * legPolyEval;
                    h[l][m] += D_xy * sin(phi) * cosecans * legPolyEval;
                }
            }

            //according to Altschuler & Newkirk for source surf at inf
            //g[l][m] *= (2.0*l + 1) / (l + 1) * numerator;
            //h[l][m] *= (2.0*l + 1) / (l + 1) * numerator;

            //according to Xudong Sun:
            g[l][m] *= (2.0*l + 1) * numerator * 100;
            h[l][m] *= (2.0*l + 1) * numerator * 100;
        }
}//*/

bool CSSS_magfield::load(const char *filename)
{
	if(!doesFileExist(filename))
		return false;

	char line[1000];
	ifstream file(filename);

	uint order = 0;

	while (!file.eof())
	{
		file.getline(line, 1000);

		if((line[0] == '#') || line[0] == '\n')
			continue;

		regex re(".*([0-9]{3}).*", regex::icase);
		cmatch what;
		regex_search(line, what, re);

		if(what[0].matched)
		{
			order = boost::lexical_cast<int>(what[1]);
			break;
		}

		re.assign(".*([0-9]{2}).*", regex::icase);
		regex_search(line, what, re);

		if(what[0].matched)
		{
			order = boost::lexical_cast<int>(what[1]);
			break;
		}

		re.assign(".*([0-9]{1}).*", regex::icase);
		regex_search(line, what, re);

		if(what[0].matched)
		{
			order = boost::lexical_cast<int>(what[1]);
			break;
		}

		printf("ERROR! loadCSSScoeff: Order of solution could not be found!\n");
		return false;
	}

	init(order);

	char lstr[1000], mstr[1000], gstr[1000], hstr[1000];

	while(file >> lstr >> mstr >> gstr >> hstr)
	{
		uint l 		= atoi(lstr);
		uint m 		= atoi(mstr);
		double gval = atof(gstr);
		double hval	= atof(hstr);

		if((l>order) || (m>l))
		{
			printf("Error! loadCSSScoeff: l: %u, m: %u\n", l, m);
			return false;
		}

		g[l][m]	= gval;
		h[l][m]	= hval;
	}
	return true;
}

void CSSS_magfield::eval(const Vec3D &pos, Vec3D &result)
{
	hcFloat r			= pos[0];
	hcFloat theta		= pos[1];
	hcFloat phi			= pos[2];

    double r_ss 		= sourceSurfaceFactor * r_sol;     // source surface
    double retval_r 	= 0.0;
    double retval_t 	= 0.0;
    double retval_phi 	= 0.0;

    for(uint l=0;l<=order;++l)
        for(uint m=0;m<=l;++m)
        {   // equation 16 from Notes on PFSS Extrapolation, Xudong Sun
            retval_r += assLegPoly[l][m](cos(theta))
                    	* ( g[l][m] * cos(m*phi) + h[l][m] * sin(m*phi) )
                    	* pow(r_sol / r, (double)(l+2))
                    	* (l+1+l*pow(r / r_ss, (double)(2*l+1))) / (l+1+l*pow(r_sol / r_ss, (double)(2*l+1)));

            // equation 17 (there has to be theta instead of cos(theta) below!)
            retval_t += -assLegPoly[l][m].getDeriv(theta)
                    	* ( g[l][m] * cos(m*phi) + h[l][m] * sin(m*phi) )
                    	* pow(r_sol / r, (double)(l+2))
                    	* (1 - pow(r / r_ss, (double)(2*l+1))) / (l+1+l*pow(r_sol / r_ss, (double)(2*l+1)));

            // equation 18
            retval_phi += assLegPoly[l][m](cos(theta))
                    	  * ( g[l][m] * sin(m*phi) - h[l][m] * cos(m*phi) )
                    	  * pow(r_sol / r, (double)(l+2))
                    	  * (1 - pow(r / r_ss, (double)(2*l+1))) / (l+1+l*pow(r_sol / r_ss, (double)(2*l+1)));//*/

        }
    result(0) = retval_r;
    result(1) = retval_t;
    result(2) = retval_phi;
}

void CSSS_magfield::dump()
{
	printf("CSSS harmonic coefficients (up to order %u):\n----------------------------------------------------------\n", order);
	for(uint l=1; l<=order; ++l)
		for(uint m=0; m<=l; ++m)
			printf("l:\t%u, m:\t%u, g_lm:\t%E, h_lm:\t%E\n", m, l, g[l][m], h[l][m]);

}

//---------------------------------------------------------------------------------------------------------------
//---   PFSS_visualization
//---------------------------------------------------------------------------------------------------------------

#ifdef GUI

PFSS_visualization::PFSS_visualization(){

	drawScale			= 0.0;
	upperOrder 			= 0;
	lowerOrder			= 0;
	upperSS				= 0;
	lowerSS				= 0;
	textScreenID		= 0;
	ssAnimScreenID		= 0;
	contourProjScreenID	= 0;
	accumulatedTimediff = 0;
	animPointer			= 0;
	animationPlaying	= false;
	fps					= 0;
	numFrames			= 0;
	magLines			= NULL;
}

PFSS_visualization::PFSS_visualization(char *filename) : hcScene3D(){

    initFromCoeffFile(filename);
}

PFSS_visualization::~PFSS_visualization()
{
    clear();
}

void PFSS_visualization::clear()
{
    if(magLines != NULL)
    {
        delete [] magLines;
        magLines = NULL;
    }
}

void PFSS_visualization::init()
{
    clear();
    hcScene3D::init();
    magLines 				= NULL;
    accumulatedTimediff 	= 0.0;
    drawScale 				= 1 / (1.5 * r_sol);
    double ssSize 			= magfield.sourceSurfaceFactor * r_sol * drawScale;
    double sunSize			 = r_sol * drawScale;
    sun.appendFragment(*(new hcDSphere3D(origin3D, 1.0)));
    sources.appendFragment(*(new hcDSphere3D(origin3D, 1.0)));
    sources.TFmodel.loadIdentity();
    sources.TFmodel.scaleHom(ssSize);
    sun.TFmodel.loadIdentity();
    sun.TFmodel.scaleHom(sunSize);
    animationPlaying = false;
}

void PFSS_visualization::initFromSynopticFITSsineLat(uint order, const char *filename, double maxSinLat, double minSinLat)
{
    init();
    SynPhotMagfield synPhot;
    synPhot.load(filename);
    magfield.determineCoefficientsFromPhotMagfield(order, 2.5*r_sol, synPhot);

    sun.texContainer.rmTexture();
    sun.texContainer.addFITSTexFromFile(filename);

    sources.texContainer.rmTexture();
    sources.texContainer.addMonoTexture(char2RGBA8(255,255,255,50));

    initScene();

}

void PFSS_visualization::initFromSynopticWSO(uint order, const char *filename)
{
    std::ifstream in(filename);

    char instrings[31][1000];
    in.getline(instrings[0], 1000);
    in.getline(instrings[0], 1000);

    uint ntheta 		= 30;
    uint nphi 			= 73;
    //hcFloat *imageData 	= new hcFloat[ntheta * nphi];
    hcImageFITS img(nphi, ntheta);

    uint i;
    uint j=0;
    while(in >> instrings[0] >> instrings[1] >> instrings[2] >> instrings[3] >> instrings[4] >> instrings[5] >> instrings[6] >> instrings[7] >> instrings[8] >> instrings[9] >> instrings[10] >> instrings[11] >> instrings[12] >> instrings[13] >> instrings[14] >> instrings[15] >> instrings[16] >> instrings[17] >> instrings[18] >> instrings[19] >> instrings[20] >> instrings[21] >> instrings[22] >> instrings[23] >> instrings[24] >> instrings[25] >> instrings[26] >> instrings[27] >> instrings[28] >> instrings[29] >> instrings[30])
    {
        if(strncmp(instrings[0], "CTxxxxxxxx", 2))
        {
            printf("ERROR! PFSS_visualization::initFromSynopticWSO: Data alignment mismatch! Check the WSO-file!\n");
            printf("j: %u, instrings[0]: %s\n", j, instrings[0]);
            exit(1);
        }

        for(i=0;i<ntheta;++i)
        {
            img(nphi - j - 1, ntheta - i - 1) = atof(instrings[i+1]);
        }
        j++;
    }
    char outfilename[1000];
    outfilename[0] = '\0';
    strcat(outfilename, filename);
    strcat(outfilename, ".FITS\0");

    img.save(outfilename);
    //delete [] imageData;
    initFromSynopticFITSsineLat(order, outfilename, 14.5/15, -14.5/15);
}

void PFSS_visualization::initFromCoeffFile(const char *filename)
{
    init();
    magfield.importCoefficients(filename, true, 2.5*r_sol);

    sun.texContainer.rmTexture();
    sun.texContainer.addTexFromFile("../textures/spicules_sst_big.jpg");

    sources.texContainer.rmTexture();
    sources.texContainer.addMonoTexture(char2RGBA8(255,255,255,50));

    initScene();
}

//*/

/*! \brief compares two coefficient files
 *
 *  treats the second file as an deviation from the first and prints the
 *  relative errors for each coefficient in outfile
 */
void PFSS_visualization::compareCoeffFiles(const char *infile1, const char *infile2, const char *outfile)
{
    FILE *coeffFile1, *coeffFile2, *compFile;
    char str[1000];
    char tempstr[100];
    uint i,j,k,z,order;
    int highestOrder1 	= 0;
    int highestOrder2 	= 0;
    tempstr[0] 			= '\0';
    str[0] 				= '\0';

    coeffFile1 = fopen(infile1, "r");
    if(!coeffFile1)
    {
        printf("ERROR! PFSSsolution_SHC_sun::compareCoeffFiles: File %s does not exist!\n", infile1);
        return;
    }
    i=0;
    while(strcmp(fgets(str, 1000, coeffFile1), "\n"))
    {
        while(str[i++] != ' ');
        strncpy(tempstr, str, --i);
        tempstr[i] = '\0';
        if (atoi(tempstr) > highestOrder1)
            highestOrder1 = atoi(tempstr);
    }

    coeffFile2 = fopen(infile2, "r");
    if(!coeffFile2)
    {
        printf("ERROR! PFSSsolution_SHC_sun::compareCoeffFiles: File %s does not exist!\n", infile1);
        return;
    }
    i=0;
    while(strcmp(fgets(str, 1000, coeffFile2), "\n"))
    {
        while(str[i++] != ' ');
        strncpy(tempstr, str, --i);
        tempstr[i] = '\0';
        if (atoi(tempstr) > highestOrder2)
            highestOrder2 = atoi(tempstr);
    }

    if(highestOrder1 != highestOrder2)
    {
        printf("ERROR! PFSSsolution_SHC_sun::compareCoeffFiles: Coefficient order does not match! (%i != %i)\n", highestOrder1, highestOrder2);
        return;
    }

    fseek(coeffFile1,0, SEEK_SET);
    fseek(coeffFile2,0, SEEK_SET);

    double **g1 = (double**)malloc(sizeof(double) * (highestOrder1+1));
    double **h1 = (double**)malloc(sizeof(double) * (highestOrder1+1));
    double **g2 = (double**)malloc(sizeof(double) * (highestOrder1+1));
    double **h2 = (double**)malloc(sizeof(double) * (highestOrder1+1));
    for(i=0;i<=highestOrder1;++i)
    {
        g1[i] = (double*) malloc(sizeof(double) * (i+1));
        h1[i] = (double*) malloc(sizeof(double) * (i+1));
        g2[i] = (double*) malloc(sizeof(double) * (i+1));
        h2[i] = (double*) malloc(sizeof(double) * (i+1));
    }

    j=0;
    while (fgets(str, 1000, coeffFile1) != NULL)
    {
        if(j==0)        // load g's
        {
            if(str[0] == '\n')
                ++j;
            else
            {
                i = 0;
                tempstr[0] = '\0';
                while(str[i++] != ' ');
                z = i;
                strncpy(tempstr, str, i-1);
                tempstr[i-1] = '\0';
                order=(uint)atoi(tempstr);
                k=0;

                while(k<=order)
                {
                    while(str[i] == ' ' || str[i] == '\t')
                        ++i;
                    z=i;
                    while(str[i] != ' ' && str[i++] != '\n');
                    strncpy(tempstr, str+z, i-1);
                    tempstr[i-1-z] = '\0';
                    g1[order][k++] = strtof(tempstr, NULL);
                    tempstr[0] = '\0';
                    z=i;
                }
                ++order;
            }
        }
        else            // load h's
        {
            if(str[0] == '\n')
                break;
            else
            {
                i = 0;

                tempstr[0] = '\0';
                while(str[i++] != ' ');
                z = i;
                strncpy(tempstr, str, i-1);
                tempstr[i-1] = '\0';
                order=(uint)atoi(tempstr);
                k=0;

                while(k<=order)
                {
                    while(str[i] == ' ' || str[i] == '\t')
                        ++i;
                    z=i;
                    while(str[i] != ' ' && str[i++] != '\n');
                    strncpy(tempstr, str+z, i-1);
                    tempstr[i-1-z] = '\0';
                    h1[order][k++] = strtof(tempstr, NULL);
                    tempstr[0] = '\0';
                    z=i;
                }
                ++order;
            }
        }
    }

    j=0;
    while (fgets(str, 1000, coeffFile2) != NULL)
    {
        if(j==0)        // load g's
        {
            if(str[0] == '\n')
                ++j;
            else
            {
                i = 0;

                tempstr[0] = '\0';
                while(str[i++] != ' ');
                z = i;
                strncpy(tempstr, str, i-1);
                tempstr[i-1] = '\0';
                order=(uint)atoi(tempstr);
                k=0;

                while(k<=order)
                {
                    while(str[i] == ' ' || str[i] == '\t')
                        ++i;
                    z=i;
                    while(str[i] != ' ' && str[i++] != '\n');
                    strncpy(tempstr, str+z, i-1);
                    tempstr[i-1-z] = '\0';
                    g2[order][k++] = strtof(tempstr, NULL);
                    tempstr[0] = '\0';
                    z=i;
                }
                ++order;
            }
        }
        else            // load h's
        {
            if(str[0] == '\n')
                break;
            else
            {
                i = 0;

                tempstr[0] = '\0';
                while(str[i++] != ' ');
                z = i;
                strncpy(tempstr, str, i-1);
                tempstr[i-1] = '\0';
                order=(uint)atoi(tempstr);
                k=0;

                while(k<=order)
                {
                    while(str[i] == ' ' || str[i] == '\t')
                        ++i;
                    z=i;
                    while(str[i] != ' ' && str[i++] != '\n');
                    strncpy(tempstr, str+z, i-1);
                    tempstr[i-1-z] = '\0';
                    h2[order][k++] = strtof(tempstr, NULL);
                    tempstr[0] = '\0';
                    z=i;
                }
                ++order;
            }
        }
    }

    fclose(coeffFile1);
    fclose(coeffFile2);
    compFile = fopen(outfile, "w");

    double error;
    double gAccum = 0.0;
    double hAccum = 0.0;
    for(i=0;i<=highestOrder1;++i)
    {
        fprintf(compFile, "%i ", i);
        for(j=0;j<=i;++j)
        {
            error = fabs((g2[i][j]-g1[i][j])/g1[i][j]);
            fprintf(compFile, "%f\t", error);
            if(!isnan((long double)error))
                gAccum += error;
        }
        fprintf(compFile, "\n");
    }

    fprintf(compFile, "\n");
    for(i=0;i<=highestOrder1;++i)
    {
        fprintf(compFile, "%i ", i);
        for(j=0;j<=i;++j)
        {
            error = fabs((h2[i][j]-h1[i][j])/h1[i][j]);
            fprintf(compFile, "%f\t", error);
            if(!isnan((long double)error))
                hAccum += error;
        }
        fprintf(compFile, "\n");
    }

    fprintf(compFile, "\nAccumulated error in g: %f\nAccumulated error in h: %f\n", gAccum, hAccum);
    fclose(compFile);

    for(i=0;i<=highestOrder1;++i)
    {
        free(g1[i]);
        free(h1[i]);
        free(g2[i]);
        free(h2[i]);
    }
    free(g1);
    free(h1);
    free(g2);
    free(h2);
}

void PFSS_visualization::initScene()
{
#define maxNumPointsPerMagLine 1000

    uint numTheta 	= 10;
    uint numPhi 	= 20;

    uint i,j,k;
    Vec3D temp;
    Vec3D pos;
    Vec3D temp_mag;
    Vec3D *posData;
    Vec3D gridPos;
    double theta, phi;

    appendObject(sun);

    magLines = new hcDObject3D[1];
    magLines[0].init();
    magLines[0].texContainer.init(3);
    magLines[0].texContainer.addMonoTexture(char2RGBA8(255, 0, 0, 255)); //downward maglines
    magLines[0].texContainer.addMonoTexture(char2RGBA8(0, 255, 0, 255)); //upward maglines
    magLines[0].texContainer.addMonoTexture(char2RGBA8(255, 255, 255, 30)); //source surface

    hcDLineStrip3D lineStrip;

    for(j=0;j<=numTheta;++j)
        for(k=0;k<numPhi;++k)
        {
            if((j==0 || j==numTheta) && k > 0) 			// compute poles only for k=0
                break;
            bool upwards;
            theta 				= j * PI / numTheta;	// TODO please review this
            phi 				= k * 2 * PI / numPhi;	// TODO please review this
            gridPos.content[0] 	= r_sol;
            gridPos.content[1] 	= theta;
            gridPos.content[2] 	= phi;

            posData 			= new Vec3D[maxNumPointsPerMagLine];

            for(i=0;i<maxNumPointsPerMagLine;++i)
            {
                if(i==0)
                {
                    magfield.eval(gridPos, temp_mag);

                    if(temp_mag.content[0] < 0)
                        upwards = false;
                    else
                        upwards = true;

                    pos = gridPos.convCoordSpher2Cart();

                    pos.scale(drawScale);
                    temp = temp_mag;
                    //temp_mag.convertSphericalToCartesian(temp);
                    temp_mag = temp_mag.convVecSpher2Cart(gridPos.convCoordSpher2Cart());
                }
                else
                {
                    if((pos.length() / drawScale < r_sol - 1E7) || (pos.length() / drawScale > magfield.sourceSurfaceFactor * r_sol))
                    {
                        lineStrip.init(i, posData);
                        lineStrip.drawOptions.computeLighting = false;

                        uint num = magLines[0].appendFragment(lineStrip);
                        if(!upwards)
                            magLines[0][num].texturePointer = 0;
                        else
                            magLines[0][num].texturePointer = 1;
                        break;
                    }

                    temp = pos;
                    temp.scale(1/drawScale);
                    temp = temp.convCoordCart2Spher();
                    magfield.eval(temp, temp_mag);
                }

                if(!upwards)
                {
                    //temp.loadZeroes();
                    //temp.minus(temp_mag);
                    temp = -temp_mag;
                    temp_mag = temp;
                }

                posData[i] = pos;
                temp_mag.scale(0.01 / temp_mag.length());
                //pos.plus(temp_mag);
                pos	+= temp_mag;
            }
            delete [] posData;
        }
    appendObject(magLines[0]);
    appendObject(sources);
}

int PFSS_visualization::loadAnimation(const char *path)
{
    uint i, j;
    numFrames 	= 1;
    lowerSS 	= 0;
    upperSS 	= 0;
    lowerOrder 	= 0;
    upperOrder 	= 0;

    char cfgFilename[1000];
    char inputFilename[1000];
    cfgFilename[0] = '\0';
    sprintf(cfgFilename, "%s%s", path, "config.cfg");
    std::ifstream config(cfgFilename);

    char option[1000], argument[1000];
    option[0] = '\0';
    argument[0] = '\0';

    while(config >> option >> argument)
    {
        if (!strcmp(option, "numFrames"))
            numFrames = atof(argument);
        if (!strcmp(option, "lowerSS"))
            lowerSS = atof(argument);
        if (!strcmp(option, "upperSS"))
            upperSS = atof(argument);
        if (!strcmp(option, "fps"))
            fps = atof(argument);
        if (!strcmp(option, "lowerOrder"))
            lowerOrder = (uint)atoi(argument);
        if (!strcmp(option, "upperOrder"))
            upperOrder = (uint)atoi(argument);
    }

    clear();
    hcScene3D::init();
    //appendElement(sun);
    magLines = new hcDObject3D[numFrames];

    for(i=0;i<numFrames;++i)
    {
        inputFilename[0] = '\0';
        sprintf(inputFilename, "%smaglines%u.obj", path, i);

        magLines[i].loadMesh(inputFilename);
        magLines[i].visible = false;
        appendObject(magLines[i]);

    }

    overlay.init();
    Vec2D ssMagScreenPos(0.38, 0.8);
    ssAnimScreenID = overlay.createAnimationScreen(ssMagScreenPos, 1.2, 0.3, numFrames);
    for(i=0;i<numFrames;++i)
    {
        inputFilename[0] = '\0';
        sprintf(inputFilename, "%sssmagfield%u.fits.contour.BMP", path, i);
        overlay[ssAnimScreenID].texContainer.addTexFromFile(inputFilename);
    }

    Fonthandler ftHandler;
    char fileNam[1000] = "../data/Roboto-Regular.ttf";
    ftHandler.init(fileNam, 20);

    uint reqWidth, reqHeight;
    float reqRelWidth, reqRelHeight;
    //uint *textlayer;

    float relHeight = 0.1;
    float relWidth = 0.5;
    uint pixelHeight = relHeight * heightPixelScale;
    uint pixelWidth = relWidth * widthPixelScale;

    Vec2D textScreenPos(-0.65, 0.9);
    textScreenID = overlay.createAnimationScreen(textScreenPos, relWidth, relHeight, numFrames);

    //textlayer = (uint*)malloc(sizeof(uint) * pixelHeight * pixelWidth);
    char text[100];

    hcImageRGBA textlayer(pixelWidth, pixelHeight);

    for(i=0;i<numFrames;++i)
    {
        text[0] = '\0';
        if(upperSS == 0)    // animation is concerned with order of spherical functions
            sprintf(text, "order: %u", lowerOrder+i);
        else
            sprintf(text, "R = %f", lowerSS + i * (float)(upperSS-lowerSS) / numFrames);

        j = 0;
        while(text[j++] != '\0');
        --j;

        ftHandler.getImgDim(text, 0, 0, reqWidth, reqHeight);
        reqRelHeight = reqHeight / heightPixelScale;
        reqRelWidth = reqWidth / heightPixelScale;
        if(reqRelHeight > relHeight)
            printf("ERROR: PFSS_visualization::loadAnimation(const char *path): Resolution of window too low for text rendering\n(height = %E, required: %E)!\n", relHeight, reqRelHeight);
        if(reqRelWidth > relWidth)
            printf("ERROR: PFSS_visualization::loadAnimation(const char *path): Resolution of window too low for text rendering\n(width = %e, required: %E)!\n", relWidth, reqRelWidth);

        ftHandler.printTextToImage(text, char2RGBA8(255, 255, 255, 255), 0, 0, pixelWidth, pixelHeight, textlayer);
        overlay[textScreenID].texContainer.addTexture2D(textlayer, false);

    }//*/
    //free(textlayer);

    Vec2D contourPosProj(0.38, 0.5);
    contourProjScreenID = overlay.createAnimationScreen(contourPosProj, 1.2, 0.3, numFrames);
    for(i=0;i<numFrames;++i)
    {
        inputFilename[0] = '\0';
        sprintf(inputFilename, "%smagfield25%u.fits.contour.BMP", path, i);
        overlay[contourProjScreenID].texContainer.addTexFromFile(inputFilename);
    }

    return 1;
}

#define outputwidth 800
#define outputheight 200

void PFSS_visualization::exportRadialMagfieldAtHeight(float r, const char *filename)
{
	hcFloat dPhi 		= 2 * PI / outputwidth;
    hcFloat dTheta 		= PI / (outputheight-1);
    //hcFloat *imageData 	= new hcFloat[outputwidth * outputheight];

    hcImageFITS img(outputwidth, outputheight);

    Vec3D pos, mag;
    pos.content[0] = r;

    for(uint x=0;x<outputwidth;++x)
        for(uint y=0;y<outputheight;++y)
        {
            pos.content[1] = PI - dTheta * y;	// TODO review this
            pos.content[2] = dPhi * x;			// TODO review this
            magfield.eval(pos, mag);
            //imageData[y * outputwidth + x] = mag[0];
            img(x, y) = mag[0];
        }

    img.save(filename);

    //delete [] imageData;
}

void PFSS_visualization::exportRadialProjectedMagfieldAtHeight(float r, const char *filename)
{
    uint x,y;
    hcFloat dPhi 		= 2 * PI / outputwidth;
    hcFloat dTheta 		= PI / (outputheight-1);
    //hcFloat *imageData 	= new hcFloat[outputwidth * outputheight];
    hcImageFITS img(outputwidth, outputheight);

    Vec3D pos, mag;
    pos.content[0] = magfield.sourceSurfaceFactor * r_sol;
    hcFloat distance = r - pos[0];

    for(x=0;x<outputwidth;++x)
        for(y=0;y<outputheight;++y)
        {
            pos.content[1] = PI - dTheta * y;	// TODO review this
            pos.content[2] = dPhi * x;			// TODO review this
            magfield.eval(pos, mag);
            img(x,y) = mag[0] * pow(pos[0]/r, 2);
        }

    img.save(filename);

    //delete [] imageData;
}

void PFSS_visualization::draw(GLuint programID)
{
    hcScene3D::draw(programID);
    overlay.draw();
}
#endif


#include <stan/math/prim/scal.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <gtest/gtest.h>

TEST(MathFunctions, falling_factorial) {
  using stan::math::falling_factorial;

  EXPECT_FLOAT_EQ(24, falling_factorial(4.0,3));
  EXPECT_FLOAT_EQ(120, falling_factorial(5.0,4));
  EXPECT_FLOAT_EQ(144.8261, falling_factorial(6.1,3.1));
  EXPECT_THROW(falling_factorial(-1, 4), std::domain_error);
}

TEST(MathFunctions, falling_factorial_nan) {
  double nan = std::numeric_limits<double>::quiet_NaN();

  EXPECT_PRED1(boost::math::isnan<double>,
               stan::math::falling_factorial(4.0, nan));

  EXPECT_PRED1(boost::math::isnan<double>,
               stan::math::falling_factorial(nan, 4.0));

  EXPECT_PRED1(boost::math::isnan<double>,
               stan::math::falling_factorial(nan, nan));
}


#ifndef TODA_ACTION_VARIABLES_GK_HPP
#define TODA_ACTION_VARIABLES_GK_HPP

#include <cmath>
#include <vector>
#include <clstatphys/physics/toda_discriminant.hpp>
#include <boost/math/quadrature/gauss_kronrod.hpp>

using namespace boost::math::quadrature;

void TodaActionVariablesGK(std::vector<double> & action_variables,
                         integrable::TodaDiscriminant const & discriminant,
                         int max_depth=5,double relative_error=1e-5){

  int matrix_size = discriminant.matrix_size();
  std::vector<double> roots(2*matrix_size);
  discriminant.total_roots(roots);

  auto func = [&discriminant](double x){
    if(discriminant(x) > 2) return std::acosh(0.5*discriminant(x));
    else if(discriminant(x) < -2) return std::acosh(-0.5*discriminant(x));
    else return 0.0;
  };

  for(int i = 0; i < matrix_size - 1; ++i){
    double error;
    if(roots[2*i+1] - roots[2*i+2] == 0)
      action_variables[i] = 0.0;
    else
      action_variables[i] = 2.0 / M_PI
                          * gauss_kronrod<double, 15>::integrate(
                                                               func,
                                                               roots[2*i+1],
                                                               roots[2*i+2],
                                                               max_depth,
                                                               relative_error,
                                                               &error
                                                               );
  }
}

#endif


/*
source file to apply LinearModels header file
*/

#include <iostream>
#include <armadillo>
#include <limits>
#include "LinearModels.hpp"
using namespace std;
using namespace arma;

// 1. Linear Regression
//	constructors and destructors
LinearRegression::LinearRegression() : fit_intercept(true) {};
LinearRegression::LinearRegression(const LinearRegression& source) : fit_intercept(source.fit_intercept), theta(source.theta) {};
LinearRegression::LinearRegression(bool fit_intercept1) : fit_intercept(fit_intercept1) {};
LinearRegression::~LinearRegression() {};

//	assignment operation
LinearRegression& LinearRegression::operator=(const LinearRegression& source) {
	if (this == &source) {
		cout << "self-assignment checked";
	}
	else {
		fit_intercept = source.fit_intercept;
		theta = source.theta;
	}
	return *this;
}

//	functions
// fit function to calculate the results and store to theta
const void LinearRegression::fit(mat X, const mat& y) {
	//	add an addtional column if fit intercept
	if (fit_intercept == true) X = join_rows(ones(X.n_rows), X);
	//	ipseduo inverse part of the equation
	mat ipseudo_inverse = (X.t() * X).i() * X.t();

	theta = ipseudo_inverse * y;		//	save results
	return;
}

// return predicted values with trained model
const mat LinearRegression::predict(mat _X) {
	//	_X is a test matrix with shape of (M, Z), pred_y will be the product of _X and theta
	if (fit_intercept == true) _X = join_rows(ones(_X.n_rows), _X);

	mat y_pred = _X * theta;
	return y_pred;
}

//	2. Ridge Regression
RidgeRegression::RidgeRegression() : fit_intercept(true), lambda(1.0) {};
RidgeRegression::RidgeRegression(const RidgeRegression& source) : fit_intercept(source.fit_intercept), theta(source.theta), lambda(source.lambda) {};
RidgeRegression::RidgeRegression(double lambda1, bool fit_intercept1) : fit_intercept(fit_intercept1), lambda(lambda1) {};
RidgeRegression::~RidgeRegression() {};

//	assignment operation
RidgeRegression& RidgeRegression::operator=(const RidgeRegression& source) {
	if (this == &source) {
		cout << "self-assignment checked";
	}
	else {
		fit_intercept = source.fit_intercept;
		lambda = source.lambda;
		theta = source.theta;
	}
	return *this;
}

//	functions
// fit function to calculate the results and store to theta
const void RidgeRegression::fit(mat X, const mat& y) {
	//	add an addtional column if fit intercept
	if (fit_intercept == true) X = join_rows(ones(X.n_rows), X);

	//	matrix for alpha

	mat A = lambda * eye(X.n_cols, X.n_cols);
	//	ipseduo inverse part of the equation
	mat ipseudo_inverse = (X.t() * X + A).i() * X.t();

	theta = ipseudo_inverse * y;		//	save results
	return;
}

// return predicted values with trained model
const mat RidgeRegression::predict(mat _X) {
	//	_X is a test matrix with shape of (M, Z), pred_y will be the product of _X and theta
	if (fit_intercept == true) _X = join_rows(ones(_X.n_rows), _X);
	mat y_pred = _X * theta;

	return y_pred;
}

// 3. Logistic Regression
//	constructors and destructors
LogisticRegression::LogisticRegression() : fit_intercept(true), lambda(0.0), penalty("l2") {};
LogisticRegression::LogisticRegression(const LogisticRegression& source) : fit_intercept(source.fit_intercept), theta(source.theta), lambda(source.lambda), penalty(source.penalty) {};
LogisticRegression::LogisticRegression(double lambda1, bool fit_intercept1, string penalty1) : fit_intercept(fit_intercept1), penalty(penalty1), lambda(lambda1) {};
LogisticRegression::~LogisticRegression() {};

//	assignment operation
LogisticRegression& LogisticRegression::operator=(const LogisticRegression& source) {
	if (this == &source) {
		cout << "self-assignment checked";
	}
	else {
		fit_intercept = source.fit_intercept;
		lambda = source.lambda;
		penalty = source.penalty;
		theta = source.theta;
	}
	return *this;
}

//	functions
// fit function to calculate the results and store to theta
const void LogisticRegression::fit(mat _X, const mat& y, double lr, double tol, long max_iter) {
	//	whether to fit an intercept
	if (fit_intercept == true) _X = join_rows(ones(_X.n_rows), _X);
	//	previous loss value to store loss
	double loss, l_prev = 999999.0;
	theta = vec(_X.n_cols, fill::randn);

	//	set loss and y_pred
	//double loss;
	mat y_pred;

	for (long i = 0; i < max_iter; i++) {
		y_pred = sigmoid(_X * theta);
		loss = _NLL(_X, y, y_pred);
		//	stop if change is less than tol
		if (l_prev - loss < tol) return;
		l_prev = loss;
		theta = theta - lr * _NLL_grad(_X, y, y_pred);
	}
}

// supplemental function to calculate negative log likelihood under current model
double LogisticRegression::_NLL(const mat& X, const mat& y, const mat& y_pred) {
	//	for X with M rows (number of examples), and N cols (number of features)
	auto M = X.n_rows, N = X.n_cols;
	int order; // type of penalty depending on value of penalty
	if (penalty == "l2") order = 2;
	else order = 1;

	double norm_theta = arma::norm(theta, order);

	double nll, penalty_val;
	nll = sum(-arma::log(y_pred.elem(find(y == 1)))) - sum(log(1 - y_pred.elem(find(y == 0))));

	if (order == 2) penalty_val = (lambda / 2) * pow(norm_theta, 2);
	else penalty_val = lambda * norm_theta;

	return (nll + penalty_val) / double(M);
}

// supplemental function to calculate Gradient of the penalized negative log likelihood wrt theta
vec LogisticRegression::_NLL_grad(const mat& X, const mat& y, const mat& y_pred) {
	//	for X with N rows (number of examples), and M cols (number of features)
	auto M = X.n_rows, N = X.n_cols;

	// calculate delta penalty
	vec d_penalty;
	if (penalty == "l2") d_penalty = lambda * theta;
	else d_penalty = lambda * sign(theta);

	return (((y - y_pred).t() * X).t() + d_penalty) / -double(M);
}

// return predicted values with trained model
const mat LogisticRegression::predict(mat _X) {
	//	_X is a test matrix with shape of (M, Z), pred_y will be the product of _X and theta
	if (fit_intercept == true) _X = join_rows(ones(_X.n_rows), _X);
	mat y_pred = _X * theta;

	return round(sigmoid(y_pred));
}

//  supplemental functions

//The logistic sigmoid function
mat sigmoid(const mat& X) {
	return 1 / (1 + exp(-X));
};

Machine Learning Algos/Machine Learning Algos/LinearModels/LinearModels.cpp


#include <Eigen/Sparse>
#include <Eigen/SPQRSupport>
#include <Eigen/OrderingMethods>
#include <Eigen/SparseQR>
#include <Eigen/Dense>
#include <iostream>
#include <fstream>
#include <vector>

// declares a column-major sparse matrix type of double
typedef Eigen::SparseMatrix<float> SparseMatrix;
typedef Eigen::Triplet<float> Triplet;

typedef Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> RowMajorMatrixXf;

const float GRID_DX = 0.1;
const float GRID_DY = 1;
const float MIN_X = 0;
const float MAX_X = 359.9999;
const float MIN_Y = 30;
const float MAX_Y = 150;

template <typename T>
T clamp(const T& v, const T& min_v, const T& max_v)
{
    return v < min_v ? min_v : (v > max_v ? max_v : v);
}

void saveAsBitmap(const Eigen::VectorXf& x, size_t rows, size_t cols, const char* fname)
{
    std::ofstream ofs(fname);
    ofs << "P2\n" << cols << " " << rows << "\n255\n";
    for(size_t rr = 0; rr < rows; rr++) {
        for(size_t cc = 0; cc < cols; cc++) {
            ofs << clamp<int>((int)x[rr * cols + cc], 0, 255) << " ";
        }
        ofs << "\n";
    }
}

inline
int wrap(int v, int min_v, int max_v)
{
    int width = max_v - min_v + 1;
    while(v < min_v) v += width;
    while(v > max_v) v -= width;
    return v;
}

inline
float sqr(float x)
{
    return x*x;
}

inline
float cube(float x)
{
    return x*x*x;
}

inline
float bspline(float x)
{
    if(x < -2) {
        return 0;
    } else if(x < -1) {
        return (cube(x+2)/6.0);
    } else if(x < 0) {
        return (-3*cube(x+2-1)+3*sqr(x+2-1)+3*(x+2-1)+1)/6;
    } else if(x < 1) {
        return ((3*cube(x+2-2) - 6*sqr(x+2-2) + 4)/6);
    } else if(x < 2) {
        return cube(1 - (x + 2 - 3))/6.0;
    } else {
        return 0;
    }
}

float kernel(float x, float width=4)
{
    // bspline width is 4
    width /= 4;
    return bspline(x / width ) / width;
}

inline
size_t grid_index(size_t width, size_t height, size_t grid_x, size_t grid_y)
{
    return grid_y * width + grid_x;
}

struct BSpline
{
    float grid_dx = GRID_DX;
    float grid_dy = GRID_DY;
    float grid_min_x = MIN_Y;
    float grid_max_x = MAX_X;
    float grid_min_y = MIN_Y;
    float grid_max_y = MAX_Y;

    size_t x_knots = (grid_max_x - grid_min_x) / grid_dx;
    size_t y_knots = (grid_max_y - grid_min_y) / grid_dy;
    size_t n_knots = x_knots * y_knots;

    SparseMatrix getWeights(const Eigen::MatrixXf& points) const
    {
        assert(points.cols() >= 2);
        std::vector<Triplet> coeffs;
        coeffs.reserve(points.size()*5);

        for(size_t pp = 0; pp < points.rows(); pp++) {
            const auto& pt = points.row(pp);
            float c_xbin = x_knots * (pt.x() - grid_min_x)/(grid_max_x - grid_min_x);
            float c_ybin = y_knots * (pt.y() - grid_min_y)/(grid_max_y - grid_min_y);

            // add weights to adjacent knots
            for(int ii = -2; ii <= 2; ii++) {
                for(int jj = -2; jj <= 2; jj++) {
                    int raw_xbin = (int)c_xbin + ii;
                    int wrapped_xbin = wrap(raw_xbin, 0, x_knots - 1);

                    int ybin = (int)c_ybin + jj;
                    if(ybin < 0 || ybin >= y_knots)
                        continue;

                    float w = kernel(c_xbin - raw_xbin)*kernel(c_ybin - ybin);
                    size_t grid_ind = grid_index(x_knots, y_knots, wrapped_xbin, ybin);
                    coeffs.push_back(Triplet(pp, grid_ind, w));
                }
            }
        }

        size_t num_points = points.rows();
        SparseMatrix Bmat(num_points, n_knots);
        Bmat.setFromTriplets(coeffs.begin(), coeffs.end());
        return Bmat;
    }

};

Eigen::VectorXf solve(const BSpline& spl, const Eigen::MatrixXf& points)
{
    std::cerr << "Filling Weights" << std::endl;
    auto Bmat = spl.getWeights(points);
    Bmat.makeCompressed();
    std::cerr << "Done" << std::endl;

    //Eigen::SparseQR<SparseMatrix, Eigen::NaturalOrdering<int>> qr;
    std::cerr << "QR" << std::endl;
    Eigen::SparseQR<SparseMatrix, Eigen::COLAMDOrdering<int>> qr(Bmat);
    std::cerr << "Done" << std::endl;
    //Eigen::SPQR<SparseMatrix> qr;
    //qr.compute(Bmat);
    //Eigen::SparseLU<SparseMatrix> qr(Bmat);
    std::cerr << "Solving: "<< std::endl;
    Eigen::VectorXf sol = qr.solve(points.col(2));
    std::cerr << "Done" << std::endl;
    return sol;
};

Eigen::VectorXf interpolate(const BSpline& spl,
        const Eigen::VectorXf& knot_values, const Eigen::MatrixXf& points)
{
    auto bmat = spl.getWeights(points);
    return bmat * knot_values;
}

void drawPoints(const BSpline& spl, const Eigen::MatrixXf& points, const char* fname)
{
    size_t x_bins = spl.x_knots;
    size_t y_bins = spl.y_knots;
    Eigen::VectorXf raster(x_bins * y_bins);
    raster.fill(0);
    for(size_t rr = 0; rr < points.rows(); rr++) {
        float x = points(rr, 0);
        float y = points(rr, 1);
        float c_xbin = x_bins * (x - spl.grid_min_x) / (spl.grid_max_x - spl.grid_min_x);
        float c_ybin = y_bins * (y - spl.grid_min_y) / (spl.grid_max_y - spl.grid_min_y);
        raster[(int)c_xbin + (int)c_ybin * x_bins] = 255;
    }
    saveAsBitmap(raster, y_bins, x_bins, fname);
}

int main(int argc, char** argv)
{
    size_t num_points = 30;
    Eigen::MatrixXf points = Eigen::MatrixXf::Random(num_points, 3); // x, y, value
    for(size_t rr = 0; rr < points.rows(); rr++) {
        points(rr, 0) = MIN_X + std::abs(points(rr, 0)) * (MAX_X - MIN_X);
        points(rr, 1) = MIN_Y + std::abs(points(rr, 1)) * (MAX_Y - MIN_Y);
        points(rr, 2) = 1;
    }

    BSpline spl;
    drawPoints(spl, points, "orig.pgm");

    // Matrix maps from knots to known points
    // v = Bx
    //
    // x is the value of the knots, B is the weights of values at each knot s.t.
    // the values reflect the positions of the input points and v is the value
    // at the input points
    Eigen::VectorXf knots = solve(spl, points);

    // Use the known knots to solve for the actual values at each knot location
    Eigen::MatrixXf grid_points(spl.x_knots*spl.y_knots, 2);
    for(size_t yy = 0; yy < spl.y_knots; yy++) {
        for(size_t xx = 0; xx < spl.x_knots; xx++) {
            grid_points(yy * spl.x_knots + xx, 0) = spl.grid_min_x + xx * spl.grid_dx;
            grid_points(yy * spl.x_knots + xx, 1) = spl.grid_min_y + yy * spl.grid_dy;
        }
    }

    Eigen::VectorXf interpolated = interpolate(spl, knots, grid_points);
    //Eigen::VectorXf interpolated = interpolate(spl, knots, points);

    saveAsBitmap(255*interpolated, spl.y_knots, spl.x_knots, "smoothed.pgm");
    return 0;
}


#include <math.h>
#include "inertiatest.hpp"
#include <frames_io.hpp>
#include <rotationalinertia.hpp>
#include <rigidbodyinertia.hpp>
#include <articulatedbodyinertia.hpp>

#include <Eigen/Core>
CPPUNIT_TEST_SUITE_REGISTRATION( InertiaTest );

using namespace KDL;
using namespace Eigen;

void InertiaTest::setUp()
{
}

void InertiaTest::tearDown()
{
}

void InertiaTest::TestRotationalInertia() {
    //Check if construction works fine
    RotationalInertia I0=RotationalInertia::Zero();
    CPPUNIT_ASSERT(Map<Matrix3d>(I0.data).isZero());
    RotationalInertia I1;
    CPPUNIT_ASSERT(Map<Matrix3d>(I1.data).isZero());
    CPPUNIT_ASSERT(Map<Matrix3d>(I0.data).isApprox(Map<Matrix3d>(I1.data)));
    RotationalInertia I2(1,2,3,4,5,6);

    //Check if copying works fine
    RotationalInertia I3=I2;
    CPPUNIT_ASSERT(Map<Matrix3d>(I3.data).isApprox(Map<Matrix3d>(I2.data)));
    I2.data[0]=0.0;
    CPPUNIT_ASSERT(!Map<Matrix3d>(I3.data).isApprox(Map<Matrix3d>(I2.data)));

    //Check if addition and multiplication works fine
    Map<Matrix3d>(I0.data).setRandom();
    I1=-2*I0;
    CPPUNIT_ASSERT(!Map<Matrix3d>(I1.data).isZero());
    I1=I1+I0+I0;
    CPPUNIT_ASSERT(Map<Matrix3d>(I1.data).isZero());

    //Check if matrix is symmetric
    CPPUNIT_ASSERT(Map<Matrix3d>(I2.data).isApprox(Map<Matrix3d>(I2.data).transpose()));

    //Check if angular momentum is correctly calculated:
    Vector omega;
    random(omega);
    Vector momentum=I2*omega;

    CPPUNIT_ASSERT(Map<Vector3d>(momentum.data).isApprox(Map<Matrix3d>(I2.data)*Map<Vector3d>(omega.data)));

}

void InertiaTest::TestRigidBodyInertia() {
    RigidBodyInertia I1(0.0);
    double mass;
    Vector c;
    RotationalInertia Ic;
    random(mass);
    random(c);
    Matrix3d Ic_eig = Matrix3d::Random();
    //make it symmetric:
    Map<Matrix3d>(Ic.data)=Ic_eig+Ic_eig.transpose();
    RigidBodyInertia I2(mass,c,Ic);
    //Check if construction works fine
    CPPUNIT_ASSERT_EQUAL(I2.getMass(),mass);
    CPPUNIT_ASSERT(!Map<Matrix3d>(I2.getRotationalInertia().data).isZero());
    CPPUNIT_ASSERT_EQUAL(I2.getCOG(),c);
    CPPUNIT_ASSERT(Map<Matrix3d>(I2.getRotationalInertia().data).isApprox(Map<Matrix3d>(Ic.data)-mass*(Map<Vector3d>(c.data)*Map<Vector3d>(c.data).transpose()-(Map<Vector3d>(c.data).dot(Map<Vector3d>(c.data))*Matrix3d::Identity()))));

    RigidBodyInertia I3=I2;
    //check if copying works fine
    CPPUNIT_ASSERT_EQUAL(I2.getMass(),I3.getMass());
    CPPUNIT_ASSERT_EQUAL(I2.getCOG(),I3.getCOG());
    CPPUNIT_ASSERT(Map<Matrix3d>(I2.getRotationalInertia().data).isApprox(Map<Matrix3d>(I3.getRotationalInertia().data)));
    //Check if multiplication and addition works fine
    RigidBodyInertia I4=-2*I2 +I3+I3;
    CPPUNIT_ASSERT_EQUAL(I4.getMass(),0.0);
    CPPUNIT_ASSERT_EQUAL(I4.getCOG(),Vector::Zero());
    CPPUNIT_ASSERT(Map<Matrix3d>(I4.getRotationalInertia().data).isZero());

    //Check if transformations work fine
    //Check only rotation transformation
    //back and forward
    Rotation R;
    random(R);
    I3 = R*I2;
    I4 = R.Inverse()*I3;
    CPPUNIT_ASSERT_EQUAL(I2.getMass(),I4.getMass());
    CPPUNIT_ASSERT_EQUAL(I2.getCOG(),I4.getCOG());
    CPPUNIT_ASSERT(Map<Matrix3d>(I2.getRotationalInertia().data).isApprox(Map<Matrix3d>(I4.getRotationalInertia().data)));
    //rotation and total with p=0
    Frame T(R);
    I4 = T*I2;
    CPPUNIT_ASSERT_EQUAL(I3.getMass(),I4.getMass());
    CPPUNIT_ASSERT_EQUAL(I3.getCOG(),I4.getCOG());
    CPPUNIT_ASSERT(Map<Matrix3d>(I3.getRotationalInertia().data).isApprox(Map<Matrix3d>(I4.getRotationalInertia().data)));

    //Check only transformation
    Vector p;
    random(p);
    I3 = I2.RefPoint(p);
    I4 = I3.RefPoint(-p);
    CPPUNIT_ASSERT_EQUAL(I2.getMass(),I4.getMass());
    CPPUNIT_ASSERT_EQUAL(I2.getCOG(),I4.getCOG());
    CPPUNIT_ASSERT(Map<Matrix3d>(I2.getRotationalInertia().data).isApprox(Map<Matrix3d>(I4.getRotationalInertia().data)));
    T=Frame(-p);
    I4 = T*I2;
    CPPUNIT_ASSERT_EQUAL(I3.getMass(),I4.getMass());
    CPPUNIT_ASSERT_EQUAL(I3.getCOG(),I4.getCOG());
    CPPUNIT_ASSERT(Map<Matrix3d>(I3.getRotationalInertia().data).isApprox(Map<Matrix3d>(I4.getRotationalInertia().data)));

    random(T);
    I3=T*I2;
    I4=T.Inverse()*I3;

    Twist a;
    random(a);
    Wrench f=I2*a;
    CPPUNIT_ASSERT_EQUAL(T*f,(T*I2)*(T*a));

    random(T);
    I3 = T*I2;
    I4 = T.Inverse()*I3;
    CPPUNIT_ASSERT_EQUAL(I2.getMass(),I4.getMass());
    CPPUNIT_ASSERT_EQUAL(I2.getCOG(),I4.getCOG());
    CPPUNIT_ASSERT(Map<Matrix3d>(I2.getRotationalInertia().data).isApprox(Map<Matrix3d>(I4.getRotationalInertia().data)));

}
void InertiaTest::TestArticulatedBodyInertia() {
    double mass;
    Vector c;
    RotationalInertia Ic;
    random(mass);
    random(c);
    Matrix3d::Map(Ic.data).triangularView<Lower>()= Matrix3d::Random().triangularView<Lower>();
    RigidBodyInertia RBI2(mass,c,Ic);
    ArticulatedBodyInertia I2(RBI2);
    ArticulatedBodyInertia I1(mass,c,Ic);
    //Check if construction works fine
    CPPUNIT_ASSERT_EQUAL(I2.M,I1.M);
    CPPUNIT_ASSERT_EQUAL(I2.H,I1.H);
    CPPUNIT_ASSERT_EQUAL(I2.I,I1.I);
    I1 = ArticulatedBodyInertia(I2);
    CPPUNIT_ASSERT_EQUAL(I2.M,I1.M);
    CPPUNIT_ASSERT_EQUAL(I2.H,I1.H);
    CPPUNIT_ASSERT_EQUAL(I2.I,I1.I);

    CPPUNIT_ASSERT_EQUAL(I2.M,(Matrix3d::Identity()*mass).eval());
    CPPUNIT_ASSERT(!I2.I.isZero());
    //CPPUNIT_ASSERT(I2.I.isApprox(Map<Matrix3d>(Ic.data)-mass*(Map<Vector3d>(c.data)*Map<Vector3d>(c.data).transpose()-(Map<Vector3d>(c.data).dot(Map<Vector3d>(c.data))*Matrix3d::Identity()))));
    //CPPUNIT_ASSERT(I2.H.isApprox(Map<Vector3d>(c.data)*Map<Vector3d>(c.data).transpose()-(Map<Vector3d>(c.data).dot(Map<Vector3d>(c.data))*Matrix3d::Identity())));
    ArticulatedBodyInertia I3=I2;
    //check if copying works fine
    CPPUNIT_ASSERT_EQUAL(I2.M,I3.M);
    CPPUNIT_ASSERT_EQUAL(I2.H,I3.H);
    CPPUNIT_ASSERT_EQUAL(I2.I,I3.I);
    //Check if multiplication and addition works fine
    ArticulatedBodyInertia I4=-2*I2 +I3+I3;
    CPPUNIT_ASSERT_EQUAL(I4.M,Matrix3d::Zero().eval());
    CPPUNIT_ASSERT_EQUAL(I4.H,Matrix3d::Zero().eval());
    CPPUNIT_ASSERT_EQUAL(I4.I,Matrix3d::Zero().eval());

    //Check if transformations work fine
    //Check only rotation transformation
    //back and forward
    Rotation R;
    random(R);
    I3 = R*I2;
    Map<Matrix3d> E(R.data);
    Matrix3d tmp = E.transpose()*I2.M*E;
    CPPUNIT_ASSERT(I3.M.isApprox(tmp));
    tmp = E.transpose()*I2.H*E;
    CPPUNIT_ASSERT_EQUAL(I3.H,tmp);
    tmp = E.transpose()*I2.I*E;
    CPPUNIT_ASSERT(I3.I.isApprox(tmp));

    I4 = R.Inverse()*I3;
    CPPUNIT_ASSERT(I2.M.isApprox(I4.M));
    CPPUNIT_ASSERT(I2.H.isApprox(I4.H));
    CPPUNIT_ASSERT(I2.I.isApprox(I4.I));
    //rotation and total with p=0
    Frame T(R);
    I4 = T*I2;
    CPPUNIT_ASSERT_EQUAL(I3.M,I4.M);
    CPPUNIT_ASSERT_EQUAL(I3.H,I4.H);
    CPPUNIT_ASSERT_EQUAL(I3.I,I4.I);

    //Check only transformation
    Vector p;
    random(p);
    I3 = I2.RefPoint(p);
    I4 = I3.RefPoint(-p);
    CPPUNIT_ASSERT_EQUAL(I2.M,I4.M);
    CPPUNIT_ASSERT(I2.H.isApprox(I4.H));
    CPPUNIT_ASSERT(I2.I.isApprox(I4.I));
    T=Frame(-p);
    I4 = T*I2;
    CPPUNIT_ASSERT_EQUAL(I3.M,I4.M);
    CPPUNIT_ASSERT_EQUAL(I3.H,I4.H);
    CPPUNIT_ASSERT_EQUAL(I3.I,I4.I);

    random(T);
    I3=T*I2;
    I4=T.Inverse()*I3;

    Twist a;
    random(a);
    Wrench f=I2*a;
    CPPUNIT_ASSERT_EQUAL(T*f,(T*I2)*(T*a));

    random(T);
    I3 = T*I2;
    I4 = T.Inverse()*I3;
    CPPUNIT_ASSERT(I2.M.isApprox(I4.M));
    CPPUNIT_ASSERT(I2.H.isApprox(I4.H));
    CPPUNIT_ASSERT(I2.I.isApprox(I4.I));
}


#include "Algorithms/ExpertIo/Level1.h"

#include <boost/test/unit_test.hpp>
#include <string>
#include <vector>

using Algorithms::ExpertIo::EasyNonConstructibleChange::
  non_constructible_change_sort;
using Algorithms::ExpertIo::NthFibonacci::get_nth_fibonacci_recursive;
using Algorithms::ExpertIo::is_valid_subsequence;
using Algorithms::ExpertIo::sorted_squared_array_algorithmic;
using Algorithms::ExpertIo::sorted_squared_array_two_indices;
using Algorithms::ExpertIo::sorted_squared_array_with_selection_sort;
using Algorithms::ExpertIo::tournament_winner;
using Algorithms::ExpertIo::two_number_sum_brute;
using Algorithms::ExpertIo::two_number_sum_with_map;

using std::string;
using std::vector;

BOOST_AUTO_TEST_SUITE(Algorithms)
BOOST_AUTO_TEST_SUITE(ExpertIo)

BOOST_AUTO_TEST_SUITE(TwoNumberSum)

const vector<int> example_array {3, 5, -4, 8, 11, 1, -1, 6};
const int example_target_sum {10};

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(ExampleFindingAnyTwoArraySumBruteForce)
{
  const vector<int> brute_solution {
    two_number_sum_brute(example_array, example_target_sum)};

  BOOST_TEST(brute_solution[0] == 11);
  BOOST_TEST(brute_solution[1] == -1);
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(ExampleFindingAnyTwoArraySumWithMap)
{
  const vector<int> map_solution {
    two_number_sum_with_map(example_array, example_target_sum)};

  BOOST_TEST(map_solution[0] == 11);
  BOOST_TEST(map_solution[1] == -1);
}

BOOST_AUTO_TEST_SUITE_END() // TwoNumberSum

BOOST_AUTO_TEST_SUITE(ValidateSubsequence)

const vector<int> sample_array {5, 1, 22, 25, 6, -1, 8, 10};
const vector<int> sample_sequence {1, 6, -1, 10};

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(ExampleWithTwoPointers)
{
  BOOST_TEST(is_valid_subsequence(sample_array, sample_sequence));
}

BOOST_AUTO_TEST_SUITE_END() // ValidateSubsequence

BOOST_AUTO_TEST_SUITE(SortedSquaredArray)

const vector<int> sample_array {1, 2, 3, 4, 5, 6, 8, 9};

const vector<int> test_case_array_8 {-2, -1};

const vector<int> test_case_array_9 {-5, -4, -3, -2, -1};

const vector<int> test_case_array_11 {-10, -5, 0, 5, 10};

const vector<int> test_case_array_12 {-7, -3, 1, 9, 22, 30};

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(SortedSquaredArrayWithAlgorithmImplementation)
{
  const auto x = sorted_squared_array_algorithmic(sample_array);

  auto iter_x = x.begin();

  for (const auto& a : sample_array)
  {
    BOOST_TEST((a * a == *iter_x));

    ++iter_x;
  }
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(SquareOfNegativeValuesGetsSorted)
{
  {
    const auto x = sorted_squared_array_algorithmic(test_case_array_8);

    BOOST_TEST(x.at(0) == 1);
    BOOST_TEST(x.at(1) == 4);
  }

}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(SquareOfNegativeValuesGetsSortedWithSelectionSort)
{
  {
    const auto x = sorted_squared_array_with_selection_sort(test_case_array_9);

    BOOST_TEST(x.at(0) == 1);
    BOOST_TEST(x.at(1) == 4);
    BOOST_TEST(x.at(2) == 9);
  }
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(SquareOfPositiveValuesGetsSortedWithTwoIndices)
{
  {
    const auto x = sorted_squared_array_two_indices(sample_array);

    BOOST_TEST(x.at(0) == 1);
    BOOST_TEST(x.at(1) == 4);
    BOOST_TEST(x.at(2) == 9);
  }
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(SquareOfNegativeValuesGetsSortedWithTwoIndices)
{
  {
    const auto x = sorted_squared_array_two_indices(test_case_array_8);

    BOOST_TEST(x.at(0) == 1);
    BOOST_TEST(x.at(1) == 4);
  }
  {
    const auto x = sorted_squared_array_two_indices(test_case_array_9);

    BOOST_TEST(x.at(0) == 1);
    BOOST_TEST(x.at(1) == 4);
    BOOST_TEST(x.at(2) == 9);
  }
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(SquareOValuesGetsSortedWithTwoIndices)
{
  {
    const auto x = sorted_squared_array_two_indices(test_case_array_11);

    BOOST_TEST(x.at(0) == 0);
    BOOST_TEST(x.at(1) == 25);
    BOOST_TEST(x.at(2) == 25);
  }
  {
    const auto x = sorted_squared_array_two_indices(test_case_array_12);

    BOOST_TEST(x.at(0) == 1);
    BOOST_TEST(x.at(1) == 9);
    BOOST_TEST(x.at(2) == 49);
  }
}

BOOST_AUTO_TEST_SUITE_END() // SortedSquaredArray

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(TournamentWinnerWithUnorderedMapWorks)
{
  const vector<vector<string>> competitions {
    {"HTML", "C#"},
    {"C#", "Python"},
    {"Python", "HTML"}};

  const vector<int> competition_results {0, 0, 1};

  const string winner {tournament_winner(competitions, competition_results)};

  BOOST_TEST(winner == "Python");
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(TournamentWinnerWithUnorderedMapWorksForOtherTestCases)
{
  // Test case 9
  {
    const vector<vector<string>> competitions {
      {"HTML", "Java"},
      {"Java", "Python"},
      {"Python", "HTML"},
      {"C#", "Python"},
      {"Java", "C#"},
      {"C#", "HTML"},
      {"SQL", "C#"},
      {"HTML", "SQL"},
      {"SQL", "Python"},
      {"SQL", "Java"}};

    const vector<int> competition_results {0, 0, 0, 0, 0, 0, 1, 0, 1, 1};

    const string winner {tournament_winner(competitions, competition_results)};

    BOOST_TEST(winner == "SQL");
  }

  // Test case 10
  {
    const vector<vector<string>> competitions {
      {"A", "B"}};

    const vector<int> competition_results {0};

    const string winner {tournament_winner(competitions, competition_results)};

    BOOST_TEST(winner == "B");
  }
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(NonConstructibleChangeSortPassesSampleCases)
{
  {
    const vector<int> given_coins {1, 2, 5};
    const int expected {4};

    const int result {non_constructible_change_sort(given_coins)};

    BOOST_TEST(result == 6);
  }

}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(NonConstructibleChangePassesSampleCases)
{
  {
    const vector<int> given_coins {1, 2, 5};
    const int expected {4};

  }

}

BOOST_AUTO_TEST_SUITE(NthFibonacci)

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(FibonacciRecursiveComputesCorrectly)
{
  BOOST_TEST(get_nth_fibonacci_recursive(1) == 0);
  BOOST_TEST(get_nth_fibonacci_recursive(2) == 1);
  BOOST_TEST(get_nth_fibonacci_recursive(16) == 610);
}

BOOST_AUTO_TEST_SUITE_END() // NthFibonacci

BOOST_AUTO_TEST_SUITE_END() // ExpertIo
BOOST_AUTO_TEST_SUITE_END() // Algorithms


#include <iostream>
#include <opencv2/opencv.hpp>
#include <Eigen/Eigen>
#include <math.h>

int main(int argc, char **argv)
{

    {
        Eigen::Matrix3d m = Eigen::Matrix3d::Identity(); //生成单位矩阵
        std::cout << "m^T:" << std::endl
                  << m.transpose() << std::endl; // transpose() 转置后输出

        Eigen::Vector3d v1(3, 0, 0);
        Eigen::Vector3d v2(0, 4, 0);
        Eigen::Vector3d v3(0, 0, 5);
        std::cout << "v1^T:" << std::endl
                  << v1 << std::endl
                  << "v2^T:" << std::endl
                  << v2.transpose() << std::endl
                  << "v3^T:" << std::endl
                  << v3.transpose() << std::endl;

        // m.block<1, 3>(0, 0)
        // 分块每块大小<1行，3列>
        // 取出（第0行，第0列）个块
        m.block<1, 3>(0, 0) = v1.transpose();
        m.block<1, 3>(1, 0) = v2.transpose();
        m.block<1, 3>(2, 0) = v3.transpose();

        std::cout << "m:" << std::endl
                  << m << std::endl;

        Eigen::Vector3d VecAll1;
        VecAll1.fill(1); //向量各个值全部填充为1
        std::cout << "VecAll1^T:" << std::endl
                  << VecAll1.transpose() << std::endl;

        Eigen::Vector3d normal = m.inverse() * VecAll1;
        double NormalLen = normal.norm(); //向量的模
        normal = normal / NormalLen;      //v1 v2 v3 构成平面的单位法向量
        double d_plane_o = 1 / NormalLen; //平面到原点的距离

        std::cout << "normal^T:" << std::endl
                  << normal.transpose() << std::endl;
        std::cout << d_plane_o << std::endl;
    }
    return 0;
}


//!
//! Contains the interface for the Gaussian distribution.
//!
//! \file distrib/gaussian.hpp
//! \author Lachlan McCalman
//! \date February 2014
//! \license Affero General Public License version 3 or later
//! \copyright (c) 2014, NICTA
//!

#pragma once

#include <Eigen/Dense>
#include <cmath>

namespace obsidian
{
  namespace distrib
  {
    //! Compute the log Gaussian distribution PDF with a diagonal covariance matrix.
    //!
    //! \param x The vector to evaluate the distribution at.
    //! \param mu The mean vector containing the means of each dimension.
    //! \param sigma The diagonal of the covariance matrix.
    //!
    double logGaussian(const Eigen::VectorXd& x, const Eigen::VectorXd& mu, const Eigen::VectorXd& sigma)
    {
      double coeff = -std::log(std::pow(2.0 * M_PI, x.size() * 0.5) * sigma.prod());
      return coeff - 0.5 * (x - mu).cwiseQuotient(sigma).squaredNorm();
    }
  }
}


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2007 François du Vignaud

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

#ifndef quantlib_tap_correlations_hpp
#define quantlib_tap_correlations_hpp

#include <ql/types.hpp>
#include <ql/utilities/disposable.hpp>
#include <ql/math/matrix.hpp>
#include <ql/math/optimization/costfunction.hpp>
#include <boost/function.hpp>
#include <vector>

namespace QuantLib {

    //! Returns the Triangular Angles Parametrized correlation matrix
    /*! The matrix \f$ m \f$ is filled with values corresponding to angles
        given in the \f$ angles \f$ vector. See equation (24) in
        "Parameterizing correlations: a geometric interpretation"
        by Francesco Rapisarda, Damiano Brigo, Fabio Mercurio

        \test
        - the correctness of the results is tested by reproducing
          known good data.
        - the correctness of the results is tested by checking
          returned values against numerical calculations.
    */
    Disposable<Matrix>
    triangularAnglesParametrization(const Array& angles,
                                    Size matrixSize,
                                    Size rank);

    Disposable<Matrix>
    lmmTriangularAnglesParametrization(const Array& angles,
                                       Size matrixSize,
                                       Size rank);

    // the same function using the angles parameterized by the following
    // transformation \f[ \teta_i = \frac{\Pi}{2} - arctan(x_i)\f]
    Disposable<Matrix>
    triangularAnglesParametrizationUnconstrained(const Array& x,
                                                 Size matrixSize,
                                                 Size rank);

    Disposable<Matrix>
    lmmTriangularAnglesParametrizationUnconstrained(const Array& x,
                                                    Size matrixSize,
                                                    Size rank);

    //! Returns the rank reduced Triangular Angles Parametrized correlation matrix
    /*! The matrix \f$ m \f$ is filled with values corresponding to angles
        corresponding  to the 3D spherical spiral paramterized by
        \f$ alpha \f$, \f$ t0 \f$, \f$ epsilon \f$ values. See equation (32) in
        "Parameterizing correlations: a geometric interpretation"
        by Francesco Rapisarda, Damiano Brigo, Fabio Mercurio

        \test
        - the correctness of the results is tested by reproducing
          known good data.
        - the correctness of the results is tested by checking
          returned values against numerical calculations.
    */
    Disposable<Matrix>
    triangularAnglesParametrizationRankThree(Real alpha,
                                             Real t0,
                                             Real epsilon,
                                             Size nbRows);

    // the same function with parameters packed in an Array
    Disposable<Matrix>
    triangularAnglesParametrizationRankThreeVectorial(const Array& paramters,
                                                      Size nbRows);

    // Cost function associated with Frobenius norm.
    // <http://en.wikipedia.org/wiki/Matrix_norm>
    class FrobeniusCostFunction : public CostFunction{
      public:
        FrobeniusCostFunction(
            const Matrix& target,
            const boost::function<Disposable<Matrix>(const Array&,
                                                     Size,
                                                     Size)>& f,
                                                     Size matrixSize,
                                                     Size rank)
        : target_(target), f_(f), matrixSize_(matrixSize), rank_(rank) {}
        Real value (const Array &x) const;
        Disposable<Array> values (const Array &x) const;
      private:
        Matrix target_;
        boost::function<Disposable<Matrix>(const Array&, Size, Size)> f_;
        Size matrixSize_;
        Size rank_;
    };
}

/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2007 François du Vignaud

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

#include <cmath>

namespace QuantLib {

    inline Disposable<Matrix> triangularAnglesParametrization(const Array& angles,
                                                       Size matrixSize,
                                                       Size rank) {

        // what if rank == 1?
        QL_REQUIRE((rank-1) * (2*matrixSize - rank) == 2*angles.size(),
                    "rank-1) * (matrixSize - rank/2) == angles.size()");
        Matrix m(matrixSize, matrixSize);

        // first row filling
        m[0][0] = 1.0;
        for (Size j=1; j<matrixSize; ++j)
            m[0][j] = 0.0;

        // next ones...
        Size k = 0; //angles index
        for (Size i=1; i<m.rows(); ++i) {
            Real sinProduct = 1.0;
            Size bound = std::min(i,rank-1);
            for (Size j=0; j<bound; ++j) {
                m[i][j] = std::cos(angles[k]);
                m[i][j] *= sinProduct;
                sinProduct *= std::sin(angles[k]);
                ++k;
            }
            m[i][bound] = sinProduct;
            for (Size j=bound+1; j<m.rows(); ++j)
                m[i][j] = 0;
        }
        return m;
    }

    inline Disposable<Matrix> lmmTriangularAnglesParametrization(const Array& angles,
                                                          Size matrixSize,
                                                          Size) {
        Matrix m(matrixSize, matrixSize);
        for (Size i=0; i<m.rows(); ++i) {
            Real cosPhi, sinPhi;
            if (i>0) {
                cosPhi = std::cos(angles[i-1]);
                sinPhi = std::sin(angles[i-1]);
            } else {
                cosPhi = 1.0;
                sinPhi = 0.0;
            }

            for (Size j=0; j<i; ++j)
                m[i][j] = sinPhi * m[i-1][j];

            m[i][i] = cosPhi;

            for (Size j=i+1; j<m.rows(); ++j)
                m[i][j] = 0.0;
        }
        return m;
    }

    inline Disposable<Matrix> triangularAnglesParametrizationUnconstrained(
                                                            const Array& x,
                                                            Size matrixSize,
                                                            Size rank) {
        Array angles(x.size());
        //we convert the unconstrained parameters in angles
        for (Size i = 0; i < x.size(); ++i)
            angles[i] = M_PI*.5 - std::atan(x[i]);
        return triangularAnglesParametrization(angles, matrixSize, rank);
    }

    inline Disposable<Matrix> lmmTriangularAnglesParametrizationUnconstrained(
                                                            const Array& x,
                                                            Size matrixSize,
                                                            Size rank) {
        Array angles(x.size());
        //we convert the unconstrained parameters in angles
        for (Size i = 0; i < x.size(); ++i)
            angles[i] = M_PI*.5 - std::atan(x[i]);
        return lmmTriangularAnglesParametrization(angles, matrixSize, rank);
    }

    inline Disposable<Matrix> triangularAnglesParametrizationRankThree(
                                            Real alpha, Real t0,
                                            Real epsilon, Size matrixSize) {
        Matrix m(matrixSize, 3);
        for (Size i=0; i<m.rows(); ++i) {
            Real t = t0 * (1 - std::exp(epsilon*Real(i)));
            Real phi = std::atan(alpha * t);
            m[i][0] = std::cos(t)*std::cos(phi);
            m[i][1] = std::sin(t)*std::cos(phi);
            m[i][2] = -std::sin(phi);
        }
        return m;
    }

    inline Disposable<Matrix> triangularAnglesParametrizationRankThreeVectorial(
                                                    const Array& parameters,
                                                    Size nbRows) {
        QL_REQUIRE(parameters.size() == 3,
                   "the parameter array must contain exactly 3 values" );
        return triangularAnglesParametrizationRankThree(parameters[0],
                                                        parameters[1],
                                                        parameters[2],
                                                        nbRows);

    }

  inline Real FrobeniusCostFunction::value(const Array& x) const {
        Array temp = values(x);
        return DotProduct(temp, temp);
    }

  inline Disposable<Array> FrobeniusCostFunction::values(const Array& x) const {
        Array result((target_.rows()*(target_.columns()-1))/2);
        Matrix pseudoRoot = f_(x, matrixSize_, rank_);
        Matrix differences = pseudoRoot * transpose(pseudoRoot) - target_;
        Size k = 0;
        // then we store the elementwise differences in a vector.
        for (Size i=0; i<target_.rows(); ++i) {
            for (Size j=0; j<i; ++j){
                result[k] = differences[i][j];
                ++k;
            }
        }
        return result;
    }
}

#endif


/* Main routine to run an implicit 3-node Lobatto finite-difference method
   for solution of a second-order, scalar-valued BVP:

      u'' = p(t)*u' + q(t)*u + r(t),  a<t<b,
      u(a) = ua,  u(b) = ub

   where the problem has stiffness that may be adjusted using
   the real-valued parameter lambda<0 [read from the command line]

   D.R. Reynolds
   Math 6321 @ SMU
   Fall 2020  */

#include <iostream>
#include <iomanip>
#include <vector>
#include <armadillo>
#include "bvp.hpp"

using namespace std;
using namespace arma;

// utility routine to map from physical/component space to linear algebra index space
//    interval:  physical interval index [1 <= interval <= N]
//    location:  location in interval [0=left, 1=midpoint, 2=right]
//    component: solution component at this location [0=u, 1=u']
int idx(int interval, int location, int component) {
  return ( 4*(interval-1) + 2*location + component );
}

// main routine
int main(int argc, char **argv) {

  // get lambda from the command line, otherwise set to -10
  double lambda;
  if (argc > 1)
    lambda = atof(argv[1]);
  if (lambda >= 0.0)
    lambda = -10.0;

  // define BVP object
  BVP bvp(lambda);

  // test 'idx' function by outputting mapping for small N
  cout << "Test output from 'idx' function for N = 5, M = 22:";
  for (int interval=1; interval<=5; interval++) {
    cout << "\n  interval " << interval << ", (loc,comp,idx):";
    for (int location=0; location<=2; location++) {
      for (int component=0; component<=1; component++) {
        cout << "  (" << location << ", " << component << ", "
             << idx(interval,location,component) << ")";
      }
    }
  }

  // loop over spatial resolutions for tests
  vector<int> N = {100, 1000, 10000};
  for (int i=0; i<N.size(); i++) {

    // output problem information
    cout << "\nImplicit Lobatto-3 FD method for BVP with lambda = "
         << lambda << ":" << ",  N = " << N[i] << endl;

    // compute/store analytical solution
    vec t = zeros(N[i]+1);
    t(0) = 0.0;
    t(N[i]) = 1.0;
    for (int j=1; j<=N[i]-1; j++)
      t(j) = 0.5*(1.0-cos((2*j-1)*M_PI/(2*(N[i]-1))));
    vec utrue(N[i]+1);
    for (int j=0; j<=N[i]; j++)
      utrue(j) = bvp.utrue(t(j));

    // set integer for overall linear algebra problem size
    int M = 4*N[i]+2;

    // create matrix and right-hand side vectors
#ifdef USE_SPARSE
    sp_mat A(M,M);
#else
    mat A(M,M);
    A.fill(0.0);
#endif
    vec b(M);
    b.fill(0.0);

    // set up linear system:
    //    recall 'idx' usage: idx(interval,location,component)
    //      interval:  physical interval index [1 <= interval <= N]
    //      location:  location in interval [0=left, 1=midpoint, 2=right]
    //      component: solution component at this location [0=u, 1=u']
    A(0,idx(1,0,0)) = 1.0;
    b(0) = bvp.ua;
    A(1,idx(N[i],2,0)) = 1.0;
    b(1) = bvp.ub;
    int irow = 2;
    for (int j=1; j<=N[i]; j++) {

      // setup interval-specific information
      double tl = t(j-1);
      double tr = t(j);
      double th = 0.5*(tl+tr);
      double h = tr-tl;

      // setup first equation for this interval:
      //    -24*y_{j-1,0} - 5*h*y_{j-1,1} + 24*y_{j-1/2,0} - 8*h*y_{j-1/2,1} + h*y_{j,1} = 0
      A(irow,idx(j,0,0)) = -24.0;
      A(irow,idx(j,0,1)) = -5.0*h;
      A(irow,idx(j,1,0)) = 24.0;
      A(irow,idx(j,1,1)) = -8.0*h;
      A(irow,idx(j,2,1)) = h;
      b(irow) = 0.0;
      irow++;

      // setup second equation for this interval:
      //    -5*h*q_{j-1}*y_{j-1,0} - (24+5*h*p_{j-1})*y_{j-1,1} - 8*h*q_{j-1/2}*y_{j-1/2,0}
      //      + (24-8*h*p_{j-1/2})*y_{j-1/2,1} + h*q_{j}*y_{j,0} + h*p_{j}*y_{j,1} = h*(5*r_{j-1}+8*r_{j-1/2}-r_{j})
      A(irow,idx(j,0,0)) = -5.0*h*bvp.q(tl);
      A(irow,idx(j,0,1)) = -(24.0 + 5.0*h*bvp.p(tl));
      A(irow,idx(j,1,0)) = -8.0*h*bvp.q(th);
      A(irow,idx(j,1,1)) = (24.0-8.0*h*bvp.p(th));
      A(irow,idx(j,2,0)) = h*bvp.q(tr);
      A(irow,idx(j,2,1)) = h*bvp.p(tr);
      b(irow) = h*(5.0*bvp.r(tl) + 8.0*bvp.r(th) - bvp.r(tr));
      irow++;

      // setup third equation for this interval:
      //    -6*y_{j-1,0} - h*y_{j-1,1} - 4*h*y_{j-1/2,1} + 6*y_{j,0} - h*y_{j,1} = 0
      A(irow,idx(j,0,0)) = -6.0;
      A(irow,idx(j,0,1)) = -h;
      A(irow,idx(j,1,1)) = -4.0*h;
      A(irow,idx(j,2,0)) = 6.0;
      A(irow,idx(j,2,1)) = -h;
      b(irow) = 0.0;
      irow++;

      // setup fourth equation for this interval:
      //    -h*q_{j-1}*y_{j-1,0} - (6+h*p_{j-1})*y_{j-1,1} - 4*h*q_{j-1/2}*y_{j-1/2,0} - 4*h*p_{j-1/2}*y_{j-1/2,1}
      //       - h*q_j*y_{j,0} + (6-h*p_j)*y_{j,1} = h*(r_{j-1} + 4*r_{j-1/2} + r_{j})
      A(irow,idx(j,0,0)) = -h*bvp.q(tl);
      A(irow,idx(j,0,1)) = -(6.0 + h*bvp.p(tl));
      A(irow,idx(j,1,0)) = -4.0*h*bvp.q(th);
      A(irow,idx(j,1,1)) = -4.0*h*bvp.p(th);
      A(irow,idx(j,2,0)) = -h*bvp.q(tr);
      A(irow,idx(j,2,1)) = (6.0-h*bvp.p(tr));
      b(irow) = h*(bvp.r(tl) + 4.0*bvp.r(th) + bvp.r(tr));
      irow++;

    }

    // solve linear system for BVP solution
#ifdef USE_SPARSE
    vec y = arma::spsolve(A,b);
#else
    vec y = arma::solve(A,b);
#endif

    // output maximum error
    vec u(N[i]+1);
    for (int j=0; j<=N[i]; j++)
      u(j) = y(idx(j+1,0,0));
    vec uerr = abs(u-utrue);
    cout << "  Maximum BVP solution error = " << std::setprecision(4)
       << uerr.max() << "\n";
  }

  return 0;
}


/*
// Copyright 2018 University of Liege
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
// Authors:
// - Adrien Crovato
*/

//// Bilinear interpolation
// Interpolate linearly a quantity defined at 4 points to another point
//
// Inputs:
// - x0, x1, x2, x3: x-coordinates of interpolation points
// - y0, y1, y2, y3: y-coordinates of interpolation points
// - z0, z1, z2, z3: z-coordinates of interpolation points
// - s0, s1, s2, s3: quantity to interpolate at interpolation point
// - x, y, z: coordinates of interpolated point
//
// Output:
// - i: interpolated quantity

#include <iostream>
#include <Eigen/Dense>
#include "interp.h"

#define TOL 1e-2 // geometric tolerance on z (measure how far target point is w.r.t. interpolation plane)

using namespace std;
using namespace Eigen;

double interp(double x0, double y0, double z0, double x1, double y1, double z1,
              double x2, double y2, double z2, double x3, double y3, double z3,
              double s0, double s1, double s2, double s3,
              double x, double y, double z) {

    //// Initialization
    Vector3d l, p, n; // unit vector
    Vector3d cg; // CG of planed formed by interpolation points
    Vector2d A, B, C, D, X; // (rotated) interpolation and interpolated points
    Vector2d E, F, G, H; // temporary vector
    double zd, avg; // distance between interpolated point and interpolation plane & plane diagonal
    double k2, k1, k0, Delta; // coefficients of k2*x^2 + k1*x + k0 and discriminant
    double u, v ;// inverse bilinear interpolation parameters
    double i0, i1, i ;// interpolated quantities

    //// Move change of coordinates out of file to improve CPU runtime
    //// Change of axes
    // Plane center
    cg(0) = 0.25*(x0 + x1 + x2 + x3);
    cg(1) = 0.25*(y0 + y1 + y2 + y3);
    cg(2) = 0.25*(z0 + z1 + z2 + z3);
    // Unit vectors
    l(0) = 0.5*(0.5*(x0+x3)-0.5*(x1+x2));
    l(1) = 0.5*(0.5*(y0+y3)-0.5*(y1+y2));
    l(2) = 0.5*(0.5*(z0+z3)-0.5*(z1+z2));
    n(0) = (y2-y0)*(z1-z3) - (z2-z0)*(y1-y3);
    n(1) = (z2-z0)*(x1-x3) - (x2-x0)*(z1-z3);
    n(2) = (x2-x0)*(y1-y3) - (y2-y0)*(x1-x3);
    p = n.cross(l);
    l = l / l.norm(); // normalize
    p = p / p.norm(); // normalize
    n = n / n.norm(); // normalize
    // Translation & Rotation
    A(0) = l(0)*(cg(0)-x0) + l(1)*(cg(1)-y0) + l(2)*(cg(2)-z0);
    A(1) = p(0)*(cg(0)-x0) + p(1)*(cg(1)-y0) + p(2)*(cg(2)-z0);
    B(0) = l(0)*(cg(0)-x1) + l(1)*(cg(1)-y1) + l(2)*(cg(2)-z1);
    B(1) = p(0)*(cg(0)-x1) + p(1)*(cg(1)-y1) + p(2)*(cg(2)-z1);
    C(0) = l(0)*(cg(0)-x2) + l(1)*(cg(1)-y2) + l(2)*(cg(2)-z2);
    C(1) = p(0)*(cg(0)-x2) + p(1)*(cg(1)-y2) + p(2)*(cg(2)-z2);
    D(0) = l(0)*(cg(0)-x3) + l(1)*(cg(1)-y3) + l(2)*(cg(2)-z3);
    D(1) = p(0)*(cg(0)-x3) + p(1)*(cg(1)-y3) + p(2)*(cg(2)-z3);
    X(0) = l(0)*(cg(0)-x) + l(1)*(cg(1)-y) + l(2)*(cg(2)-z);
    X(1) = p(0)*(cg(0)-x) + p(1)*(cg(1)-y) + p(2)*(cg(2)-z);
    // Error measure
    avg = sqrt((x2-x0)*(x2-x0) + (y2-y0)*(y2-y0) + (z2-z0)*(z2-z0));
    zd = n(0)*(x-cg(0)) + n(1)*(y-cg(1)) + n(2)*(z-cg(2));
    #ifdef VERBOSE
    if (abs(zd/avg) > TOL) {
        cout << endl << "Interpolated point is far from interpolation plane; bilinear interpolation might be inaccurate!" << endl;
        cout << "Distance versus interpolation plane diagonal: " << zd << " / " << avg << " > " << TOL << endl;
        cout << "This might be caused by a coarse panelling of the body surface." << endl;
    }
    #endif

    //// Interpolation
    // Intermediate vectors
    E = B-A;
    F = D-A;
    G = (A-B) + (C-D);
    H = X-A;
    // Coefficients
    k2 = G(0)*F(1) - G(1)*F(0); //
    k1 = E(0)*F(1) - E(1)*F(0) + H(0)*G(1) - H(1)*G(0);
    k0 = H(0)*E(1) - H(1)*E(0);
    // Solve quadratic equation
    if (k2 == 0) {
        v = -k0/k1;
    }
    else {
        Delta = k1*k1 - 4*k2*k0;
        if (Delta < 0) {
            cout << endl << "Discriminant of inverse bilinear interpolation is negative. Cannot interpolate!" << endl;
            exit(EXIT_FAILURE);
        }
        else if (Delta == 0) {
            v = (-k1) / (2.0*k2);
        }
        else {
            Delta = sqrt(Delta);
            v = (-k1 - Delta) / (2.0*k2);
        }
    }
    u = (H(0) - F(0)*v) / (E(0) + G(0)*v);
    // Interpolation along l
    i0 = (1-u)*s0 + u*s1;
    i1 = (1-u)*s3 + u*s2;
    // Interpolation along p
    i = (1-v)*i0 + v*i1;
    return i;
}


#include "setup.h"

#include <NTL/ZZXFactoring.h>

#include "util.h"

namespace {
    using namespace NTL;
}

namespace gnfs {
    ZZ check_f_irred(const ZZX& f, const ZZ& m) {
        ZZ c;
        vec_pair_ZZX_long factors;

        factor(c, factors, f);

        // just evaluate at first factor
        // if irreducible, this spits out f(m)
        return eval(factors[0].a, m);
    }
}