
// QuadraticInfo.cpp - class to compute facet angle from quadratic Bezier segment

#define _USE_MATH_DEFINES  // so Windows version of <cmath> & <math.h> have M_PI
#include <cmath>
#include <limits>
#include <stdio.h>

#include "QuadraticInfo.hpp"

#include <Cg/atan2.hpp>
#include <Cg/determinant.hpp>
#include <Cg/dot.hpp>
#include <Cg/isnan.hpp>
#include <Cg/sign.hpp>
#include <Cg/saturate.hpp>

#include "countof.h"
#include "eval_bezier.hpp"  // XXX for evalRationalBezierCubic

#include "angle_range.hpp"  // for isWithinRange, normalizeAngle, & absoluteNormalizeAngle

// Compute 2D squared distance
template <typename FLOAT, typename FLOAT2>
static FLOAT sqdist(FLOAT2 v)
{
    return dot(v, v);
}

QuadraticInfo::QuadraticInfo(const float3 cps[3])
{
    for (size_t i=0; i<countof(cp); i++) {
        cp[i] = cps[i];
    }
    computeMonomialCoeffs();
    computeLimitAngleAndDirection();
    computeCurvatureState();
}

void QuadraticInfo::computeCurvatureState()
{
    double2 v1 = double2(cp[1].xy) - cp[0].xy;
    double2 v2 = double2(cp[2].xy) - cp[1].xy;
    curvature = determinant(float2x2(v1, v2));
}

// Maple math:
// 
// > q:=(1-t)^2*cp0 + 2*(1-t)*t*cp1 + t^2*cp2;
// 
//                          2                          2
//              q := (1 - t)  cp0 + 2 (1 - t) t cp1 + t  cp2
// 
// > diff(q,t);
// 
//           -2 (1 - t) cp0 - 2 t cp1 + 2 (1 - t) cp1 + 2 t cp2
// 
// > collect(%,t);
// 
//               (2 cp0 - 4 cp1 + 2 cp2) t - 2 cp0 + 2 cp1
//
// > A := coeff(%,t,1);
//
//                      A: = 2 cp0 - 4 cp1 + 2 cp2
//
// > B := coeff(%%,t,0);
//
//                         B := -2 cp0 + 2 cp1
//
void QuadraticInfo::computeMonomialCoeffs()
{
    A = double2( 2*double2(cp[0].xy) - 4*double2(cp[1].xy) + 2*double2(cp[2].xy));
    B = double2(-2*double2(cp[0].xy) + 2*double2(cp[1].xy));
}

void QuadraticInfo::computeLimitAngleAndDirection() 
{
    if (A.y != 0 || A.x != 0) {
        limit_angle = atan2(A.y, A.x);
        double limit_anglen = atan2(-A.y, -A.x);
        limit_angle_neg = limit_angle + M_PI;
        if (limit_angle_neg > M_PI) {
            limit_angle_neg -= 2*M_PI;
        }
        limit_direction = A;
    } else {
        limit_angle = atan2(B.y, B.x);
        limit_direction = B;
    }
}

static double3 homodiff(const double3& a, const double3& b)
{
    return double3(a.xy*b.z - b.xy*a.z, a.z*b.z);
}

// returns angle
double QuadraticInfo::biasQuadraticFacetAngleAndFindT(float t,
                                                      float t_direction,
                                                      double theta,
                                                      float bias)
{
    assert(t_direction == -1 || t_direction == +1);
    // Is the curve essentially flat here?
    if (fabs(curvature) <= 0.0000001) {
        // Yes, just return t;
        return t;
    }
    const float curvature_direction = float(sign(curvature));
    const float oriented_bias = bias*t_direction*curvature_direction;
    const float tentative_theta = float(theta + oriented_bias);

    const bool tentative_theta_crosses_limit_angle = isWithinRange(float(limit_angle),
        float(theta), tentative_theta);
    const bool tentative_theta_crosses_limit_angle_neg = isWithinRange(float(limit_angle_neg),
        float(theta), tentative_theta);

    // shouldn't both be true!
    assert(!(tentative_theta_crosses_limit_angle_neg && tentative_theta_crosses_limit_angle));

    if (tentative_theta_crosses_limit_angle) {
        assert(t_direction == 1);
        double new_theta = limit_angle;
        return std::numeric_limits<double>::infinity();
    } else if (tentative_theta_crosses_limit_angle_neg) {
        assert(t_direction == -1);
        double new_theta = limit_angle_neg;
        return -std::numeric_limits<double>::infinity();
    } else {
        float biased_angle = tentative_theta;
        return findCorrectT(biased_angle, t, t_direction);
    }
}

