
// graph.cpp - implementation of drawing parametric graph of curve for "hairline" stroking algorithm testbed

#include <assert.h>
#include <stdio.h>
#include <math.h>
#include <GL/glew.h>
#ifdef __APPLE__
# include <GLUT/glut.h>
#else
# include <GL/glut.h>
# ifdef _WIN32
#  include <windows.h>  // for wglGetProcAddress
# else
#  include <GL/glx.h>  // for glXGetProcAddress
# endif
#endif

#include <Cg/double.hpp>
#include <Cg/vector/xyzw.hpp>
#include <Cg/vector.hpp>
#include <Cg/degrees.hpp>
#include <Cg/dot.hpp>
#include <Cg/inverse.hpp>
#include <Cg/mul.hpp>
#include <Cg/radians.hpp>
#include <Cg/isnan.hpp>
#include <Cg/determinant.hpp>

#include <vector>
#include <limits>

#include "cg4cpp_xform.hpp"

#include "gl_framebuffer.hpp"

#include "request_vsync.h"

#include "countof.h"
#include "hairline.hpp"
#include "polar/global_polar_settings.hpp"
#include "simple_path.hpp"
#include "mouse.hpp"
#include "keyinput.hpp"
#include "hodograph.hpp"
#include "cubic_solve.hpp"
#include "glcolor.hpp"

#include "polar/QuadraticInfo.hpp"
#include "polar/CubicInfo.hpp"

#include "polar/cubicInflectionPoint.hpp"  // for cubicInflectionPoint
#include "polar/IndexedStrokedPath.hpp"  // for StrokeLink

#include "cmdline.hpp"

GLFramebuffer *graph_fb;

static int graph_window = -1;
int step_mode = 1;
bool clamp_mode = false;

void hideParametricGraphWindow()
{
    assert(graph_window >= 0);
    glutSetWindow(graph_window);
    delete graph_fb;
    graph_fb = NULL;
    glutDestroyWindow(graph_window);
    graph_window = -1;
}

void postGraphWindowRedisplay()
{
    if (graph_window >= 0) {
        glutPostWindowRedisplay(graph_window);
    }
}

enum ParametricGraphMenuEntry {
    TOGGLE_HALF_SCALE_RATIO_CURVE,
    TOGGLE_RESCALED_CURVE,
    TOGGLE_CLAMP_MODE,
    CYCLE_STEP_MODE,
    TOGGLE_CURVATURE,
    TOGGLE_CHANGE_IN_CURVATURE,
    HALF_SAMPLES,
    DOUBLE_SAMPLES,
    HIDE_GRAPH_WINDOW,
    DECREASE_LINE_WIDTH,
    INCREASE_LINE_WIDTH,
};

bool showHalfScaleRatioCurve = false;
bool showRescaledCurve = false;
int numParametricGraphSamples = 1024;
bool drawCurvature = true;
bool drawChangeInCurvature = false;

float graph_line_width = 1.0;

static void parametricGraphMenu(int item)
{
    switch (item) {
    case TOGGLE_HALF_SCALE_RATIO_CURVE:
        showHalfScaleRatioCurve = !showHalfScaleRatioCurve;
        break;
    case TOGGLE_RESCALED_CURVE:
        showRescaledCurve = !showRescaledCurve;
        break;
    case TOGGLE_CLAMP_MODE:
        clamp_mode = !clamp_mode;
        printf("clamp_mode = %d\n", clamp_mode);
        break;
    case CYCLE_STEP_MODE:
        step_mode = !step_mode;
        printf("step_mode = %d\n", step_mode);
        break;
    case TOGGLE_CURVATURE:
        drawCurvature = !drawCurvature;
        printf("drawCurvature = %d\n", drawCurvature);
        break;
    case TOGGLE_CHANGE_IN_CURVATURE:
        drawChangeInCurvature = !drawChangeInCurvature;
        printf("drawChangeInCurvature = %d\n", drawChangeInCurvature);
        break;
    case DOUBLE_SAMPLES:
        numParametricGraphSamples *= 2;
        printf("numParametricGraphSamples = %d\n", numParametricGraphSamples);
        break;
    case HALF_SAMPLES:
        numParametricGraphSamples /= 2;
        printf("numParametricGraphSamples = %d\n", numParametricGraphSamples);
        break;
    case HIDE_GRAPH_WINDOW:
        hideParametricGraphWindow();
        break;
    case DECREASE_LINE_WIDTH:
        if (graph_line_width >= 1.0f) {
            graph_line_width -= 0.5;
        }
        printf("graph_line_width = %g\n", graph_line_width);
        break;
    case INCREASE_LINE_WIDTH:
        graph_line_width += 0.5;
        printf("graph_line_width = %g\n", graph_line_width);
        break;
    default:
        assert(!"bad graph menu item");
    }
    postGraphWindowRedisplay();
}

