
// CenterPointArc.cpp - partial elliptical arc representations implementations

#define _USE_MATH_DEFINES
#include <math.h>  // for M_PI

#include <Cg/double.hpp>
#include <Cg/vector/xyzw.hpp>
#include <Cg/vector.hpp>

#include "CenterPointArc.hpp"

#include <Cg/abs.hpp>
#include <Cg/acos.hpp>
#include <Cg/all.hpp>
#include <Cg/any.hpp>
#include <Cg/clamp.hpp>
#include <Cg/distance.hpp>
#include <Cg/dot.hpp>
#include <Cg/isinf.hpp>
#include <Cg/isnan.hpp>
#include <Cg/length.hpp>
#include <Cg/max.hpp>
#include <Cg/mul.hpp>
#include <Cg/radians.hpp>
#include <Cg/sqrt.hpp>
#include <Cg/transpose.hpp>

// Avoid isnan confusion; always use cg4cpp version, even for scalar usage.
// <math.h> implements isnan as a macro, conflicting with an overloaded
// function version implemented in <Cg/isnan.hpp>.  This forces <Cg/isnan.hpp>
// to #undef the isnan macro.  <cmath> doesn't introduce std::isnan until C++11.
using Cg::isnan;

// Equation F.6.5.4
static double arc_angle_helper(double2 u, double2 v)
{
    double cos_value = dot(u,v)/(length(u)*length(v)),
           clamped_cos_value = clamp(cos_value, -1.0, 1.0);
    double theta_sign = double(u.x*v.y - u.y*v.x < 0 ? -1 : 1);
    double angle = theta_sign * acos(clamped_cos_value);
    return angle;
}

template <typename T>
static bool illbehaved(T a)
{
    return any(Cg::isnan(a)) || any(Cg::isinf(a));
}

CenterPointArc::CenterPointArc(const EndPointArc &src)
{
    // Handle these two easy/common degenerate cases first.
    if (all(src.p[0] == src.p[1])) {
        // "If the endpoints (x1, y1) and (x2, y2) are identical, then this
        // is equivalent to omitting the elliptical arc segment entirely."
        CenterPointArc::form = DEGENERATE_POINT;
        // other members left undefined
        return;
    }
    if (src.radii.x == 0 || src.radii.y == 0) {
        CenterPointArc::form = DEGENERATE_LINE;
        CenterPointArc::p[0] = src.p[0];
        CenterPointArc::p[1] = src.p[1];
        // other members left undefined
        return;
    }

    // Conversion from endpoint arc to center arc parameterization
    // See http://www.w3.org/TR/SVG11/implnote.html#ArcImplementationNotes
    const double2 v1 = double2(src.p[0]),  // v1.x = x1, v1.y = y1
                  v2 = double2(src.p[1]);  // v2.x = x2, v2.y = x2

    // Ensure radii are positive
    // Take the absolute value of rX and rY:
    double2 r = double2(abs(src.radii));  // r not const since could be adjusted if too small

    // Any nonzero value for either of the flags fA or fS is taken to mean the value 1
    const bool fA = src.large_arc_flag,
               fS = src.sweep_flag;

    // psi is taken mod 360 degrees.
    // [fine since sin and cos functions do this (better)]
    const double1 psi = radians(double1(src.x_axis_rotation));

    // Step 1: Compute (x1', y1') according to the formula
    // cw = clockwise
    const double2x2 cw_rotate_by_psi = double2x2(cos(psi), sin(psi),
                                                 -sin(psi), cos(psi));

    const double2 half_v_diff = (v1-v2)/2,
                  v1p = mul(cw_rotate_by_psi, half_v_diff);

    { // BEGIN F.6.6 Correction of out-of-range radii

        // Step 1: Ensure radii are non-zero
        // If rX = 0 or rY = 0, then treat this as a straight line
        // from (x1, y1) to (x2, y2) and stop.
        // [already earlier]

        // Otherwise,
        // Step 2: Ensure radii are positive
        // Take the absolute value of rX and rY:
        // [already done]

        // Step 3: Ensure radii are large enough
        // Using the primed coordinate values of equation (F.6.5.1), compute
        const double Lambda = dot(v1p/r,v1p/r);

        // If the result of the above equation is less than or equal to 1,
        // then no further change need be made to rX and rY.  If the result
        // of the above equation is greater than 1, then make the replacements
        if (Lambda > 1) {
            r = sqrt(Lambda)*r;
        }
        // Step 4: Proceed with computations

        // Proceed with the remaining elliptical arc computations, such as
        // those in section F.6.5.  Note: As a consequence of the radii
        // corrections in this section, equation (F.6.5.2) for the center
        // of the ellipse always has at least one solution (i.e. the
        // radicand is never negative).  In the case that the radii are
        // scaled up using equation (F.6.6.3), the radicand of (F.6.5.2)
        // is zero and there is exactly one solution for the center of the
        // ellipse.
    } // END F.6.6 Correction of out-of-range radii

    // Step 2: Compute (cX', cY') according to the [re-arranged] formula
    // Rearrange sqrt((rx^2*ry^2 - rx^2*y1p^2 - ry^2*x1p^2)/
    //                (rx^2*y1p^2 + ry^2*x1p^2))
    // to be sqrt(rx^2*ry^2)/(rx^2*y1p^2 + ry^2*x1p^2))-1
    // because sqrt((- rx^2*y1p^2 - ry^2*x1p^2)/
    //              (rx^2*y1p^2 + ry^2*x1p^2))
    // equals -1
    double numer = r.x*r.x * r.y*r.y,
           denom = dot(r*r, (v1p*v1p).yx);
    double radicand = numer /denom - 1;
    // Add max to make sure numer/denom-1 can't be negative
    double k = sqrt(max(0, radicand));
    // where the + sign is chosen if fA notequal fS
    // and the - sign is chosen if fA equal fS
    if (fA == fS) {
        k = -k;
    }
    double2 cp = k*(r*(v1p/r).yx);
    cp.y = -cp.y;

    // Step 3: Compute (cX, cY) from (cX', cY')
    // ccw = counterclockwise
    const double2x2 ccw_rotate_by_psi = transpose(cw_rotate_by_psi);
    const double2 average_v = (v1+v2)/2,
                  c = mul(ccw_rotate_by_psi, cp) + average_v;

    // Step 4: Compute theta1 and delta-theta

    // This angle function can be used to express theta1 and delta-theta as follows
    double1 theta1 = arc_angle_helper(double2(1,0), (v1p-cp)/r),
            delta_theta = arc_angle_helper((v1p-cp)/r, (-v1p-cp)/r);
    // where theta1 is fixed in the range -360 degrees < delta-theta < 360
    // degrees such that:
    // if fS equal 0, then delta-theta < 0,
    // else if fS equal 1, then delta-theta > 0.
    //
    // In other words, if fS = 0 and the right side of (F.6.5.6) is > 0,
    // then subtract 360 degrees, whereas if fS = 1 and the right side of
    // (F.6.5.6) is < 0, then add 360 degrees.  In all other cases leave it as is.
    const double pi = M_PI;
    if (!fS && delta_theta > 0) {
        delta_theta = delta_theta - 2*pi;
    } else if (fS && delta_theta < 0) {
        delta_theta = delta_theta + 2*pi;
    }

    // Weird stuff could happen due to division by zero or
    // overflow; treat any such situation as a degenerate line.
    if (illbehaved(c) || illbehaved(r) ||
        illbehaved(theta1) || illbehaved(delta_theta) || illbehaved(psi)) {
        CenterPointArc::form = DEGENERATE_LINE;
        CenterPointArc::p[0] = src.p[0];
        CenterPointArc::p[1] = src.p[1];
        // other members left undefined
        return;
    }

    // Assign back to the member variables 
    // (scoping avoids ambiguity because several locals are declared
    // double precision with identical names to the member variables)
    CenterPointArc::form = BEHAVED;
    CenterPointArc::center = float2(c);
    CenterPointArc::radii = float2(r);
    CenterPointArc::p[0] = src.p[0];
    CenterPointArc::p[1] = src.p[1];
    CenterPointArc::theta1 = float(theta1);
    CenterPointArc::delta_theta = float(delta_theta);
    CenterPointArc::psi = float(psi);

    // Early return possible for degenerate cases!
}

