
// CubicInfo.cpp - class to compute facet angle from cubic Bezier segment

#include <math.h>
#include <limits>
#include <stdio.h>

#include "CubicInfo.hpp"

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

#include "countof.h"
#include "eval_bezier.hpp"
#include "cubicInflectionPoint.hpp"

#include "angle_range.hpp"

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

template <typename FLOAT, typename FLOAT2>
FLOAT computeCurvatureSign(float t, const FLOAT2& A, const FLOAT2& B, const FLOAT2& C)
{
    // Gradient coefficients A, B, and C work for quadratic and cubic Bezier segments.
    const FLOAT2 gradient = FLOAT2(t*(t*A + B) + C);  // Horner quadratic evaluation
    const FLOAT2 gradient2 = FLOAT2(2*t*A + B);  // Second order gradient

    const FLOAT curvature_numer = gradient.x*gradient2.y - gradient.y*gradient2.x;
    // Skip computing the curvature denominator as it is always positive.
    const FLOAT curvature_denom = pow(sqdist<FLOAT, FLOAT2>(gradient), FLOAT(1.5));

    return curvature_numer;
}

template <typename FLOAT, typename FLOAT2>
FLOAT computeCurvatureSignAtZero(FLOAT2& B, const FLOAT2& C)
{
    // Gradient coefficients A, B, and C work for quadratic and cubic Bezier segments.
    const FLOAT2 gradient = FLOAT2(C);  // Horner quadratic evaluation
    const FLOAT2 gradient2 = FLOAT2(B);  // Second order gradient

    const FLOAT curvature_numer = gradient.x*gradient2.y - gradient.y*gradient2.x;
    // Skip computing the curvature denominator as it is always positive.
    return curvature_numer;
}

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

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
static float biasCubicFacetAngle(int max_interval,
                                 const float angle_bound[/*max_interval+2*/],
                                 const double curvature[/*max_interval+1*/],
                                 float t,
                                 float t_direction,
                                 float theta,
                                 int& interval,
                                 float& bias)
{
    assert(t_direction == -1 || t_direction == +1);
    double interval_curvature = curvature[interval];
    float curvature_direction = float(sign(interval_curvature));
    float oriented_bias = bias*t_direction*curvature_direction;
    float tentative_theta = float(theta + oriented_bias);
    const int left_edge = interval;
    const int right_edge = interval+1;
    bool tentative_theta_in_interval = isWithinOrientedRange(tentative_theta,
        angle_bound[left_edge], angle_bound[right_edge], curvature_direction);

    if (tentative_theta_in_interval) {
        bias = 0;
        const float new_theta = tentative_theta;
        // Leave interval unchanged.
        return new_theta;
    } else {
        const float clamped_tentative_theta = clampToOrientedRange(tentative_theta,
            angle_bound[left_edge], angle_bound[right_edge], curvature_direction);
        if (clamped_tentative_theta == angle_bound[left_edge] && t_direction == -1) {
            if (interval == 0) {
                const float new_theta = angle_bound[left_edge];
                bias = 0;
                return new_theta;
            } else {
                float new_theta = angle_bound[left_edge];
                bias -= absoluteNormalizeAngle(new_theta - theta);
                interval--;
                return new_theta;
            }
        } else if (clamped_tentative_theta == angle_bound[right_edge] && t_direction == +1) {
            if (interval >= max_interval) {
                assert(interval == max_interval);
                const float new_theta = angle_bound[right_edge];
                bias = 0;
                return new_theta;
            } else {
                const float new_theta = angle_bound[right_edge];
                bias -= absoluteNormalizeAngle(new_theta - theta);
                interval++;
                return new_theta;
            }
        } else {
            bias = 0;
            const float new_theta = tentative_theta;
            // Leave interval unchanged.
            return new_theta;
        }
    }
}

CubicInfo::CubicInfo(const float3 cps[4])
{
    for (int i=0; i<4; i++) {
        cp[i] = cps[i];
    }
    computeMonomialCoeffs();
    computeLimitAngleAndDirection();
    computeCurvatureState();
}