void makeParametricGraphMenu()
{
    int menu_id = glutCreateMenu(parametricGraphMenu);
    glutAddMenuEntry("Double samples", DOUBLE_SAMPLES);
    glutAddMenuEntry("Half samples", HALF_SAMPLES);
    glutAddMenuEntry("Increase line width", INCREASE_LINE_WIDTH);
    glutAddMenuEntry("Decrease line width", DECREASE_LINE_WIDTH);
    glutAddMenuEntry("Toggle half-scale ratio curve (purple)", TOGGLE_HALF_SCALE_RATIO_CURVE);
    glutAddMenuEntry("Toggle rescaled ratio curve (white)", TOGGLE_RESCALED_CURVE);
    glutAddMenuEntry("[c] Toggle clamp mode", TOGGLE_CLAMP_MODE);
    glutAddMenuEntry("[q] Cycle step mode", CYCLE_STEP_MODE);
    glutAddMenuEntry("Toggle curvature (cyan)", TOGGLE_CURVATURE);
    glutAddMenuEntry("Toggle change in curvature (blueish)", TOGGLE_CHANGE_IN_CURVATURE);
    glutAddMenuEntry("Hide window", HIDE_GRAPH_WINDOW);
    glutAttachMenu(GLUT_RIGHT_BUTTON);
}

struct AngularBounds {
    int winding;
    int num_intervals;
    const float* bound;
    const float* t;

    AngularBounds(int w, int n, const float* b)
        : winding(w)
        , num_intervals(n)
        , bound(b)
        , t(NULL)
    {}
    AngularBounds(int w, int n, const float* b, const float* t_)
        : winding(w)
        , num_intervals(n)
        , bound(b)
        , t(t_)
    {}

    void print(const char *name) {
        printf("%s: winding=%+d, num_intervals=%d\n", name, winding, num_intervals);
        for (int i=0; i<num_intervals; i++) {
            float bound_deg = degrees(bound[i]);
            printf("  %d: bound=%g (rad=%g), t=%g\n", i, bound_deg, bound[i], t[i]);
        }
    }

    float find(int interval, float t0, float t1) {
        float interval_avg = (t[interval] + t[interval+1])/2;
        if (t[interval] <= t0 && t0 <= t[interval+1]) {
            return t0;
        } else if (t[interval] <= t1 && t1 <= t[interval+1]) {
            return t1;
        } else {
            float dif1 = fabs(t[interval] - t0);
            float dif2 = fabs(t[interval] - t1);
            float dif3 = fabs(t[interval+1] - t0);
            float dif4 = fabs(t[interval+1] - t1);

            float dif_min = min(min(dif1,dif2),min(dif3,dif4));

            if (dif_min == dif1 || dif_min == dif3) {
                return t[0];
            } else {
                return t[1];
            }
        }
    }
};

struct BoundedAngleDelta {
    int interval;
    float theta;
    float delta;

    BoundedAngleDelta(int i, float t, float d)
        : interval(i)
        , theta(t)
        , delta(d)
    {}
    BoundedAngleDelta(const AngularBounds& ab, float t, float theta_, float d)
        : theta(theta_)
        , delta(d)
    {
        calculateInterval(ab, t);
    }

    void calculateInterval(const AngularBounds& ab, float t) {
        for (int i=0; i<ab.num_intervals-1; i++) {
            if (ab.t[i] <= t && t <= ab.t[i+1]) {
                interval = i;
                return;
            }
        }
        assert(!"bad interval");
    }

    void print(const char* name) {
        float theta_deg = degrees(theta);
        float delta_deg = degrees(delta);
        printf("%s: interval=%d, theta=%f, delta=%f\n", name, interval, theta_deg, delta_deg);
    }
};

static const float PI = float(3.14159265358979323846);
static const float DOUBLE_PI = 2*PI;

static BoundedAngleDelta angleIntervalAdd(const AngularBounds ab, BoundedAngleDelta bad)
{
    assert(ab.num_intervals >= 2);

    float remainder = 0;
    int interval = bad.interval;

    // Out of bounds interval saturates to lower & upper bound angles.
    if (interval < 0) {
        return BoundedAngleDelta(0, ab.bound[0], 0);
    } else if (interval >= ab.num_intervals-1) {
        return BoundedAngleDelta(ab.num_intervals-2, ab.bound[ab.num_intervals-1], 0);
    }

    float start = ab.bound[bad.interval];
    float stop = ab.bound[bad.interval+1];
    int winding = ab.winding;
    int interval_bias = 1;

    if (interval & 1) {
        start = ab.bound[bad.interval+1];
        stop = ab.bound[bad.interval];
        interval_bias = -1;
    }

    if (winding == +1) {
        if (bad.delta > 0) {
            while (stop < bad.theta) {
                stop += DOUBLE_PI;
            }
            float new_theta = bad.theta + bad.delta;
            int interval = bad.interval;
            if (new_theta > stop) {
                remainder = stop - new_theta;
                new_theta = stop;
                interval += interval_bias;
                assert(remainder*bad.delta < 0);  // expect oppposite signs
            }
            while (new_theta >= DOUBLE_PI) {
                new_theta -= DOUBLE_PI;
            }
            return BoundedAngleDelta(interval, new_theta, remainder);
        } else {
            while (start > bad.theta) {
                start -= DOUBLE_PI;
            }
            float new_theta = bad.theta + bad.delta;
            int interval = bad.interval;
            if (new_theta < start) {
                remainder = start - new_theta;
                new_theta = start;
                interval -= interval_bias;
                assert(remainder*bad.delta < 0);  // expect oppposite signs
            }
            return BoundedAngleDelta(interval, new_theta, remainder);
        }
    } else {
        assert(winding == -1);
        if (bad.delta > 0) {
            while (stop > bad.theta) {
                stop -= DOUBLE_PI;
            }
            float new_theta = bad.theta - bad.delta;
            if (new_theta < stop) {
                remainder = new_theta - stop;
                new_theta = stop;
                interval += interval_bias;
                assert(remainder*bad.delta < 0);  // expect oppposite signs
            }
            while (new_theta >= DOUBLE_PI) {
                new_theta -= DOUBLE_PI;
            }
            return BoundedAngleDelta(interval, new_theta, remainder);
        } else {
            while (start < bad.theta) {
                start += DOUBLE_PI;
            }
            float new_theta = bad.theta - bad.delta;
            float remainder = 0;
            if (new_theta > start) {
                remainder = new_theta - start;
                new_theta = start;
                interval -= interval_bias;
                assert(remainder*bad.delta < 0);  // expect oppposite signs
            }
            return BoundedAngleDelta(interval, new_theta, remainder);
        }
    }
}