template <typename FLOAT, typename FLOAT2>
static FLOAT2 get_ab(FLOAT theta, const FLOAT2& A, const FLOAT2& B)
{
    // Rotate A & B by theta
    const FLOAT a = sin(theta)*A.x - A.y*cos(theta);
    const FLOAT b = sin(theta)*B.x - B.y*cos(theta);

    return FLOAT2(a,b);
}

double QuadraticInfo::findCorrectT(float angle,
                                   float orig_t, 
                                   float t_direction)
{
    const double2 ab = get_ab<double,double2>(angle, A, B);
    const double a = ab.x;
    const double b = ab.y;

    // given: 0 = a*t + b
    //
    // solve for t: t = -b/a;

    // Linear, so find one solution...
    if (fabs(a) > 0.00001) {
        const double t = -b / a;
        return t;
    } else {
        if (t_direction == 1) {
            return std::numeric_limits<double>::infinity();
        } else {
            assert(t_direction == -1);
            return -std::numeric_limits<double>::infinity();
        }
    }
}


double QuadraticInfo::fromBiasedAngleFindT(float theta,
                                           float t, float t_direction, float bias)
{
    const float biased_t = float(biasQuadraticFacetAngleAndFindT(t, t_direction, theta, bias));
    return biased_t;
}

double2 QuadraticInfo::computeQuadraticFacetTPair(float t,
                                                  float theta,
                                                  float max_turning_angle_delta,
                                                  int step_mode)
{
    float step_neg = max_turning_angle_delta;
    float step_pos = max_turning_angle_delta;
    switch (step_mode-1) {
    case -1:
        step_neg *= 0.5f;
        break;
    case 0:
        break;
    case +1:
        step_pos *= 0.5;
        break;
    case 2:
        step_neg *= 0.5;
        step_pos *= 0.5;
        break;
    }

    double t_neg = fromBiasedAngleFindT(theta, t, -1, step_neg);
    double t_pos = fromBiasedAngleFindT(theta, t, +1, step_pos);

    return double2(t_neg, t_pos);
}

float QuadraticInfo::computeFacetAngle(float t,
                                       float max_turning_angle_delta,
                                       int step_mode)
{
    const double2 gradient = A*t+B;
    const double theta = atan2(gradient.y, gradient.x);

    double2 t_pair;
    t_pair = computeQuadraticFacetTPair(t, float(theta), max_turning_angle_delta, step_mode);

    double t_neg = t_pair[0];
    double t_pos = t_pair[1];

    const double2 gradient_neg = A*t_neg+B;
    const double2 gradient_pos = A*t_pos+B;

    const double theta_neg = atan2(gradient_neg.y, gradient_neg.x);
    const double theta_pos = atan2(gradient_pos.y, gradient_pos.x);

    const float dtheta_pos = normalizeAngle(float(theta_pos - theta));
    const float dtheta_neg = normalizeAngle(float(theta - theta_neg));

    if (sqdist<double,double2>(gradient_pos)>0 && dtheta_pos == dtheta_pos) {
        if (!(1.5*max_turning_angle_delta > fabs(dtheta_pos))) {
            printf("dtheta_pos too big %g\n", dtheta_pos);
        }
    }
    if (sqdist<double,double2>(gradient_neg)>0 && dtheta_neg == dtheta_neg) {
        if (!(1.5*max_turning_angle_delta > fabs(dtheta_neg))) {
            printf("dtheta_neg too big %g\n", dtheta_neg);
        }
    }

    assert(t_neg <= t);
    assert(t <= t_pos);

    extern bool clamp_mode;
    if (clamp_mode) {
        t_neg = saturate(t_neg);
        t = saturate(t);
        t_pos = saturate(t_pos);
    }

    // Evaluate position on cubic at t_neg, t, and t_pos.
    const double3 Pn = evalRationalBezierQuadratic(cp, t_neg);
    const double3 P0 = evalRationalBezierQuadratic(cp, double(t));
    const double3 Pp = evalRationalBezierQuadratic(cp, t_pos);

    const double2 Dp0 = homodiff(Pp, P0).xy;
    const double2 D0n = homodiff(P0, Pn).xy;

    float Dp0_rad = float(atan2(Dp0.y, Dp0.x));
    float D0n_rad = float(atan2(D0n.y, D0n.x));

    // If either computed angle "blows up" as NaN, remap to the limit angle.
    if (Cg::isnan(Dp0_rad)) {
        Dp0_rad = float(limit_angle);
    }
    if (Cg::isnan(D0n_rad)) {
        D0n_rad = float(limit_angle_neg);
    }

    const float abs_dif = absoluteNormalizeAngle(Dp0_rad - D0n_rad);
    return abs_dif;
}