void CubicInfo::computeLimitAngleAndDirection() 
{
    if (A.y != 0 || A.x != 0) {
        limit_angle = float(atan2(A.y, A.x));
        limit_direction = A;
    } else if (B.y != 0 || B.x != 0) {
        limit_angle = float(atan2(B.y, B.x));
        limit_direction = B;
    } else {
        limit_angle = float(atan2(C.y, C.x));
        limit_direction = C;
    }
}

void CubicInfo::computeMonomialCoeffs()
{
    A = double2(9*double2(cp[1].xy)-3*double2(cp[0].xy)-9*double2(cp[2].xy)+3*double2(cp[3].xy));
    B = double2(-12*double2(cp[1].xy)+6*double2(cp[0].xy)+6*double2(cp[2].xy));
    C = double2(-3*double2(cp[0].xy)+3*double2(cp[1].xy));
}

void CubicInfo::computeReversingLineCurvatureState(const double key_point[1])
{
    kind = REVERSING_LINE;
    inflection_t[0] = float(key_point[0]);

    const float i0t = inflection_t[0];
    const double2 i0_gradient = (A*i0t+B)*i0t+C;
    const float i0_theta = float(atan2(i0_gradient.y, i0_gradient.x));

    t_bound[0] = -std::numeric_limits<double>::infinity();
    t_bound[1] = i0t;
    t_bound[2] = +std::numeric_limits<double>::infinity();
    t_bound[3] = sqrt(-1.0); // NaN

    angle_bound[0] = limit_angle;
    angle_bound[1] = i0_theta;
    angle_bound[2] = limit_angle;
    angle_bound[3] = sqrtf(-1.0f); // NaN

    curvature[0] = computeCurvatureSign<double,double2>(i0t - 10, A, B, C);
    curvature[1] = computeCurvatureSign<double,double2>(i0t + 10, A, B, C);
    curvature[2] = sqrt(-1.0); // NaN

    assert(curvature[0] * curvature[1] <= 0);
}

void CubicInfo::computeSerpintineCurvatureState(const double key_point[2])
{
    kind = SERPENTINE;
    if (key_point[0] <= key_point[1]) {
        inflection_t[0] = float(key_point[0]);
        inflection_t[1] = float(key_point[1]);
    } else {
        // Swap so parametric t for inflections in increasing order.
        inflection_t[0] = float(key_point[1]);
        inflection_t[1] = float(key_point[0]);
    }

    const float i0t = inflection_t[0];
    const float i1t = inflection_t[1];

    const double2 i0_gradient = (A*i0t+B)*i0t+C;
    const float i0_theta = float(atan2(i0_gradient.y, i0_gradient.x));

    const double2 i1_gradient = (A*i1t+B)*i1t+C;
    const float i1_theta = float(atan2(i1_gradient.y, i1_gradient.x));

    t_bound[0] = -std::numeric_limits<double>::infinity();
    t_bound[1] = i0t;
    t_bound[2] = i1t;
    t_bound[3] = +std::numeric_limits<double>::infinity();

    angle_bound[0] = limit_angle;
    angle_bound[1] = i0_theta;
    angle_bound[2] = i1_theta;
    angle_bound[3] = limit_angle;

    curvature[0] = computeCurvatureSign<double,double2>(i0t - 10, A, B, C);
    curvature[1] = computeCurvatureSign<double,double2>((i0t+i1t)/2, A, B, C);
    curvature[2] = computeCurvatureSign<double,double2>(i1t + 10, A, B, C);

    assert(curvature[0] * curvature[1] <= 0);
    assert(curvature[1] * curvature[2] <= 0);
    assert(curvature[0] * curvature[2] >= 0);
}

void CubicInfo::computeLoopCurvatureState()
{
    kind = LOOP;
    curvature[0] = computeCurvatureSignAtZero<double,double2>(B, C);
    curvature[1] = sqrt(-1.0); // NaN
    curvature[2] = sqrt(-1.0); // NaN
}