void test_angleIntervalAdd()
{
    float bound1[2] = { radians(30.0f), radians(30.0f) };

    AngularBounds ab1(1, countof(bound1), bound1);
    BoundedAngleDelta bad1(0, radians(40.0f), radians(1.0f));

    BoundedAngleDelta result1 = angleIntervalAdd(ab1, bad1);

    bad1.print("bad1");
    result1.print("result1");

    bad1.delta = radians(340.0f);
    bad1.print("bad1");
    BoundedAngleDelta result2 = angleIntervalAdd(ab1, bad1);
    result2.print("result2");

    float bound2[4] = { radians(30.0f), radians(80.0f), radians(10.0f), radians(30.0f) };
    AngularBounds ab2(1, countof(bound2), bound2);

    BoundedAngleDelta bad2(0, radians(40.0f), radians(50.0f));
    BoundedAngleDelta result3 = angleIntervalAdd(ab2, bad2);
    bad2.print("bad2");
    result3.print("result3");
    BoundedAngleDelta result4 = angleIntervalAdd(ab2, result3);
    result4.print("result4");

    BoundedAngleDelta bad3(0, radians(40.0f), radians(120.0f));
    BoundedAngleDelta result5 = angleIntervalAdd(ab2, bad3);
    bad3.print("bad3");
    result5.print("result5");
    BoundedAngleDelta result6 = angleIntervalAdd(ab2, result5);
    result6.print("result6");
    BoundedAngleDelta result7 = angleIntervalAdd(ab2, result6);
    result7.print("result7");
    result7.delta = -radians(50.0f);
    result7.print("result7");
    BoundedAngleDelta result8 = angleIntervalAdd(ab2, result7);
    result8.print("result8");
    result7.delta = -radians(105.0f);
    result7.print("result7");
    BoundedAngleDelta result9 = angleIntervalAdd(ab2, result7);
    result9.print("result9");
    result9 = angleIntervalAdd(ab2, result9);
    result9.print("result9");
    result9 = angleIntervalAdd(ab2, result9);
    result9.print("result9");

    result7.delta = -radians(135.0f);
    result7.print("result7");
    result9 = angleIntervalAdd(ab2, result7);
    result9.print("result9");
    result9 = angleIntervalAdd(ab2, result9);
    result9.print("result9");
    result9 = angleIntervalAdd(ab2, result9);
    result9.print("result9");
    result9 = angleIntervalAdd(ab2, result9);
    result9.print("result9");

    float bound3[2] = { radians(130.0f), radians(130.0f) };

    AngularBounds ab3(-1, countof(bound1), bound3);
    BoundedAngleDelta bad10(0, radians(40.0f), radians(1.0f));
    bad10.print("bad10");
    result9 = angleIntervalAdd(ab3, bad10);
    result9.print("result9");
    bad10.delta = radians(-1.0f);
    result9 = angleIntervalAdd(ab3, bad10);
    result9.print("result9");

    bad2.delta = radians(210.0f);
    bad2.print("bad2");
    result9 = angleIntervalAdd(ab2, bad2);
    result9.print("result9");
    result9 = angleIntervalAdd(ab2, result9);
    result9.print("result9");
    result9 = angleIntervalAdd(ab2, result9);
    result9.print("result9");
    result9 = angleIntervalAdd(ab2, result9);
    result9.print("result9");
    result9 = angleIntervalAdd(ab2, result9);
    result9.print("result9");

    float bound4[4] = { radians(-40.0f), radians(160.0f), radians(-150.0f), radians(-40.0f) };
    AngularBounds ab4(-1, countof(bound2), bound4);

    BoundedAngleDelta bad11(1, radians(-170.0f), radians(2.0f));
    bad11.print("bad11");
    result9 = angleIntervalAdd(ab4, bad11);
    result9.print("result9");
    result9.delta = radians(40.0f);
    result9.print("result9");
    result9 = angleIntervalAdd(ab4, result9);
    result9.print("result9");
    result9 = angleIntervalAdd(ab4, result9);
    result9.print("result9");
}

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

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

    const FLOAT curvature_numer = gradient.x*gradient2.y - gradient.y*gradient2.x;
    const FLOAT curvature_denom = pow(sqdist<FLOAT,FLOAT2>(gradient), FLOAT(1.5));
    const FLOAT curvature = curvature_numer/curvature_denom;
    return curvature;
}

