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

psEllipseShape psEllipseAxesToShape(psEllipseAxes axes)
{
    psEllipseShape shape;

    double r1 = 1.0 / PS_SQR(axes.major) + 1.0 / PS_SQR(axes.minor);
    double r2 = 1.0 / PS_SQR(axes.major) - 1.0 / PS_SQR(axes.minor);

    double sxr = r1 + r2*cos(2*axes.theta);
    double syr = r1 - r2*cos(2*axes.theta);

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

    return shape;
}

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

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

    shape.sxy = PS_MIN(PS_MAX(shape.sxy, -r1), r1);

    axes.theta = atan2 (-2.0*shape.sxy, f2) / 2.0;

    double Ar = 0.25*f1 + 0.25*sqrt(PS_SQR(f2) + 4*PS_SQR(shape.sxy));
    double Br = 0.25*f1 - 0.25*sqrt(PS_SQR(f2) + 4*PS_SQR(shape.sxy));

    axes.minor = 1.0 / sqrt (Ar);
    axes.major = 1.0 / sqrt (Br);

    return axes;
}