void CubicInfo::computeCurvatureState()
{
    double key_point[2];
    const int num_inflections = cubicInflectionPoint(cp, key_point);

    switch (num_inflections) {
    default:
        assert(!"invalid num_inflections");
    case 0:
        computeLoopCurvatureState();
        break;
    case 1:
        computeReversingLineCurvatureState(key_point);
        break;
    case 2:
        computeSerpintineCurvatureState(key_point);
        break;
    }
}

double CubicInfo::pickBetterIntervalBound(int interval,
                                          float angle,
                                          float orig_t,
                                          float t_direction)
{
    const int left_edge = interval;
    const int right_edge = interval+1;
    const float left_dif = absoluteNormalizeAngle(angle - angle_bound[left_edge]);
    const float right_dif = absoluteNormalizeAngle(angle - angle_bound[right_edge]);
    if (left_dif < right_dif) {
        return t_bound[left_edge];
    } else {
        return t_bound[right_edge];
    }
}

float CubicInfo::findCorrectLoopT(float angle,
                                   float orig_t, 
                                   float t_direction)
{
    const double3 abc = get_abc<double,double2,double3>(angle, A, B, C);
    const double a = abc.x;
    const double b = abc.y;
    const double c = abc.z;
    double t = std::numeric_limits<double>::quiet_NaN();

    // Is equation linear or quadratic?
    if (a == 0) {
        // Linear, so find one solution...
        const double t0 = -c / b;
        t = t0;
    } else {
        // Quadratic, so find quadratic solutions...
        const double discr = b*b - 4*a*c;
        // Based on discriminant, decide between 2 and 0 solutions...
        if (discr >= 0) {
            // Two solutions...
            double t0 = (-b - sqrt(discr)) / (2*a);
            double t1 = (-b + sqrt(discr)) / (2*a);

            const double2 t0_gradient = (A*t0+B)*t0+C;
            const double2 t1_gradient = (A*t1+B)*t1+C;
            const float t0_angle = float(atan2(t0_gradient.y, t0_gradient.x));
            const float t1_angle = float(atan2(t1_gradient.y, t1_gradient.x));
            if (t1_angle == float(limit_angle)) {
                t1 = t_direction*std::numeric_limits<double>::infinity();
            }
            if (t0_angle == float(limit_angle)) {
                t0 = t_direction*std::numeric_limits<double>::infinity();
            }

            const float dif_t0 = absoluteNormalizeAngle(t0_angle - angle);
            const float dif_t1 = absoluteNormalizeAngle(t1_angle - angle);

            if (dif_t0 < dif_t1) {
                t = t0;
            } else {
                t = t1;
            }
        } else {
            // No real solutions so find best interval extreme.
            printf("no quadratic solution!\n");
            if (t_direction == -1) {
                t = -std::numeric_limits<double>::infinity();
            } else {
                assert(t_direction == +1);
                t = +std::numeric_limits<double>::infinity();
            }
        }
    }
    return float(t);
}