template <typename FLOAT, typename FLOAT2>
FLOAT computeCurvature(float t, const FLOAT2& A, const FLOAT2& B, const FLOAT2& C)
{
    // Gradient coefficients A, B, and C work for 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;
    const FLOAT curvature_denom = pow(sqdist<FLOAT,FLOAT2>(gradient), FLOAT(1.5));
    const FLOAT curvature = curvature_numer/curvature_denom;
    return curvature;
}

template <typename FLOAT, typename FLOAT2>
FLOAT computeChangeInCurvature(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;
    const FLOAT sqdist_gradient = sqdist<FLOAT,FLOAT2>(gradient);
    const FLOAT curvature_denom = pow(sqdist_gradient, FLOAT(1.5));

    const FLOAT p0 = determinant(float2x2(gradient,A));
    const FLOAT p1 = dot(gradient,gradient2);
    const FLOAT p2 = pow(sqdist_gradient, FLOAT(2.5));

    const FLOAT curvature_change = 2*p0/curvature_denom - 3 * curvature_numer*p1/p2;
    return curvature_change;
}

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

    const FLOAT curvature_numer = gradient.x*gradient2.y - gradient.y*gradient2.x;
    const FLOAT sqdist_gradient = sqdist<FLOAT,FLOAT2>(gradient);
    const FLOAT curvature_denom = pow(sqdist_gradient, FLOAT(2.5));

    const FLOAT p0 = determinant(float2x2(gradient,gradient2));
    const FLOAT p1 = dot(gradient,gradient2);
    const FLOAT p2 = pow(sqdist_gradient, FLOAT(2.5));

    const FLOAT curvature_change = - 3 * curvature_numer*p1/p2;
    return curvature_change;
}

template <typename FLOAT, typename FLOAT2, typename FLOAT3>
static bool computeAngleStepAtInflectionT(const FLOAT2& A,
                                          const FLOAT2& B,
                                          const FLOAT2& C,
                                          FLOAT t,
                                          float angle_step,
                                          float2& ts)  // in radians
{
    const FLOAT2 gradient = (A*t+B)*t+C;
    const float theta = atan2(gradient.y, gradient.x);
    const float deg = degrees(theta);

    const float thetan = theta - angle_step;
    const float thetap = theta + angle_step;

    const FLOAT3 abcn = get_abc<FLOAT,FLOAT2,FLOAT3>(thetan, A, B, C);
    const FLOAT3 abc0 = get_abc<FLOAT,FLOAT2,FLOAT3>(theta,  A, B, C);
    const FLOAT3 abcp = get_abc<FLOAT,FLOAT2,FLOAT3>(thetap, A, B, C);

    const FLOAT discr_n = abcn.y*abcn.y - 4*abcn.x*abcn.z;
    const FLOAT eff_discr_n = max(discr_n, 0.0);
    const FLOAT discr_p = abcp.y*abcp.y - 4*abcp.x*abcp.z;
    const FLOAT eff_discr_p = max(discr_p, 0.0);

    // Use solutions with bigger discriminant
    if (eff_discr_n >= 0 && eff_discr_n>eff_discr_p) {
        const FLOAT t0n = (-abcn.y - sqrt(eff_discr_n))/(2*abcn.x);
        const FLOAT t1n = (-abcn.y + sqrt(eff_discr_n))/(2*abcn.x);
        if (t0n < t1n) {
            ts = float2(t0n,t1n);
        } else {
            ts = float2(t1n,t0n);
        }
        return true;
    }
    if (eff_discr_p >= 0 && eff_discr_p>eff_discr_n) {
        const FLOAT t0p = (-abcp.y - sqrt(eff_discr_p))/(2*abcp.x);
        const FLOAT t1p = (-abcp.y + sqrt(eff_discr_p))/(2*abcp.x);
        if (t0p < t1p) {
            ts = float2(t0p,t1p);
        } else {
            ts = float2(t1p,t0p);
        }
        return true;
    }
    return false;
}

template <typename FLOAT2>
static void computeMonomialCubicCoeffs(const float3 cps[4],
                                       FLOAT2& A,
                                       FLOAT2& B,
                                       FLOAT2& C)
{
    A = FLOAT2(9*FLOAT2(cps[1].xy)-3*FLOAT2(cps[0].xy)-9*FLOAT2(cps[2].xy)+3*FLOAT2(cps[3].xy));
    B = FLOAT2(-12*FLOAT2(cps[1].xy)+6*FLOAT2(cps[0].xy)+6*FLOAT2(cps[2].xy));
    C = FLOAT2(-3*FLOAT2(cps[0].xy)+3*FLOAT2(cps[1].xy));
}

