#ifdef HAVE_CONFIG_H
# include "config.h"
#endif

#include <stdio.h>
#include <assert.h>
#include "psConstants.h"
#include "psEllipse.h"

// ellipse rotation (major, minor, theta) -> (x2, y2, xy)
psEllipsePol psEllipseAxesToPol(psEllipseAxes axes)
{
    psEllipsePol pol;

    pol.e0 = PS_SQR(axes.major) + PS_SQR(axes.minor);
    double ds = PS_SQR(axes.major) - PS_SQR(axes.minor);

    pol.e1 = ds*cos(2*axes.theta);
    pol.e2 = ds*sin(2*axes.theta);

    assert (isfinite(pol.e0));
    assert (isfinite(pol.e1));
    assert (isfinite(pol.e2));

    return pol;
}

// ellipse rotation (major, minor, theta) -> (x2, y2, xy)
psEllipsePol psEllipseShapeToPol(psEllipseShape shape)
{
    psEllipsePol pol;

    double r = 1.0 / (1.0 - PS_SQR(shape.sx)*PS_SQR(shape.sy)*PS_SQR(shape.sxy));

    pol.e0 = r*(PS_SQR(shape.sx) + PS_SQR(shape.sy));
    pol.e1 = r*(PS_SQR(shape.sx) - PS_SQR(shape.sy));
    // XXX I do not understand this negative sign
    pol.e2 = -r*(2.0*PS_SQR(shape.sx)*PS_SQR(shape.sy)*shape.sxy);

    assert (isfinite(pol.e0));
    assert (isfinite(pol.e1));
    assert (isfinite(pol.e2));

    return pol;
}

// ellipse rotation (major, minor, theta) -> (x2, y2, xy)
// XXXX handle case where e0 < LIMIT
psErrorCode psEllipsePolToAxes(const psEllipsePol pol,
			       const float minMinorAxis,
			       psEllipseAxes *axes)
{
    axes->theta = +0.5 * atan2 (pol.e2, pol.e1); // theta in radians

    double cs = cos(2*axes->theta);
    double sn = sin(2*axes->theta);
    double ds = 0;

    if ((cs > 0.707) || (cs < -0.707)) {
	ds = pol.e1 / cs;
    } else {
	ds = pol.e2 / sn;
    }

    float LIMIT = PS_SQR(minMinorAxis);
    if (pol.e0 < LIMIT) {
	// if e0 is too small, we are really out of luck
	axes->major = minMinorAxis;
	axes->minor = minMinorAxis;
    } else {
	if (2.0*(pol.e0 - ds) < LIMIT) {
	    // if e0 - ds is too small, saturate the minor axis at minMinorAxis
	    axes->major = sqrt(pol.e0 - LIMIT);
	    axes->minor = sqrt(LIMIT);
	} else {
	    // normal values for e0 & ds
	    axes->major = sqrt(0.5*(pol.e0 + ds));
	    axes->minor = sqrt(0.5*(pol.e0 - ds));
	}
    }

    if (!isfinite(axes->major) || !isfinite(axes->minor) || !isfinite(axes->theta)) {
	return psError(PS_ERR_BAD_PARAMETER_VALUE, true, "Shape of object is NaN");
    }

    return PS_ERR_NONE;
}

// ellipse rotation (major, minor, theta) -> (x2, y2, xy)
psEllipseMoments psEllipseAxesToMoments(psEllipseAxes axes)
{
    psEllipseMoments moments;

    double f1 = PS_SQR(axes.major) + PS_SQR(axes.minor);
    double f2 = PS_SQR(axes.major) - PS_SQR(axes.minor);

    moments.x2 = +0.5*f1 + 0.5*f2*cos(2*axes.theta);
    moments.y2 = +0.5*f1 - 0.5*f2*cos(2*axes.theta);
    moments.xy = +0.5*f2*sin(2*axes.theta);

    assert (isfinite(moments.x2));
    assert (isfinite(moments.y2));
    assert (isfinite(moments.xy));

    return moments;
}