// Helper to return arc tangent of a 2D vector.
static float atan2(float2 xy)
{
    return atan2(xy.y, xy.x);  // Y then X for atan2.
}

CenterPointArc CenterPointArc::makeCircularCenterPointArc(
    const float2& center,
    const float2& start_direction,  // normalized
    const float2& end_direction,  // normalized
    float radius)
{
    CenterPointArc arc;
    arc.form = CenterPointArc::BEHAVED;
    arc.center = center;
    arc.radii = radius;  // Circular major and minor axis radii the same.
    arc.p[0] = start_direction;
    arc.p[1] = end_direction;
    arc.theta1 = atan2(start_direction - center);
    const float theta2 = atan2(end_direction - center);
    arc.delta_theta = theta2 - arc.theta1;
    const float pi = float(M_PI);
    if (arc.delta_theta >= pi) {
        arc.delta_theta -= 2*pi;
    } else if (arc.delta_theta <= -pi) {
        arc.delta_theta += 2*pi;
    }
    arc.psi = 0;  // Zero OK since round join is circular.
    return arc;
}

// See F.6.3 Parameterization alternatives 
// http://www.w3.org/TR/SVG/implnote.html#ArcParameterizationAlternatives
double2 CenterPointArc::eval(double theta) const {
    double dpsi = psi,
           cos_psi = cos(dpsi),
           sin_psi = sin(dpsi),
           cos_theta = cos(theta),
           sin_theta = sin(theta);
    double2 result;
    result.x = cos_psi*radii.x*cos_theta - sin_psi*radii.y*sin_theta;
    result.y = sin_psi*radii.x*cos_theta + cos_psi*radii.y*sin_theta;
    result += center;
    return result;
}

// Evaluate position on arc where t is a [0,1] weighting of theta.
double2 CenterPointArc::evalPercent(double t) const
{
    const double theta = theta1 + t*delta_theta;

    return eval(theta);
}