double drawCubicRatioCurve(const int num_samples, const float3 cps[4], const float max_turning_angle_delta, const float scale)
{
    CubicInfo info(cps);

    double max_facetAngle = 0;
    double prior_ratio = 0;
    glBegin(GL_LINE_STRIP); {
        for (int i=0; i<=num_samples; i++) {
            float t = i/float(num_samples);

            if (!clamp_mode) {
                t = t*1.4 - 0.2;
            }

            const float facetAngle = info.computeFacetAngle(t, scale*max_turning_angle_delta, step_mode);
            const float ratio = facetAngle/max_turning_angle_delta;
            if (0.0 <= t && t <= 1.0) {
                if (facetAngle > max_facetAngle) {
                    max_facetAngle = facetAngle;
                }
            }
            glVertex2f(t, ratio);
            prior_ratio = ratio;
        }
    } glEnd();
    return max_facetAngle;
}

double drawQuadraticRatioCurve(const int num_samples, const float3 cps[3], const float max_turning_angle_delta, const float scale)
{
    QuadraticInfo info(cps);

    double max_facetAngle = 0;
    double prior_ratio = 0;
    glBegin(GL_LINE_STRIP); {
        for (int i=0; i<=num_samples; i++) {
            float t = i/float(num_samples);

            if (!clamp_mode) {
                t = t*1.4 - 0.2;
            }

            const float facetAngle = info.computeFacetAngle(t, scale*max_turning_angle_delta, step_mode);
            const float ratio = facetAngle/max_turning_angle_delta;
            if (0.0 <= t && t <= 1.0) {
                if (facetAngle > max_facetAngle) {
                    max_facetAngle = facetAngle;
                }
            }
            glVertex2f(t, ratio);
            prior_ratio = ratio;
        }
    } glEnd();
    return max_facetAngle;
}


void setColor(const GLfloat color[3])
{
    if (isBackgroundWhite()) {
        const float darken_factor = 0.4f;
        const GLfloat darker_color[3] = {
            darken_factor * color[0],
            darken_factor * color[1],
            darken_factor * color[2]
        };
        sRGB_glColor3fv(darker_color);
    } else {
        sRGB_glColor3fv(color);
    }
}

#define sRGB_glColor3fv  @@ use setColor

void drawInflectionLines(const int size, const double key_point[],
                         const float3 cps[4], const float max_turning_angle_delta)
{
    double2 A;
    double2 B;
    double2 C;
    computeMonomialCubicCoeffs(cps, A, B, C);

    glEnable(GL_LINE_STIPPLE);
    for (int i=0; i<size; i++) {
        const double t = key_point[i];
        float2 t_neg_pos;

        glLineStipple(3, 0x5555);
        glBegin(GL_LINES); {
            const GLfloat *foreground = isBackgroundWhite() ? black : white;
            setColor(foreground);
            glVertex2f(float(t),1.2);
            glVertex2f(float(t),-0.2);

            bool got = computeAngleStepAtInflectionT<double,double2,double3>(A, B, C, t, max_turning_angle_delta, t_neg_pos);
            if (got) {
                setColor(red);
                float t_neg = t_neg_pos[0];
                glVertex2f(t_neg,1.2);
                glVertex2f(t_neg,-0.2);
                setColor(green);
                float t_pos = t_neg_pos[1];
                glVertex2f(t_pos,1.2);
                glVertex2f(t_pos,-0.2);
            }
        } glEnd();

        // Double steps by max_turning_angle_delta
        bool got2 = computeAngleStepAtInflectionT<double,double2,double3>(A, B, C, t, 2*max_turning_angle_delta, t_neg_pos);
        if (got2) {
            glLineStipple(3, 0x3333);
            glBegin(GL_LINES); {
                setColor(dark_red);
                float t_neg = t_neg_pos[0];
                glVertex2f(t_neg,1.2);
                glVertex2f(t_neg,-0.2);
                setColor(dark_green);
                float t_pos = t_neg_pos[1];
                glVertex2f(t_pos,1.2);
                glVertex2f(t_pos,-0.2);
            } glEnd();
        }

        // Half steps by max_turning_angle_delta (orange)
        bool got3 = computeAngleStepAtInflectionT<double,double2,double3>(A, B, C, t, 0.5*max_turning_angle_delta, t_neg_pos);
        if (got3) {
            glLineStipple(3, 0x3333);
            glBegin(GL_LINES); {
                setColor(orange);
                float t_neg = t_neg_pos[0];
                glVertex2f(t_neg,1.2);
                glVertex2f(t_neg,-0.2);
                setColor(orange);
                float t_pos = t_neg_pos[1];
                glVertex2f(t_pos,1.2);
                glVertex2f(t_pos,-0.2);
            } glEnd();
        }
    }
    glDisable(GL_LINE_STIPPLE);
}