float CubicInfo::findCorrectSerpentineT(int interval, float angle, float orig_t, float t_direction)
{
    const double3 abc = get_abc<double,double2,double3>(angle, A, B, C);
    const double a = abc.x;
    const double b = abc.y;
    const double c = abc.z;
    double t = std::numeric_limits<double>::quiet_NaN();
    const int left_edge = interval;
    const int right_edge = interval+1;
    // Is equation linear or quadratic?
    if (a == 0) {
        // Linear, so find one solution...
        const double t0 = -c / b;
        // Is solution t0 in bounds of interval of interest?
        if (t_bound[left_edge] <= t0 && t0 <= t_bound[right_edge]) {
            t = t0;
        } else {
            t = pickBetterIntervalBound(interval, angle, orig_t, t_direction);
        }
    } else {
        // Quadratic, so find quadratic solutions...
        const double discr = b*b - 4*a*c;
        // Based on discriminant, decide between 2 and 0 solutions...
        if (discr >= 0) {
            // Two solutions...
            const double sqrt_discr = sqrt(discr);
            double t0 = (-b - sqrt_discr) / (2*a);
            double t1 = (-b + sqrt_discr) / (2*a);

            // XXX could be better structured
            const bool t0_in_bounds = t_bound[left_edge] <= t0 && t0 <= t_bound[right_edge];
            const bool t1_in_bounds = t_bound[left_edge] <= t1 && t1 <= t_bound[right_edge];

            if (t0_in_bounds && t1_in_bounds) {
                if (t_direction == -1) {
                    const double d0 = orig_t - t0;
                    const double d1 = orig_t - t1;
                    if (d0 >= 0 && d1 >= 0) {
                        if (d0 < d1) {
                            t = t0;
                        } else {
                            t = t1;
                        }
                    }
                    else if(d0 >= 0) {
                        t = t0;
                    } else if (d1 >= 0) {
                        t = t1;
                    } else {
                        t = pickBetterIntervalBound(interval, angle, orig_t, t_direction);;
                    }
                } else {
                    assert(t_direction == 1);
                    const double d0 = t0 - orig_t;
                    const double d1 = t1 - orig_t;
                    if (d0 >= 0 && d1 >= 0) {
                        if (d0 < d1) {
                            t = t0;
                        } else {
                            t = t1;
                        }
                    } else if (d0 >= 0) {
                        t = t0;
                    } else if (d1 >= 0) {
                        t = t1;
                    } else {
                        t = pickBetterIntervalBound(interval, angle, orig_t, t_direction);;
                    }
                }
            }
            // Is solution t0 in bounds of interval of interest?
            else if (t0_in_bounds) {
                t = t0;
            }
            // Is solution t0 in bounds of interval of interest?
            else if (t1_in_bounds) {
                t = t1;
            } else {
                t = pickBetterIntervalBound(interval, angle, orig_t, t_direction);
            }
            if (t_direction == -1) {
                if (t > orig_t) {
                    t = pickBetterIntervalBound(interval, angle, orig_t, t_direction);
                }
            } else {
                assert(t_direction == 1);
                if (t < orig_t) {
                    t = pickBetterIntervalBound(interval, angle, orig_t, t_direction);
                }
            }
        } else {
            // No real solutions so find best interval extreme.
            t = pickBetterIntervalBound(interval, angle, orig_t, t_direction);
        }
    }
    return float(t);
}

double CubicInfo::fromBiasedAngleFindReversingLineT(float theta,
                                                    int interval,
                                                    float t, float t_direction,
                                                    float bias)
{
    return fromBiasedAngleFindSerpentineT(theta, interval, t, t_direction, bias);
}

double CubicInfo::fromBiasedAngleFindSerpentineT(float theta,
                                                 int interval,
                                                 float t, float t_direction,
                                                 float bias)
{
    const int max_interval = 2;
    float biased_angle = theta;
    int iterations = 0;
    while (bias > 0 && iterations < 4) {
        biased_angle = biasCubicFacetAngle(
            max_interval,
            angle_bound,
            curvature,
            t, t_direction,
            biased_angle, interval, bias);
        iterations++;
    }
    if (iterations >= 4) {
        assert(bias < 0.0001);  // bias should be small after these iterations
    }

    float biased_t = findCorrectSerpentineT(interval, biased_angle, t, t_direction);
    return biased_t;
}

