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

psEllipseAxes psEllipseMomentsToAxes(psEllipseMoments moments)
{
    psEllipseAxes axes;

    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;
    }

    axes.major = sqrt (0.5*(moments.x2 + moments.y2) + f);
    axes.minor = sqrt (0.5*(moments.x2 + moments.y2) - f);
    axes.theta = atan2 (2*moments.xy, moments.x2 - moments.y2) / 2;
    // theta in radians

    return axes;
}

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

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

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

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

    return shape;
}

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

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

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

    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));