// ellipse rotation (x2, y2, xy) -> (major, minor, theta)
psEllipseAxes psEllipseMomentsToAxes(psEllipseMoments moments, double maxAR)
{
    psEllipseAxes axes;
    psEllipseAxes badValue = {NAN, NAN, NAN};

    if (!isfinite(moments.x2)) return badValue;
    if (!isfinite(moments.y2)) return badValue;
    if (!isfinite(moments.xy)) return badValue;

    if (moments.x2 < 0) return badValue;
    if (moments.y2 < 0) return badValue;

    double g1 = moments.x2 + moments.y2;
    double g2 = moments.x2 - moments.y2;
    double g3 = sqrt(PS_SQR(g2) + 4*PS_SQR(moments.xy));

    axes.major = sqrt (0.5*(g1 + g3));
    axes.theta = +0.5 * atan2 (+2.0*moments.xy, g2); // theta in radians

    // long, thin objects are likely to have a poorly measured minor axis
    // the angle and major axis are likely to be ok.
    // restrict the axis ratio
    double rAR2 = (g1 - g3) / (g1 + g3);
    if (rAR2 < 1.0/PS_SQR(maxAR)) {
        axes.minor = axes.major / maxAR;
    } else {
        axes.minor = sqrt (0.5*(g1 - g3));
    }

    assert (isfinite(axes.major));
    assert (isfinite(axes.minor));
    assert (isfinite(axes.theta));

    return axes;
}

// ellipse rotation (major, minor, theta) -> (sx, sy, sxy)
psEllipseShape psEllipseAxesToShape(psEllipseAxes axes)
{
    psEllipseShape shape;
    psEllipseShape badValue = {NAN, NAN, NAN};

    if (!isfinite(axes.minor)) return badValue;
    if (!isfinite(axes.major)) return badValue;
    if (!isfinite(axes.theta)) return badValue;

    if (axes.minor <= 0) return badValue;
    if (axes.major <= 0) return badValue;

    double f1 = 1.0 / PS_SQR(axes.minor) + 1.0 / PS_SQR(axes.major);
    double f2 = 1.0 / PS_SQR(axes.minor) - 1.0 / PS_SQR(axes.major);

    double sxr = 0.5*f1 - 0.5*f2*cos(2*axes.theta);
    double syr = 0.5*f1 + 0.5*f2*cos(2*axes.theta);

    // sxr, syr cannot be < 0 (f1 >= f2)

    shape.sx  = +1.0 / sqrt(sxr);
    shape.sy  = +1.0 / sqrt(syr);
    shape.sxy = -0.5*f2*sin(2*axes.theta);

    assert (isfinite(shape.sx));
    assert (isfinite(shape.sy));
    assert (isfinite(shape.sxy));

    return shape;
}

// ellipse derotation (sx, sy, sxy) -> (major, minor, theta)
psEllipseAxes psEllipseShapeToAxes(psEllipseShape shape, double maxAR)
{
    psEllipseAxes axes;

    double f1 = 1.0 / PS_SQR(shape.sy) + 1.0 / PS_SQR(shape.sx);
    double f2 = 1.0 / PS_SQR(shape.sy) - 1.0 / PS_SQR(shape.sx);
    double f3 = sqrt(PS_SQR(f2) + 4*PS_SQR(shape.sxy));

    axes.minor = sqrt (2.0 / (f1 + f3));
    axes.theta = -0.5 * atan2 (+2.0*shape.sxy, f2);

    // long, thin objects are likely to have a poorly measured major axis
    // the angle and minor axis are likely to be ok.
    // restrict the axis ratio
    double rAR2 = (f1 - f3) / (f1 + f3);
    if (rAR2 < 1.0/PS_SQR(maxAR)) {
        axes.major = axes.minor * maxAR;
    } else {
        axes.major = sqrt (2.0 / (f1 - f3));
    }

    assert (isfinite(axes.theta));
    assert (isfinite(axes.major));
    assert (isfinite(axes.minor));

    return axes;
}

// XXX keep this construction?
// force the axis ratio to be less than 10
// double r1 = 0.5*0.95*sqrt (PS_SQR(f1) - PS_SQR(f2));

//    double f = sqrt (0.25*PS_SQR(moments.x2 - moments.y2) + PS_SQR(moments.xy));
//    if (f > (moments.x2 + moments.y2) / 2.0) {
//        f = 0.98*(moments.x2 + moments.y2) / 2.0;