float CubicInfo::fixOutOfBoundsTheta(float theta, float t, int num_bounds, bool both_in_interval[/*num_bounds-1*/])
{
    // Determine the interval bound theta closest to the evaluated theta.
    double min_diff = normalizeAngle(theta - angle_bound[0]);
    int theta_index = 0;
    for (int i=1; i<num_bounds; i++) {
        double diff = normalizeAngle(theta - angle_bound[i]);
        if (diff < min_diff) {
            min_diff = diff;
            theta_index = i;
        }
    }
    assert(theta_index < num_bounds);
    // Careful when theta is close to the limit angle...
    // Is the theta_index the first or last index into angle_bound (either being the limit angle)?
    if (theta_index == 0 || theta_index == num_bounds-1) {
        // Yes...
        // Use t to pick which limit angle interval.
        if (t < 0.5f) {
            theta_index = 0;  // first interval.
        } else {
            theta_index = num_bounds-2;  // -2 for last interval.
        }
    }
    // This reason theta might outside each possible interval is due to numerical error so
    // expect the absolute difference to be small.
    const double absolute_dif = absoluteNormalizeAngle(theta - angle_bound[theta_index]);
#if 0
    assert(absolute_dif < 0.05);  // less than ~2.86 degrees
#else
    if (absolute_dif >= 0.05) {
        printf("theta = %g, absolute_dif = %g\n", theta, absolute_dif);
    }
#endif
    // Reassign theta to an in-range theta.
    float fixed_theta = angle_bound[theta_index];
    // But use the original t "as is".

    // Re-evaluate theta_in_interval & both_in_interval with new theta.
    both_in_interval[theta_index] = true;
    // Re-evaluate that theta/t is in a single interval now.
    int in_num_intervals = 0;
    for (int i=0; i<num_bounds-1; i++) {
        in_num_intervals += both_in_interval[i];
    }
    assert(in_num_intervals == 1);
    return fixed_theta;
}

double2 CubicInfo::computeReversingLineCubicFacetTPair(float t,
                                                       float theta,
                                                       float max_turning_angle_delta,
                                                       int step_mode)
{
    bool both_in_interval[2];
    for (int i=0; i<icountof(both_in_interval); i++) {
        const bool theta_in_interval = isWithinOrientedRange(theta, angle_bound[i], angle_bound[i+1], float(sign(curvature[i])));

        const bool within_left_side  = t_bound[i] <= t;
        const bool within_right_side = t <= t_bound[i+1];
        const bool t_in_interval = within_left_side && within_right_side;

        both_in_interval[i] = t_in_interval && theta_in_interval;
    }

    const int in_num_intervals = both_in_interval[0] + both_in_interval[1];

    // Are theta & t found together in no intervals?
    if (in_num_intervals < 1) {
        // Compute a new theta based on the inflection angles.
        theta = fixOutOfBoundsTheta(theta, t, 3, both_in_interval);
    } else {
        if (in_num_intervals > 1) {
            printf("in_num_intervals = %d\n", in_num_intervals);
        }
    }

    double t_neg = t_bound[0];
    double t_pos = t_bound[2];
    for (int i=0; i<icountof(both_in_interval); i++) {
        if (both_in_interval[i]) {
            const int max_interval = 2;  // for serpentine
            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 tenative_t_neg = fromBiasedAngleFindReversingLineT(theta, i, t, -1, step_neg);
            if (tenative_t_neg <= t) {
                if (tenative_t_neg > t_neg) {
                    t_neg = tenative_t_neg;
                }
            }
            double tenative_t_pos = fromBiasedAngleFindReversingLineT(theta, i, t, +1, step_pos);
            if (tenative_t_pos >= t) {
                if (tenative_t_pos < t_pos) {
                    t_pos = tenative_t_pos;
                }
            }
        }
    }
    return double2(t_neg, t_pos);
}