void drawCubicCurvature(const int num_samples, const float3 cps[4])
{
    float2 A = (9*cps[1].xy-3*cps[0].xy-9*cps[2].xy+3*cps[3].xy);
    float2 B = (-12*cps[1].xy+6*cps[0].xy+6*cps[2].xy);
    float2 C = (-3*cps[0].xy+3*cps[1].xy);

    glBegin(GL_LINE_STRIP); {
        for (int i=0; i<=num_samples; i++) {
            float t = i/float(num_samples);

            if (!clamp_mode) {
                t = t*1.4 - 0.2;
            }

            float curvature = computeCurvature<float,float2>(t, A, B, C);
            static float curvature_scale = 100;
            glVertex2f(t, curvature * curvature_scale);
        }
    } glEnd();
}

void drawQuadraticCurvature(const int num_samples, const float3 cps[3])
{
    float2 A = ( 2*cps[0].xy - 4*cps[1].xy + 2*cps[2].xy);
    float2 B = (-2*cps[0].xy + 2*cps[1].xy);

    glBegin(GL_LINE_STRIP); {
        for (int i=0; i<=num_samples; i++) {
            float t = i/float(num_samples);

            if (!clamp_mode) {
                t = t*1.4 - 0.2;
            }

            float curvature = computeCurvature<float,float2>(t, A, B);
            static float curvature_scale = 100;
            glVertex2f(t, curvature * curvature_scale);
        }
    } glEnd();
}

void drawChangeInCubicCurvature(const int num_samples, const float3 cps[4])
{
    float2 A = (9*cps[1].xy-3*cps[0].xy-9*cps[2].xy+3*cps[3].xy);
    float2 B = (-12*cps[1].xy+6*cps[0].xy+6*cps[2].xy);
    float2 C = (-3*cps[0].xy+3*cps[1].xy);

    glBegin(GL_LINE_STRIP); {
        for (int i=0; i<=num_samples; i++) {
            float t = i/float(num_samples);

            if (!clamp_mode) {
                t = t*1.4 - 0.2;
            }

            float curvature = computeChangeInCurvature<float,float2>(t, A, B, C);
            static float curvature_scale = 10;
            glVertex2f(t, curvature * curvature_scale);
        }
    } glEnd();
}

void drawChangeInQuadraticCurvature(const int num_samples, const float3 cps[3])
{
    float2 A = ( 2*cps[0].xy - 4*cps[1].xy + 2*cps[2].xy);
    float2 B = (-2*cps[0].xy + 2*cps[1].xy);

    glBegin(GL_LINE_STRIP); {
        for (int i=0; i<=num_samples; i++) {
            float t = i/float(num_samples);

            if (!clamp_mode) {
                t = t*1.4 - 0.2;
            }

            float curvature = computeChangeInCurvature<float,float2>(t, A, B);
            static float curvature_scale = 10;
            glVertex2f(t, curvature * curvature_scale);
        }
    } glEnd();
}

static void drawGridLines()
{
    glBegin(GL_LINES); {
        const GLfloat *foreground = isBackgroundWhite() ? black : white;
        setColor(foreground);
        glVertex2f(-4,0);
        glVertex2f(4,0);
        setColor(green);
        glVertex2f(-4,1);
        glVertex2f(4,1);

        // Red horizontal lines mark where ratio is greater than 1.
        setColor(red);
        glVertex2f(-4,2);
        glVertex2f(4,2);

        setColor(half_red);
        glVertex2f(-4,1.5);
        glVertex2f(4,1.5);

        setColor(quarter_red);
        glVertex2f(-4,1.25);
        glVertex2f(4,1.25);

        // vertical lines for t=0 & t=1
        setColor(blue);
        glVertex2f(0.0,1.2);
        glVertex2f(0.0,-0.2);

        glVertex2f(1.0,1.2);
        glVertex2f(1.0,-0.2);

        // Page Down toggles draw_ref_t
        if (draw_ref_t) {
            // vertical lines for t=ref_t
            setColor(windows_blue);
            glVertex2f(ref_t,1+200);
            glVertex2f(ref_t,0-200);
        }
    } glEnd();
}