// Returns normalized gradient (the tangent) of the arc at angle theta.
double2 CenterPointArc::tangent(double theta) const
{
    if (form == CenterPointArc::DEGENERATE_POINT) {
        return double2(0);
    }
    double rx = radii.x,
           ry = radii.y,
           sin_psi = sin(psi),
           cos_psi = cos(psi),
           sin_theta = sin(theta),
           cos_theta = cos(theta);

    double2 dARCdTheta;
    dARCdTheta.x = -cos_psi*rx*sin_theta - sin_psi*ry*cos_theta;
    dARCdTheta.y = -sin_psi*rx*sin_theta + cos_psi*ry*cos_theta;
    double len = length(dARCdTheta);
    if (len > 0) {
        double2 tangent = dARCdTheta/len;
        // Is the arc rotating in the negative direction?
        if (delta_theta < 0) {
            // Yes, flip the tangent direction then.
            tangent = -tangent;
        }
        return tangent;
    }
    return double2(0);
}

// Returns normalized gradient at the initial angle.
double2 CenterPointArc::initialTangent() const
{
    const double initial_theta = theta1;
    return tangent(initial_theta);
}

float2 CenterPointArc::initialTangentf() const
{
    return float2(initialTangent());
}

// Returns normalized gradient at the terminal angle.
double2 CenterPointArc::terminalTangent() const
{
    const double terminal_theta = theta1 + delta_theta;
    return tangent(terminal_theta);
}

float2 CenterPointArc::terminalTangentf() const
{
    return float2(terminalTangent());
}

double CenterPointArc::approximateArcLength() const
{
    double2 p04 = double2(p[0]),
            p14 = evalPercent(0.25),
            p24 = evalPercent(0.50),
            p34 = evalPercent(0.75),
            p44 = double2(p[1]);

    const double approxLength = distance(p04,p14)
                              + distance(p14,p24)
                              + distance(p24,p34)
                              + distance(p34,p44);
    return approxLength;
}

void CenterPointArc::splitPercent(double t, CenterPointArc arc_pair[2]) const
{
    const double mid_delta_theta = t*delta_theta;
    const double mid_theta = theta1 + mid_delta_theta;
    const float2 mid = float2(eval(mid_theta));

    CenterPointArc& arc1 = arc_pair[0];
    arc1.form = form;
    arc1.center = center;
    arc1.radii = radii;
    arc1.p[0] = p[0];
    arc1.p[1] = mid;
    arc1.theta1 = theta1;
    arc1.delta_theta = float(mid_delta_theta);
    arc1.psi = psi;

    CenterPointArc& arc2 = arc_pair[1];
    arc2.form = form;
    arc2.center = center;
    arc2.radii = radii;
    arc2.p[0] = mid;
    arc2.p[1] = p[1];
    arc2.theta1 = float(mid_theta);
    arc2.delta_theta = float(delta_theta - mid_delta_theta);
    arc2.psi = psi;
}

// Given two 2D lines (assumed to cross by their construction), find their point of intersection.
static float2 intersection_of_two_2D_lines(float2 r, float2 a,  // point p @ r heading in a's direction
                                           float2 s, float2 b)  // point q @ s heading in b's direction
{
    float numer = b.x*(s.y-r.y)-b.y*(s.x-r.x),
          denom = b.x*a.y-a.x*b.y,
          lambda = numer/denom;

   assert(denom != 0);
   float2 p = r+lambda*a;
   return p;
}

void CenterPointArc::splitToConics(ConicSplitFunc func, float max_angle_per_conic, void* handle) const
{
    assert(form == BEHAVED);  // XXX fix me!
    assert(max_angle_per_conic > 0);
    const float abs_delta_theta = fabs(delta_theta);
    const float split_theta = abs_delta_theta/max_angle_per_conic;
    const int splits = int(floor(split_theta));
    assert(splits >= 0);
    const float percent_theta_step = delta_theta/(splits+1);

    // Array of 3 control points [pa,pb,pc] for the conic Bezier triangle.
    float2 cp[3];
    float2& pa = cp[0];
    float2& pb = cp[1];
    float2& pc = cp[2];

    pa = p[0];  // Arc's start-point position.
    const double2 t0 = tangent(theta1);  // Arc's start-point tangent.
    float2 ta = float2(t0);
    const float w = cos(fabs(percent_theta_step)/2);
    for (int i=1; i<=splits; i++) {
        const float theta = theta1 + percent_theta_step*i;
        pc = float2(eval(theta));
        const float2 tc = float2(tangent(theta));

        pb = intersection_of_two_2D_lines(pa, ta, pc, -tc);
        
        func(cp, w, handle);
        pa = pc;
        ta = tc;
    }
    const double2 t1 = tangent(theta1 + delta_theta);
    pc = p[1];  // Arc's end-point position.
    const float2 tc = float2(t1);  // Arc's end-point tangent.
    pb = intersection_of_two_2D_lines(pa, ta, pc, -tc);
    func(cp, w, handle);
}