double2 CubicInfo::computeSerpentineCubicFacetTPair(float t,
                                                    float theta,
                                                    float max_turning_angle_delta,
                                                    int step_mode)
{
    bool both_in_interval[3];
    for (int i=0; i<icountof(both_in_interval); i++) {
        const bool theta_in_interval = isWithinOrientedRange(theta, angle_bound[i], angle_bound[i+1], float(sign(curvature[i])));

        const bool within_left_side  = t_bound[i] <= t;
        const bool within_right_side = t <= t_bound[i+1];
        const bool t_in_interval = within_left_side && within_right_side;

        both_in_interval[i] = t_in_interval && theta_in_interval;
    }

    const int in_num_intervals = both_in_interval[0] + both_in_interval[1] + both_in_interval[2];

    // Are theta & t found together in no intervals?
    if (in_num_intervals < 1) {
        // Compute a new theta based on the inflection angles.
        theta = fixOutOfBoundsTheta(theta, t, 4, both_in_interval);
    } else {
        if (in_num_intervals > 1) {
            printf("in_num_intervals = %d\n", in_num_intervals);
        }
    }

    // Initial bounds should be infinite.
    double t_neg = t_bound[0];
    double t_pos = t_bound[3];
    for (int i=0; i<icountof(both_in_interval); i++) {
        if (both_in_interval[i]) {
            const int max_interval = 2;  // for serpentine
            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 tenative_t_neg = fromBiasedAngleFindSerpentineT(theta, i, t, -1, step_neg);
            if (tenative_t_neg <= t) {
                if (tenative_t_neg > t_neg) {
                    t_neg = tenative_t_neg;
                }
            }
            double tenative_t_pos = fromBiasedAngleFindSerpentineT(theta, i, t, +1, step_pos);
            if (tenative_t_pos >= t) {
                if (tenative_t_pos < t_pos) {
                    t_pos = tenative_t_pos;
                }
            }
        }
    }
    return double2(t_neg, t_pos);
}

// returns angle
float CubicInfo::biasCubicFacetAngleLoop(float t,
                                          float t_direction,
                                          float theta,
                                          float bias)
{
    assert(t_direction == -1 || t_direction == +1);
    const float curvature_direction = float(sign(curvature[0]));
    const float oriented_bias = bias*t_direction*curvature_direction;
    const float tentative_theta = theta + oriented_bias;

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

    if (tentative_theta_crosses_limit_angle) {
        const float new_theta = limit_angle;
        return new_theta;
    } else {
        const float new_theta = tentative_theta;
        // Leave interval unchanged.
        return new_theta;
    }
}

double CubicInfo::fromBiasedAngleFindLoopT(float theta,
                                           float t, float t_direction, float bias)
{
    // Is the curve essentially flat here?
    const double epsilon_from_flat_curvature = 0.0000001;
    if (fabs(curvature[0]) <= epsilon_from_flat_curvature) {
        // Yes, just return t;
        return t;
    }
    const float biased_angle = biasCubicFacetAngleLoop(t, t_direction, theta, bias);
    const float biased_t = findCorrectLoopT(biased_angle, t, t_direction);
    return biased_t;
}

double2 CubicInfo::computeLoopCubicFacetTPair(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 = fromBiasedAngleFindLoopT(
        theta, t, -1, step_neg);
    double t_pos = fromBiasedAngleFindLoopT(
        theta, t, +1, step_pos);
    return double2(t_neg, t_pos);
}

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

    double2 t_pair;
    switch (kind) {
    default:
        assert(!"unknown kind");
    case LOOP:
        t_pair = computeLoopCubicFacetTPair(t, theta, max_turning_angle_delta, step_mode);
        break;
    case SERPENTINE:
        t_pair = computeSerpentineCubicFacetTPair(t, theta, max_turning_angle_delta, step_mode);
        break;
    case REVERSING_LINE:
        t_pair = computeReversingLineCubicFacetTPair(t, theta, max_turning_angle_delta, step_mode);
        break;
    }

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

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

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

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

    if (sqdist<double,double2>(gradient_pos)>0 && dtheta_pos == dtheta_pos) {
        if (!(1.55*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.55*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 = evalRationalBezierCubic(cp, t_neg);
    const double3 P0 = evalRationalBezierCubic(cp, double(t));
    const double3 Pp = evalRationalBezierCubic(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 = limit_angle;
    }
    if (Cg::isnan(D0n_rad)) {
        D0n_rad = limit_angle;
    }

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