void drawParametricGraph()
{
    const float max_turning_angle_delta = stroke_tessellator_settings.MaxTurningAngleDelta();
    drawGridLines();

    // Is there at least one link?
    if (simple.indexed_path_state.numLinks() > 0) {
        // Yes, draw hodograph for first link.
        const StrokeLink& link = simple.indexed_path_state.ClampedLink(current_link);

        // Collected "patch" worth of 6 indexed control points.
        float3 link_control_points[6];
        patchControlPoints(link_control_points, link);

        CPUStrokeTessellator stroker(stroke_tessellator_settings);
        stroker.setTrivialCulling(false);
        const unsigned int primitive_id = 0;
        bool cull = prepStroker(stroker, current_paint, link_control_points, link.ndx, primitive_id);
        if (cull) {
            printf("CPU culled hodograph\n");
            return;
        }

        if (stroker.LinkTypeIsQuadratic()) {
            float3 cps[3];
            stroker.GetQuadraticControlPoints(cps);

            const int num = numParametricGraphSamples;
            for (int i=0; i<=num; i++) {
                const float t = i/float(num);
                const float3 p3 = evalRationalBezierQuadraticGradient(cps, t);
                const float2 p = p3.xy/p3.z;
                const float rad = atan2(p.y, p.x);
                const float deg = degrees(rad);
            }

            setColor(yellow);
            const double max_facetAngle = drawQuadraticRatioCurve(numParametricGraphSamples, cps, max_turning_angle_delta, 1.0f);

            if (drawCurvature) {
                setColor(cyan);
                drawQuadraticCurvature(numParametricGraphSamples, cps);
            }
            if (drawChangeInCurvature) {
                setColor(windows_blue);
                drawChangeInQuadraticCurvature(numParametricGraphSamples, cps);
            }
        } else {
            float3 cps[4];
            stroker.GetCubicControlPoints(cps);

            int num_inflections = 0;
            double inflections[2];
            num_inflections = cubicInflectionPoint(cps, inflections);
            
            drawInflectionLines(num_inflections, inflections, cps, max_turning_angle_delta);

            if (showHalfScaleRatioCurve) {
                setColor(purple);
                drawCubicRatioCurve(numParametricGraphSamples, cps, max_turning_angle_delta, 0.5f);
            }

            setColor(yellow);
            const double max_facetAngle = drawCubicRatioCurve(numParametricGraphSamples, cps, max_turning_angle_delta, 1.0f);

            if (showRescaledCurve) {
                const GLfloat *foreground = isBackgroundWhite() ? black : white;
                setColor(foreground);
                float scale = max_turning_angle_delta/max_facetAngle;
                if (scale > 1) {
                    scale = 1;
                }
                drawCubicRatioCurve(numParametricGraphSamples, cps, max_turning_angle_delta, scale);
            }

            if (drawCurvature) {
                setColor(cyan);
                drawCubicCurvature(numParametricGraphSamples, cps);
            }
            if (drawChangeInCurvature) {
                setColor(windows_blue);
                drawChangeInCubicCurvature(numParametricGraphSamples, cps);
            }
        }
    }
}

MouseState ms_graph;

void graphConfigureProjection()
{
    float3x3 inverse_projection, viewport;

    viewport = ortho(0,ms_graph.window_width, 0,ms_graph.window_height);
    float left = -ms_graph.window_width, right = ms_graph.window_width, top = -ms_graph.window_height, bottom = ms_graph.window_height;
    left = -1;
    right = 1;
    top = 1;
    bottom = -1;
#if 1
    float aspect_ratio = float(ms_graph.window_height)/ms_graph.window_width;
    if (aspect_ratio > 1) {
        top *= aspect_ratio;
        bottom *= aspect_ratio;
    } else {
        left /= aspect_ratio;
        right /= aspect_ratio;
    }
#endif
    glMatrixLoadIdentityEXT(GL_PROJECTION);
    glMatrixOrthoEXT(GL_PROJECTION, left, right, bottom, top,
        -1, 1);
    ms_graph.projection = ortho(left, right, bottom, top);
    inverse_projection = inverse_ortho(left, right, bottom, top);
    ms_graph.view_width = right - left;
    ms_graph.view_height = bottom - top;
    ms_graph.win2view = mul(inverse_projection, viewport);
    ms_graph.view2win = inverse(ms_graph.win2view);
}

void graphReshape(int w, int h)
{
    glViewport(0,0,w,h);

    ms_graph.window_width = w;
    ms_graph.window_height = h;
    ms_graph.aspect_ratio = ms_graph.window_height/ms_graph.window_width;

    establishFramebuffer(graph_fb, w, h);

    graphConfigureProjection();
}

void graphDisplay()
{
    updateColorSpace(graph_fb);
    graph_fb->lineWidth(graph_line_width);
    graph_fb->bind(); {
        glClear(GL_COLOR_BUFFER_BIT | GL_STENCIL_BUFFER_BIT);
        MatrixLoadToGL(ms_graph.view);
        glMatrixPushEXT(GL_MODELVIEW); {
            glMatrixTranslatefEXT(GL_MODELVIEW, -3, -0.1, 0);
            glMatrixScalefEXT(GL_MODELVIEW, 6, 0.8, 1);
            drawParametricGraph();
        } glMatrixPopEXT(GL_MODELVIEW);
    } graph_fb->copy();
    glBindFramebuffer(GL_FRAMEBUFFER, 0);
    glutSwapBuffers();
}

void graphMouse(int button, int state, int mouse_space_x, int mouse_space_y)
{
    mouse_space_y = ms_graph.window_height - mouse_space_y;
    const int modifiers = glutGetModifiers() & (GLUT_ACTIVE_CTRL|GLUT_ACTIVE_SHIFT);

    if (button == GLUT_LEFT_BUTTON) {
        if (state == GLUT_DOWN) { 
            float2 win = float2(mouse_space_x, mouse_space_y);
            {
                ms_graph.anchor = project(mul(ms_graph.win2view, float3(win,1)));

                ms_graph.rotate_x = mouse_space_x;
                ms_graph.scale_y = mouse_space_y;
                // Reverse the normal zoom convention; use Ctrl to rotate
                if ((glutGetModifiers() & GLUT_ACTIVE_CTRL)) {
                    ms_graph.scaling = 1;
                } else {
                    ms_graph.scaling = 0;
                }
                if (!(glutGetModifiers() & GLUT_ACTIVE_SHIFT)) {
                    ms_graph.zooming = 1;
                } else {
                    ms_graph.zooming = 0;
                }
            }
        } else {
            assert(state == GLUT_UP);
            ms_graph.zooming = 0;
            ms_graph.scaling = 0;
        }
    }
    if (button == GLUT_MIDDLE_BUTTON) {
        if (state == GLUT_DOWN) {
            if (modifiers & GLUT_ACTIVE_CTRL) {
                ms_graph.slide_scale = 1.0/8.0;
            } else {
                ms_graph.slide_scale = 1.0;
            }
            ms_graph.slide_x = mouse_space_x;
            ms_graph.slide_y = mouse_space_y;
            if (modifiers & GLUT_ACTIVE_SHIFT) {
                // If Shiftheld down, confine translation to Y direction
            } else {
                ms_graph.x_sliding = true;
            }
            ms_graph.y_sliding = true;
        } else {
            assert(state == GLUT_UP);
            ms_graph.x_sliding = false;
            ms_graph.y_sliding = false;
        }
        if (verbose) {
            printf("sliding%s%s: %f,%f\n",
                ms_graph.x_sliding ? "X" : "",
                ms_graph.y_sliding ? "Y" : "",
                ms_graph.slide_x, ms_graph.slide_y);
        }
    }
}

void graphMotion(int mouse_space_x, int mouse_space_y)
{
    mouse_space_y = ms_graph.window_height - mouse_space_y;
    if (ms_graph.zooming || ms_graph.scaling) {
        float3x3 t, r, s, m;
        float angle = 0;
        float zoom = 1;
        if (ms_graph.scaling) {
            angle = 0.3 * (ms_graph.rotate_x - mouse_space_x) * canvas_width/ms_graph.window_width;
        }
        if (ms_graph.zooming) {
            zoom = pow(1.003f, (ms_graph.scale_y - mouse_space_y) * canvas_height/ms_graph.window_height);
        }

        t = translate(ms_graph.anchor);
        r = rotate(angle);
        s = scale(zoom, zoom);

        r = mul(r, s);
        m = mul(t, r);
        t = translate(-ms_graph.anchor);
        m = mul(m, t);
        ms_graph.view = mul(m, ms_graph.view);
        ms_graph.inverse_view = inverse(ms_graph.view);

        validateModelview();

        ms_graph.rotate_x = mouse_space_x;
        ms_graph.scale_y = mouse_space_y;
        postGraphWindowRedisplay();
    }
    if (ms_graph.x_sliding || ms_graph.y_sliding) {
        float x_offset = 0,
              y_offset = 0;

        float3x3 m;

        if (ms_graph.x_sliding) {
            x_offset = (mouse_space_x - ms_graph.slide_x) * ms_graph.view_width/ms_graph.window_width;
            x_offset *= ms_graph.slide_scale;
        }
        if (ms_graph.y_sliding) {
            y_offset = (ms_graph.slide_y - mouse_space_y) * ms_graph.view_height/ms_graph.window_height;
            y_offset *= ms_graph.slide_scale;
        }
        m = translate(x_offset, y_offset);
        ms_graph.view = mul(m, ms_graph.view);
        ms_graph.inverse_view = inverse(ms_graph.view);

        validateModelview();

        ms_graph.slide_y = mouse_space_y;
        ms_graph.slide_x = mouse_space_x;
        ms_graph.scale_y = mouse_space_y;
        postGraphWindowRedisplay();
    }
}

static void graph_keyboard(unsigned char c, int x, int y)
{
    switch (c) {
    case '+':
    case '=':
        advanceCurrentLink();
        return;
    case '-':
    case '_':
        reverseCurrentLink();
        return;
    case 'q':
        step_mode = (step_mode+1)%4;
        printf("step_mode = %d\n", step_mode-1);
        break;
    case 'c':
        clamp_mode = !clamp_mode;
        printf("clamp_mode = %d\n", clamp_mode);
        break;
    default:
        keyboard(c, x, y);
        return;
    }
    postGraphWindowRedisplay();
}

void toggleParametricGraphWindow()
{
    if (graph_window < 0){
        // Next window should appear to the right
        setMainWindow();
        glutInitWindowPosition(glutGet(GLUT_WINDOW_X), glutGet(GLUT_WINDOW_Y)+glutGet(GLUT_WINDOW_HEIGHT)+45);
        glutInitWindowSize(canvas_width, 200);
        graph_window = glutCreateWindow("Parametric graph for the Cubic Bezier.");
        glutDisplayFunc(graphDisplay);
        glutReshapeFunc(graphReshape);
        glutKeyboardFunc(graph_keyboard);
        glutSpecialFunc(special);
        glutMouseFunc(graphMouse);
        glutMotionFunc(graphMotion);
        makeParametricGraphMenu();
        requestSynchornizedSwapBuffers(frame_sync);
        glClearColor(0,0,0,0);
    } else{
        hideParametricGraphWindow();
    }
}

void initParametricGraphViewMatrices()
{
    ms_graph.view = float3x3(1,0,0,
        0,1,0,
        0,0,1);
    ms_graph.inverse_view = inverse(ms_graph.view);
}
