#include <vector>
#include <iostream>
#include <algorithm>
#include <cmath>
#include <cstdio>
#include <fstream>
#include <cstdlib>
#include <cassert>
#include <Eigen/Dense>
#include <mpfr.h>

#include "glframewindow.h"

#define MY_CIRCLE_LIST 1

//#define DEFAULT_DEGREE 4
//#define DEFAULT_CPS 10
#define DEFAULT_DEGREE_PARAM 1.5
#define DEFAULT_POINTS_PARAM 1.0
#define DEFAULT_CENTRIPETAL_PARAM 0.5

#define MAX_DEGREE 9
#define MIN_DEGREE 2
#define MAX_CPS 20

#define OBJ_LEAST_SQUARES 1
#define OBJ_LOG_SUM_EXP 2
#define OBJ_MANHATTAN 3
const char *OBJ_STRINGS[] = { NULL, "Least Squares", "LogSumExp", "Manhattan" };

#define PARAM_UNIFORM 1
#define PARAM_CHORD 2
#define PARAM_CENTRIPETAL 3
const char *PARAM_STRINGS[] = { NULL, "Uniform", "Chord Length", "Centripetal" };

#define MAX_ITERATIONS 256
#define APPROX_STEP_SIZE 1

#define MIN_SAMPLES 256
#define SAMPLE_RATE 32

#define MIN_SAMPLE_POINTS 8

#define DEFAULT_PRECISION 53
#define EPSILON 0.00000001
#define INVALID -69.0

#define CLOSED_CURVE_DISTANCE 20
#define DEFAULT_KNOWN_CURVE_PARAM 4
#define CUSP_THRESHOLD 0.25

#define WINDOW_HEIGHT 768
#define WINDOW_WIDTH 1024
#define WINDOW_TITLE "B-Spline Approximations"
#define SCROLL_SPEED 20

//define MPFR_EPSILON_DENOM 100000000000000.
mpfr_t MPFR_EPSILON;

// declaration of mpfr_t variables that will be used throughout
// placed here to avoid re-initialization
mpfr_t evalx, evaly, sqrd, sqrdr;
mpfr_t dx, dy;
mpfr_t denom, expsumx, expsumy, ldiff, rdiff, curx, cury;

int basisSaved = 0;
int evalSaved = 0;

class MyGLFrameWindow : public GLFrameWindow {
public:
  MyGLFrameWindow();

  void setupWidgets();

  // overrideables from GLFrameWindow
  virtual void OnGLDraw();
  virtual void OnGLInit();
  virtual int handleForGLArea(int event);

  void Reset() {
    data.clear();
    samples.clear();
    cusps.clear();
    curve.degree = 0;
  };

  void computeCurve(void);
  void recompute();

  void toggleShowControlPoints();
  void toggleShowSamplePoints();
  void toggleShowInputCurve();
  void toggleCusps();
  void setObjective(int obj);
  void setParamMethod(int method);
  void loadKnownCurves(std::istream &is);
  void printKnownCurves(const char *fname);
  void addKnownCurve();

private:
  struct point {
    point(): x(0), y(0) { }
    point(double x, double y) : x(x), y(y) { }
    double x, y;

    point operator*(double f) {
      point res(f*x, f*y);
      return res;
    }
    point operator+(const point& other) {
      point res(x+other.x, y+other.y);
      return res;
    }
    point operator-(const point& other) {
      point res(x-other.x, y-other.y);
      return res;
    }
    friend point operator*(double f, const point& rhs) {
      point res(rhs);
      return res * f;
    }
    friend double dot(point p1, point p2) {
      return p1.x*p2.x + p1.y*p2.y;
    }
    friend double norm(point p) {
      return sqrt(p.x*p.x + p.y*p.y);
    }
    friend double distance(point p1, point p2) {
      return sqrt(squareDistance(p1, p2));
    }
    friend double squareDistance(point p1, point p2) {
      return (p1.x-p2.x)*(p1.x-p2.x)+(p1.y-p2.y)*(p1.y-p2.y);
    }
    friend bool operator!=(point a, point b) {
      return a.x != b.x || a.y != b.y;
    }
  };
  struct spline {
    spline(): degree(0) { init(); }
    spline(int deg): degree(deg) { init(); }
    spline(int deg, std::vector<point> &points, std::vector<double> &knots): degree(deg), points(points), knots(knots) {
      init();
      getFakeKnots();
    }
    spline(int deg, std::vector<point> &points, std::vector<double> &knots, std::vector<double> &params): degree(deg), points(points), knots(knots), params(params) {
      init();
      getFakeKnots();
    }
    void init() {
      paramBasis = NULL;
      paramEval = NULL;
      paramEvalSet = NULL;
      paramEvalPoint = NULL;
    }
    ~spline() {
      if (paramBasis != NULL) {
        delete [] paramBasis;
      }
      if (paramEval != NULL) {
        for (int i = 0; i < params.size(); i++) {
          mpfr_clears(paramEval[2*i], paramEval[2*i+1], (mpfr_ptr)0);
        }
        delete [] paramEval;
        delete [] paramEvalSet;
      }
      if (paramEvalPoint != NULL) {
        delete [] paramEvalPoint;
        delete [] paramEvalSet;
      }
    }

    std::vector<point> points;
    std::vector<double> knots;
    std::vector<double> fknots;
    std::vector<double> params;
    int degree;

    std::vector<point> bpoints;
    std::vector<point> paramPoints;

    int paramBasisLen;
    double *paramBasis;
    point *paramEvalPoint;
    mpfr_t *paramEval;
    bool *paramEvalSet;

    double evaluateBasis(int m, int i, double t);
    double evaluateBasisParams(int i, int param);
    point evaluate(double u);
    point evaluateParams(int param);
    point evaluateParamsInternal(int param);
    void mpfr_evaluate(mpfr_t *points, double u, mpfr_t &x, mpfr_t &y);
    void mpfr_evaluateParams(mpfr_t *points, int param, mpfr_t &x, mpfr_t &y);
    void mpfr_evaluateParamsInternal(mpfr_t *points, int param, mpfr_t &x, mpfr_t &y);
    void clearParamEval();
    void computeCurve(int samples);
    void getFakeKnots();
    void reverse();

    void print(std::ostream &os);
    bool read(std::istream &is);
    double compare(std::vector<point> &data, std::vector<int> &samples, spline &sp);

    spline &operator=(spline &p2) {
      degree = p2.degree;
      points = p2.points;
      knots = p2.knots;
      fknots = p2.fknots;
      params = p2.params;
      bpoints = p2.bpoints;
      if (paramBasis != NULL) {
        delete [] paramBasis;
      }
      // steals the param arrays
      paramBasis = p2.paramBasis;
      p2.paramBasis = NULL;
      paramEval = p2.paramEval;
      p2.paramEval = NULL;
      paramEvalPoint = p2.paramEvalPoint;
      p2.paramEvalPoint = NULL;
      paramEvalSet = p2.paramEvalSet;
      p2.paramEvalSet = NULL;
    }
  };

  std::vector<point> data;
  std::vector<int> samples;
  std::vector<int> cusps;
  spline curve;

  double *costs[MAX_DEGREE+2];
  bool *tried[MAX_DEGREE+2];
  spline *solutions[MAX_DEGREE+2];

  bool showControlPoints;
  bool showSamplePoints;
  bool showInputCurve;

  double degreeParam;
  double pointsParam;
  double centripetalParam;
  double knownCurveParam;

  bool running;
  int objective;
  int paramMethod;
  bool detectCusps;
  int offsetx;
  int offsety;
  std::vector<spline> knownCurves;

  void printPoints(std::vector<point> &pts);

  void sampleData();
  void initializeCostMatrix(int max_cps);
  double costFactor(int deg, int cps);
  int minCPs(int deg);

  void logSumExp(int deg, int cps);
  void logSumExpCost(spline &sp, mpfr_t *points, mpfr_t &cost);
  void logSumExpGradient(spline &sp, mpfr_t *points, mpfr_t *gradient);

  void leastSquares(int deg, int cps);
  double leastSquaresCost(spline &sp);

  void manhattan(int deg, int cps);
  double manhattanCost(spline &sp);
  void manhattanGradient(spline &sp, std::vector<point> &gradient);

  void parameterize(spline &sp, int cps);
  void approxCurve();
  int compareKnownCurves(spline &sp);
};

MyGLFrameWindow *glwindow = NULL;

static int sgn(double a) {
  if (a == 0) return 0;
  return a < 0 ? -1 : 1;
}

static double abs(double a) {
  return a < 0 ? -a : a;
}

static void CounterCallback(Fl_Widget *o, void *v) {
  Fl_Counter *c = (Fl_Counter *)o;
  *((double*)v) = c->value();
  glwindow->recompute();
  glwindow->getGL().redraw();
}

MyGLFrameWindow::MyGLFrameWindow() : GLFrameWindow(WINDOW_WIDTH, WINDOW_HEIGHT, WINDOW_TITLE), showControlPoints(true),
    degreeParam(DEFAULT_DEGREE_PARAM), pointsParam(DEFAULT_POINTS_PARAM), objective(OBJ_LEAST_SQUARES), showSamplePoints(true),
    showInputCurve(true), offsetx(0), offsety(0), paramMethod(PARAM_UNIFORM), centripetalParam(DEFAULT_CENTRIPETAL_PARAM),
    running(false), detectCusps(false), knownCurveParam(DEFAULT_KNOWN_CURVE_PARAM) {
  for (int i = 0; i < MAX_DEGREE + 2; i++) {
    costs[i] = NULL;
    tried[i] = NULL;
    solutions[i] = NULL;
  }
  Reset();
}

void MyGLFrameWindow::setupWidgets() {
  getDegreeCounter().callback(CounterCallback, &degreeParam);
  getPointsCounter().callback(CounterCallback, &pointsParam);
  getCentripetalCounter().callback(CounterCallback, &centripetalParam);
  getKnownCurveCounter().callback(CounterCallback, &knownCurveParam);

  getDegreeCounter().value(DEFAULT_DEGREE_PARAM);
  getPointsCounter().value(DEFAULT_POINTS_PARAM);
  getCentripetalCounter().value(DEFAULT_CENTRIPETAL_PARAM);
  getKnownCurveCounter().value(DEFAULT_KNOWN_CURVE_PARAM);

  getDegreeCounter().step(1, 10);
  getPointsCounter().step(1, 10);
  getCentripetalCounter().step(0.01, 0.1);
  getKnownCurveCounter().step(1, 10);

  getDegreeCounter().bounds(0, 500);
  getPointsCounter().bounds(0, 500);
  getCentripetalCounter().bounds(0, 5);
  getKnownCurveCounter().bounds(0, 100);
}

void MyGLFrameWindow::recompute() {
  running = true;
  getGL().redraw();
  approxCurve();
  computeCurve();
  running = false;
}

// draw stuff using opengl
// the opengl context has been created, set rendering options
void MyGLFrameWindow::OnGLInit(){
  // Turn on Gouraud shading
  glShadeModel(GL_SMOOTH);

  // Create a white background when clear colour buffers
  glClearColor(1.0, 1.0, 1.0, 0.0);

  // Initialize the cube display list
  glNewList(MY_CIRCLE_LIST, GL_COMPILE);
  glBegin(GL_LINE_STRIP);
  for(GLint i = 0; i <= 15; i++){
    GLfloat cosine = 5*cos(i*2*M_PI/15.0);
    GLfloat sine = 5*sin(i*2*M_PI/15.0);
    glVertex2f(cosine,sine);
  }
  glEnd();
  glEndList();

  // change the projective matrix 
  glMatrixMode(GL_PROJECTION); 
  // orthographic projection 
  glLoadIdentity(); 

  // set the viewport 
  glViewport(0.0, 0.0, getGL().w(), getGL().h()); 

  glOrtho(0.0, getGL().w(), getGL().h(), 0.0, -0.1, 0.1); 

  // set modelview matix to identity 
  glMatrixMode( GL_MODELVIEW ); 
  glLoadIdentity();

  // setup font
  gl_font(FL_COURIER, 10);
};

void MyGLFrameWindow::OnGLDraw(){

  // apply the x, y offset
  glMatrixMode(GL_PROJECTION);
  glLoadIdentity();
  glViewport(0, 0, getGL().w(), getGL().h());
  glOrtho(offsetx, getGL().w() + offsetx, getGL().h() + offsety, offsety, -0.1, 0.1);

  // reset modelview
  glMatrixMode(GL_MODELVIEW);
  glLoadIdentity();

  // clear the window
  glClear( GL_COLOR_BUFFER_BIT );  

  // draw the line between sample points and parametrized points
  if (showInputCurve && curve.degree > 0) {
    glColor3f(0.8, 0.0, 0.0);
    for (int i = 0; i < samples.size(); i++) {
      glBegin(GL_LINE_STRIP);
        glVertex2i(data[samples[i]].x, data[samples[i]].y);
        glVertex2i(curve.paramPoints[i].x, curve.paramPoints[i].y);
      glEnd();
    }
  }

  // draw the data
  if (showInputCurve) {
    glColor3f( 0, 0, 0 );
    if (data.size() > 1) {
      glBegin(GL_LINE_STRIP);
      for (int i = 0; i < data.size(); i++) {
        glVertex2i(data[i].x, data[i].y);
      }
      glEnd();
    }
  }

  // draw the sampled points
  if (showSamplePoints) {
    glColor3f(0.49, 0.15, 0.8);
    if (samples.size() > 0) {
      for (int i = 0; i < samples.size(); i++) {
        glPushMatrix();
          glTranslatef(data[samples[i]].x, data[samples[i]].y, 0.0);
          glCallList(MY_CIRCLE_LIST);
        glPopMatrix();
      }
    }
  }

  // draw the control polygon if necessary
  if (showControlPoints && curve.degree > 0) {
    glColor3f(0.0, 0.0, 0.5);
    glBegin(GL_LINE_STRIP);
    for (int i = 0; i < curve.points.size(); i++) {
      glVertex2i(curve.points[i].x, curve.points[i].y);
    }
    glEnd();

    for (int i = 0; i < curve.points.size(); i++) {
      glPushMatrix();
        glTranslatef(curve.points[i].x, curve.points[i].y, 0.0);
        glCallList(MY_CIRCLE_LIST);
      glPopMatrix();
    }
  }

  // draw the spline
  if (curve.degree > 0) {
    glColor3f(0.0, 0.9, 0.0);
    glBegin(GL_LINE_STRIP);
    for (int i = 0; i < curve.bpoints.size(); i++) {
      glVertex2i(curve.bpoints[i].x, curve.bpoints[i].y);
    }
    glEnd();
  }

  // display optimization options
  glColor3f(0.0, 0.0, 0.0);
  char str[100] = { 0 };
  sprintf(str, "objective: %s", OBJ_STRINGS[objective]);
  gl_draw(str, offsetx + 10, offsety + 10);
  sprintf(str, "params: %s", PARAM_STRINGS[paramMethod]);
  gl_draw(str, offsetx + 10, offsety + 22);
  sprintf(str, "known curves: %zd", knownCurves.size());
  gl_draw(str, offsetx + 10, offsety + 34);

  // display current degree and cps
  if (curve.degree > 0) {
    sprintf(str, "degree: %d", curve.degree);
    gl_draw(str, offsetx + 10, offsety + 46);
    sprintf(str, "cps: %zd", curve.points.size());
    gl_draw(str, offsetx + 10, offsety + 58);
  }
  if (running) {
    gl_draw("working...", offsetx + 10, offsety + 70);
  }

  // flush the buffer 
  glFlush();
};

// handle events sent to this window, or it's children
// NOTE: return 1 if you process an event, zero otherwise.
int MyGLFrameWindow::handleForGLArea(int event){
  point ev(Fl::event_x(), Fl::event_y());
  switch (event) {
  case FL_FOCUS:
    return 1;
  case FL_UNFOCUS:
    return 1;
  case FL_KEYDOWN:
    switch (Fl::event_key()) {
    case FL_Up:
      offsety -= SCROLL_SPEED;
      break;
    case FL_Down:
      offsety += SCROLL_SPEED;
      break;
    case FL_Left:
      offsetx -= SCROLL_SPEED;
      break;
    case FL_Right:
      offsetx += SCROLL_SPEED;
      break;
    }
    getGL().redraw();
    return 1;
  case FL_PUSH:
    if (Fl::event_state() & FL_BUTTON1) {
      // add a point to the line
      Reset();
      getGL().redraw();
    }
    return 1;
  case FL_DRAG:
    printf("Drag event.\n");
    if (Fl::event_state() & FL_BUTTON1) {
      data.push_back(point(Fl::event_x() + offsetx, Fl::event_y() + offsety));
      if (detectCusps && data.size() > 2) {
        int i = data.size() - 2;
        point v1 = data[i] - data[i-1];
        point v2 = data[i+1] - data[i];
        double ctheta = dot(v1, v2) / (norm(v1) * norm(v2));
        if (ctheta < CUSP_THRESHOLD) {
          cusps.push_back(i);
          printf("Cusp detected at (%f, %f)\n", data[i].x, data[i].y);
        }
      }
      getGL().redraw();
    }
    return 1;
  case FL_RELEASE:
    if (Fl::event_button() == FL_LEFT_MOUSE) {
      // check for closed curve
      if (data.size() > 1 && squareDistance(data[0], data[data.size()-1]) <= CLOSED_CURVE_DISTANCE * CLOSED_CURVE_DISTANCE) {
        printf("Closed curve detected.\n");
        data[data.size()-1] = data[0];
      }
      recompute();
      getGL().redraw();
    }
    return 1;
  }
  return 0;
};

void MyGLFrameWindow::toggleShowControlPoints() {
  showControlPoints = !showControlPoints;
  getGL().redraw();
}

void MyGLFrameWindow::toggleShowSamplePoints() {
  showSamplePoints = !showSamplePoints;
  getGL().redraw();
}

void MyGLFrameWindow::toggleShowInputCurve() {
  showInputCurve = !showInputCurve;
  getGL().redraw();
}

void MyGLFrameWindow::toggleCusps() {
  detectCusps = !detectCusps;
  recompute();
}

void MyGLFrameWindow::setObjective(int obj) {
  objective = obj;
  recompute();
  getGL().redraw();
}

void MyGLFrameWindow::setParamMethod(int method) {
  paramMethod = method;
  recompute();
  getGL().redraw();
}

static void Save(Fl_Widget *W, void* Data) {
  glwindow->SaveToFile("screenshot.png");
}

static void AddKnownCurve(Fl_Widget *W, void *Data) {
  glwindow->addKnownCurve();
}

static void SaveKnownCurves(Fl_Widget *W, void *Data) {
  glwindow->printKnownCurves("known_curves_out");
}

static void Reset(Fl_Widget *W, void* Data) {
  glwindow->Reset();
  glwindow->getGL().redraw();
}

static void Quit(Fl_Widget *W, void* Data) {
  if(glwindow != NULL){
    delete glwindow;
    glwindow = NULL;
  }
}

static void toggleShowControlPoints(Fl_Widget* W, void* Data) {
  glwindow->toggleShowControlPoints();
}

static void toggleShowSamplePoints(Fl_Widget *W, void *Data) {
  glwindow->toggleShowSamplePoints();
}

static void toggleShowInputCurve(Fl_Widget *W, void *Data) {
  glwindow->toggleShowInputCurve();
}

static void SetLeastSquares(Fl_Widget *W, void *Data) {
  glwindow->setObjective(OBJ_LEAST_SQUARES);
}

static void SetManhattan(Fl_Widget *W, void *Data) {
  glwindow->setObjective(OBJ_MANHATTAN);
}

static void SetLogSumExp(Fl_Widget *W, void *Data) {
  glwindow->setObjective(OBJ_LOG_SUM_EXP);
}

static void SetUniformParams(Fl_Widget *W, void *Data) {
  glwindow->setParamMethod(PARAM_UNIFORM);
}

static void SetChordLengthParams(Fl_Widget *W, void *Data) {
  glwindow->setParamMethod(PARAM_CHORD);
}

static void SetCentripetalParams(Fl_Widget *W, void *Data) {
  glwindow->setParamMethod(PARAM_CENTRIPETAL);
}

static void ToggleCusps(Fl_Widget *W, void *Data) {
  glwindow->toggleCusps();
}

Fl_Menu_Item mainMenuitems[] = {
  { "&File",       0, 0, 0, FL_SUBMENU },
    { "&Save",     0, Save },
    { "&Add Known Curve", 0, AddKnownCurve },
    { "&Save Known Curves", 0, SaveKnownCurves },
    { "&Reset",    0, Reset, 0, FL_MENU_DIVIDER },
    { "&Quit",     0, Quit },
    { 0 },
  { "&View",       0, 0, 0, FL_SUBMENU },
    { "&Toggle Control Points", 0, toggleShowControlPoints },
    { "&Toggle Sample Points", 0, toggleShowSamplePoints },
    { "&Toggle Input Curve", 0, toggleShowInputCurve },
    { 0 },
  { "&Optimization", 0, 0, 0, FL_SUBMENU },
    { "&Least Squares", 0, SetLeastSquares },
    { "&Manhattan",     0, SetManhattan },
    { "&LogSumExp",     0, SetLogSumExp, 0, FL_MENU_DIVIDER },
    { "&Uniform",       0, SetUniformParams },
    { "&Chord Length",  0, SetChordLengthParams },
    { "&Centripetal",   0, SetCentripetalParams, 0, FL_MENU_DIVIDER },
    { "&Toggle Cusps",  0, ToggleCusps },
    { 0 },
  { 0 }
};

double MyGLFrameWindow::spline::evaluateBasis(int m, int i, double t) {
  if (m == 0) {
    return fknots[i-1] <= t && t < fknots[i] ? 1 : 0;
  }
  double result = 0;
  double coeff = fknots[i+m-1] == fknots[i-1] ? 0 : (t-fknots[i-1])/(fknots[i+m-1]-fknots[i-1]);
  if (coeff != 0) {
    result += coeff * evaluateBasis(m-1, i, t);
  }
  coeff = fknots[i+m] == fknots[i] ? 0 : (fknots[i+m]-t)/(fknots[i+m]-fknots[i]);
  if (coeff != 0) {
    result += coeff * evaluateBasis(m-1, i+1, t);
  }
  return result;
}

double MyGLFrameWindow::spline::evaluateBasisParams(int i, int param) {
  if (paramBasis == NULL) {
    int len = (points.size()+2) * params.size();
    paramBasis = new double[len];
    for (int i = 0; i < len; i++) {
      paramBasis[i] = INVALID;
    }
    paramBasisLen = len;
  }
  if (paramBasis[i*params.size() + param] == INVALID) {
    paramBasis[i*params.size() + param] = evaluateBasis(degree, i, params[param]);
  }
  else {
    basisSaved++;
  }
  return paramBasis[i*params.size() + param];
}

void MyGLFrameWindow::spline::getFakeKnots(void) {
  fknots.clear();
  fknots.push_back(knots[0]);
  for (int i=0; i < knots.size(); i++) {
    fknots.push_back(knots[i]);
  }
  fknots.push_back(knots[knots.size()-1]);
  fknots.push_back(knots[knots.size()-1]);
}

MyGLFrameWindow::point MyGLFrameWindow::spline::evaluate(double t) {
  point numer(0, 0);
  if (t == knots[knots.size()-1]) {
    return points[points.size()-1];
  }
  for (int i=0; i < points.size(); i++) {
    numer = numer + points[i] * evaluateBasis(degree, i+1, t);
  }
  return numer;
}

void MyGLFrameWindow::spline::reverse() {
  std::vector<point> tmp;
  while (points.size() > 0) {
    tmp.push_back(points.back());
    points.pop_back();
  }
  points = tmp;
  bpoints.clear();
}

void MyGLFrameWindow::spline::mpfr_evaluate(mpfr_t *pts, double t, mpfr_t &x, mpfr_t &y) {
  if (t >= knots[knots.size()-1]) {
    mpfr_set(x, pts[(points.size()-1)*2], MPFR_RNDN);
    mpfr_set(y, pts[(points.size()-1)*2+1], MPFR_RNDN);
    return;
  }
  mpfr_set_d(x, 0.0, MPFR_RNDN);
  mpfr_set_d(y, 0.0, MPFR_RNDN);
  for (int i = 0; i < points.size(); i++) {
    double basis = evaluateBasis(degree, i+1, t);
    mpfr_mul_d(dx, pts[2*i], basis, MPFR_RNDN);
    mpfr_mul_d(dy, pts[2*i+1], basis, MPFR_RNDN);
    mpfr_add(x, x, dx, MPFR_RNDN);
    mpfr_add(y, y, dy, MPFR_RNDN);
  }
}

void MyGLFrameWindow::spline::mpfr_evaluateParamsInternal(mpfr_t *pts, int param, mpfr_t &x, mpfr_t &y) {
  if (params[param] >= knots[knots.size()-1]) {
    mpfr_set(x, pts[(points.size()-1)*2], MPFR_RNDN);
    mpfr_set(y, pts[(points.size()-1)*2+1], MPFR_RNDN);
    return;
  }
  mpfr_set_d(x, 0.0, MPFR_RNDN);
  mpfr_set_d(y, 0.0, MPFR_RNDN);
  for (int i = 0; i < points.size(); i++) {
    double basis = evaluateBasisParams(i+1, param);
    mpfr_mul_d(dx, pts[2*i], basis, MPFR_RNDN);
    mpfr_mul_d(dy, pts[2*i+1], basis, MPFR_RNDN);
    mpfr_add(x, x, dx, MPFR_RNDN);
    mpfr_add(y, y, dy, MPFR_RNDN);
  }
}

MyGLFrameWindow::point MyGLFrameWindow::spline::evaluateParamsInternal(int param) {
  if (params[param] >= knots[knots.size()-1]) {
    return points[points.size()-1];
  }
  point p(0, 0);
  for (int i = 0; i < points.size(); i++) {
    double basis = evaluateBasisParams(i+1, param);
    p.x += basis * points[i].x;
    p.y += basis * points[i].y;
  }
  return p;
}

void MyGLFrameWindow::spline::clearParamEval() {
  for (int i = 0; i < params.size(); i++) {
    paramEvalSet[i] = false;
  }
}

void MyGLFrameWindow::spline::mpfr_evaluateParams(mpfr_t *pts, int param, mpfr_t &x, mpfr_t &y) {
  if (paramEval == NULL) {
    paramEval = new mpfr_t[params.size() * 2];
    paramEvalSet = new bool[params.size()];
    for (int i = 0; i < params.size(); i++) {
      mpfr_inits(paramEval[2*i], paramEval[2*i+1], (mpfr_ptr)0);
      paramEvalSet[i] = false;
    }
  }
  if (!paramEvalSet[param]) {
    mpfr_evaluateParamsInternal(pts, param, paramEval[2*param], paramEval[2*param+1]);
    paramEvalSet[param] = true;
  }
  else {
    evalSaved++;
  }
  mpfr_set(x, paramEval[2*param], MPFR_RNDN);
  mpfr_set(y, paramEval[2*param+1], MPFR_RNDN);
}

MyGLFrameWindow::point MyGLFrameWindow::spline::evaluateParams(int param) {
  if (paramEvalPoint == NULL) {
    paramEvalPoint = new point[params.size()];
    paramEvalSet = new bool[params.size()];
    for (int i = 0; i < params.size(); i++) {
      paramEvalSet[i] = false;
    }
  }
  if (!paramEvalSet[param]) {
    paramEvalPoint[param] = evaluateParamsInternal(param);
    paramEvalSet[param] = true;
  }
  else {
    evalSaved++;
  }
  return paramEvalPoint[param];
}

void MyGLFrameWindow::computeCurve(void) {
  if (curve.degree > 0) {
    int samples = MIN_SAMPLES;
    samples = MIN_SAMPLES > SAMPLE_RATE * curve.points.size() ? MIN_SAMPLES : SAMPLE_RATE * curve.points.size();
    curve.computeCurve(samples);
  }
}

void MyGLFrameWindow::spline::computeCurve(int samples) {
  assert(points.size() > degree);
  bpoints.clear();

  int L = points.size()-degree;
  int n = degree;
  for (int s=0; s < samples; s++) {
    double u = knots[n-1] + (knots[L+n-1]-knots[n-1])*s/((double)samples);
    bpoints.push_back(evaluate(u));
  }
  bpoints.push_back(points[points.size()-1]);

  // compute the curve at the parameters
  paramPoints.clear();
  for (int i = 0; i < params.size(); i++) {
    paramPoints.push_back(evaluate(params[i]));
  }
}

void MyGLFrameWindow::spline::print(std::ostream &os) {
  os << "S " << degree << " " << points.size() << std::endl;
  for (int i = 0; i < points.size(); i++) {
    os << "p " << points[i].x << " " << points[i].y << std::endl;
  }
  for (int i = 0; i < knots.size(); i++) {
    os << "k " << knots[i] << std::endl;
  }
  os << "S 0 0" << std::endl;
}

bool MyGLFrameWindow::spline::read(std::istream &is) {
  char c = 'x';
  is >> c;
  if (c != 'S') {
    return false;
  }
  is >> degree;
  int cps;
  is >> cps;
  for (int i = 0; i < cps; i++) {
    is >> c;
    if (c != 'p') {
      return false;
    }
    double x, y;
    is >> x;
    is >> y;
    points.push_back(point(x, y));
  }
  for (int i = 0; i < cps + degree - 1; i++) {
    is >> c;
    if (c != 'k') {
      return false;
    }
    double knot;
    is >> knot;
    knots.push_back(knot);
  }
  getFakeKnots();
  is >> c;
  if (c != 'S') {
    return false;
  }
  is >> cps; is >> cps;
  return true;
}

double MyGLFrameWindow::spline::compare(std::vector<point> &data, std::vector<int> &samples, spline &sp) {
  int s = samples.size() - 1;
  computeCurve(s);
  sp.degree = degree;
  sp.knots = knots;
  point vtr = bpoints[s/2] - bpoints[0];
  point vsr = data[samples[s/2]] - data[samples[0]];
  if (norm(vtr) < 10 || norm(vsr) < 10) {
    // just an arbitrary fallback
    vtr = bpoints[s/4] - bpoints[0];
    vsr = data[samples[s/4]] - data[samples[0]];
  }
  // rotate the known curve so we agree on the direction of the vector between the first sample and mid sample
  double theta = acos(dot(vtr, vsr)/(norm(vtr) * norm(vsr)));
  //double thetay = acos(dot(vty, vsy)/(norm(vty) * norm(vsy)));
  //double theta = (thetax + thetay)/2;
  double ct = cos(theta);
  double st = sin(theta);
  sp.points.clear();
  for (int i = 0; i < points.size(); i++) {
    sp.points.push_back(point(ct*points[i].x - st*points[i].y, st*points[i].x + ct*points[i].y));
  }
  sp.getFakeKnots();
  sp.computeCurve(s);
  double minxs = sp.bpoints[0].x, maxxs = sp.bpoints[0].x, minys = sp.bpoints[0].y, maxys = sp.bpoints[0].y;
  for (int i = 1; i < bpoints.size(); i++) {
    if (sp.bpoints[i].x < minxs) minxs = sp.bpoints[i].x;
    if (sp.bpoints[i].x > maxxs) maxxs = sp.bpoints[i].x;
    if (sp.bpoints[i].y < minys) minys = sp.bpoints[i].y;
    if (sp.bpoints[i].y > maxys) maxys = sp.bpoints[i].y;
  }
  double minxt = data[samples[0]].x, maxxt = data[samples[0]].x, minyt = data[samples[0]].y, maxyt = data[samples[0]].y;
  for (int i = 1; i < samples.size(); i++) {
    if (data[samples[i]].x < minxt) minxt = data[samples[i]].x;
    if (data[samples[i]].x > maxxt) maxxt = data[samples[i]].x;
    if (data[samples[i]].y < minyt) minyt = data[samples[i]].y;
    if (data[samples[i]].y > maxyt) maxyt = data[samples[i]].y;
  }
  /*point vtx = bpoints[maxxt] - bpoints[minxt];
  point vsx = data[samples[maxxs]] - data[samples[minxs]];
  point vty = bpoints[maxyt] - bpoints[minyt];
  point vsy = data[samples[maxys]] - data[samples[minys]];*/
  double factorx = (maxxt-minxt) / (maxxs - minxs);
  double factory = (maxyt-minyt) / (maxys - minys);
  double factor = (factorx + factory)/2;
  point origin((minxt+maxxt)/2 - factorx*(minxs+maxxs)/2, (minyt+maxyt)/2 - factory*(minys+maxys)/2);
  for (int i = 0; i < points.size(); i++) {
    // also need to scale by a factor to match the vector used previously
    sp.points[i] = factor * sp.points[i];
    // finally, translate the curve so they have the same center
    sp.points[i] = sp.points[i] + origin;
  }
  double sum = 0, sum2 = 0;
  sp.params.clear();
  double beg = sp.knots[0];
  double end = sp.knots.back();
  for (int i = 0; i < s; i++) {
    sp.params.push_back(beg + (end-beg)*((double)i)/s);
    point eval = sp.evaluate(sp.params.back());
    sum += distance(eval, data[samples[i]]);
    sum2 += distance(eval, data[samples[s-i-1]]);
  }
  sp.params.push_back(end);
  if (sum2 < sum) {
    double sum2 = 0;
    std::vector<double> params2;
    for(int i = 0; i < sp.params.size(); i++) {
      params2.push_back(sp.params[sp.params.size()-i-1]);
    }
    sp.params = params2;
  }
  sum = sum2 < sum ? sum2 : sum;
  double rel = (maxxt-minxt + maxyt-minyt)/2;
  sum /= rel;
  printf("Compare sum: %f\n", sum);
  return sum;
}

void MyGLFrameWindow::sampleData() {
  samples.clear();
  double rate = ((double)data.size())/SAMPLE_RATE;
  if (detectCusps) {
    rate /= (cusps.size() + 1);
  }
  rate = rate < 1.0 ? 1.0 : rate;
  int cuspind = 0;
  for (double i = 0.0; i < data.size(); i += rate) {
    int ind = (int)i;
    // transform cusp points
    if (cusps.size() > cuspind && ind > cusps[cuspind]) {
      samples.push_back(cusps[cuspind]);
      cusps[cuspind] = samples.size() - 1;
      cuspind++;
    }
    samples.push_back(ind);
  }
  if (data.size() == 0) {
    return;
  }
  if (samples[samples.size() - 1] != data.size() - 1) {
    samples.push_back(data.size() - 1);
  }
  printf("%zd points sampled.\n", samples.size());
}

void MyGLFrameWindow::printPoints(std::vector<point> &pts) {
  printf("points:\n");
  for (int i = 0; i < pts.size(); i++) {
    printf("(%f, %f)\n", pts[i].x, pts[i].y);
  }
  printf("end points\n");
}

void MyGLFrameWindow::parameterize(spline &sp, int cps) {
  assert(sp.degree > 0);
  sp.params.clear();
  if (paramMethod == PARAM_UNIFORM) {
    for (int i = 0; i < samples.size(); i++) {
      double u = ((double)i)/(samples.size() - 1);
      sp.params.push_back(u);
    }
  }
  else if (paramMethod == PARAM_CHORD || paramMethod == PARAM_CENTRIPETAL) {
    double expon = paramMethod == PARAM_CHORD ? 1 : centripetalParam;
    double L = 0;
    for (int i = 1; i < samples.size(); i++) {
      L += pow(distance(data[samples[i-1]], data[samples[i]]), expon);
    }
    double sum = 0;
    int j = 0;
    do {
      sp.params.push_back(sum / L);
      j++;
      sum += pow(distance(data[samples[j-1]], data[samples[j]]), expon);
    } while(j < samples.size()-1);
    sp.params.push_back(1);
  }
  sp.knots.clear();
  for (int i = 0; i < sp.degree; i++) {
    sp.knots.push_back(0);
  }
  if (cusps.size() > 0) {
    int denom = cps - sp.degree - cusps.size() * sp.degree;
    for (int i = 1; i < denom; i++) {
      sp.knots.push_back(((double)i)/denom);
    }
    for (int i = 0; i < cusps.size(); i++) {
      for (int j = 0; j < sp.degree; j++) {
        sp.knots.push_back(sp.params[cusps[i]]);
      }
    }
    std::sort(sp.knots.begin(), sp.knots.end());
  }
  else {
    // generate knots using the average method
    for (int i = 0; i < cps - sp.degree - 1; i++) {
      double u = 0;
      for (int k = 0; k < sp.degree; k++) {
        u += sp.params[sp.degree + i + 1];
      }
      u /= sp.degree;
      sp.knots.push_back(u);
    }
  }
  for(int i = 0; i < sp.degree; i++) {
    sp.knots.push_back(1);
  }
}

void MyGLFrameWindow::leastSquares(int degree, int cps) {
  // initialize the various parameters
  int ocps = cps;
  if (cps > samples.size()) {
    cps = samples.size() - 1;
    if (cps < degree + 1) {
      cps = degree + 1;
    }
  }
  spline result(degree);
  parameterize(result, cps);
  result.getFakeKnots();
  
  std::vector<point> Q;
  Q.push_back(point(0, 0));
  for (int k = 1; k < samples.size()-1; k++) {
    point Qk = data[samples[k]] - result.evaluateBasis(degree, 1, result.params[k])*data[samples[0]]
      - result.evaluateBasis(degree, cps, result.params[k])*data[samples[samples.size()-1]];
    Q.push_back(Qk);
  }

  Eigen::MatrixXd Qm(cps - 2, 2);
  for (int i = 1; i < cps - 1; i++) {
    point row(0, 0);
    for (int k = 1; k < samples.size()-1; k++) {
      row = row + result.evaluateBasis(degree, i + 1, result.params[k]) * Q[k];
    }
    Qm(i - 1, 0) = row.x;
    Qm(i - 1, 1) = row.y;
  }

  // now Q is known
  Eigen::MatrixXd N(samples.size() - 2, cps - 2);
  for (int k = 1; k < samples.size() - 1; k++) {
    for (int i = 1; i < cps - 1; i++) {
      N(k-1, i-1) = result.evaluateBasis(degree, i + 1, result.params[k]);
    }
  }

  Eigen::MatrixXd M = N.transpose() * N;

  // solve eqns
  Eigen::MatrixXd P;
  if (cps > 2) {
    P = M.colPivHouseholderQr().solve(Qm);
  }

  result.points.push_back(data[samples[0]]);
  for(int i = 0; i < cps - 2; i++) {
    double x = P(i, 0);
    double y = P(i, 1);
    point Pi(x, y);
    result.points.push_back(Pi);
  }
  result.points.push_back(data[samples[samples.size() - 1]]);

  // save the cost
  costs[degree][ocps] = leastSquaresCost(result);
  solutions[degree][ocps] = result;

  //printf("LeastSquares computed with degree %d, ocps %d, cost %f\n", degree, ocps, costs[degree][ocps]);
}

void MyGLFrameWindow::logSumExpCost(spline &sp, mpfr_t *points, mpfr_t &cost) {
  assert(sp.params.size() == samples.size());
  mpfr_set_d(cost, 0.0, MPFR_RNDN);
  for (int i = 0; i < samples.size(); i++) {
    // evaluate the curve at the i-th parameter
    sp.mpfr_evaluateParams(points, i, evalx, evaly);
    // compute sqr distance
    mpfr_sub_d(sqrd, evalx, data[samples[i]].x, MPFR_RNDN);
    mpfr_sub_d(sqrdr, evaly, data[samples[i]].y, MPFR_RNDN);
    mpfr_mul(sqrd, sqrd, sqrd, MPFR_RNDN);
    mpfr_mul(sqrdr, sqrdr, sqrdr, MPFR_RNDN);
    mpfr_add(sqrd, sqrd, sqrdr, MPFR_RNDN);
    // add to sum
    mpfr_exp(sqrd, sqrd, MPFR_RNDN);
    mpfr_add(cost, cost, sqrd, MPFR_RNDN);
  }
  mpfr_log(cost, cost, MPFR_RNDN);
}

void MyGLFrameWindow::logSumExpGradient(spline &sp, mpfr_t *points, mpfr_t *gradient) {
  assert(sp.params.size() == samples.size());
  mpfr_set_d(gradient[0], 0.0, MPFR_RNDN);
  mpfr_set_d(gradient[1], 0.0, MPFR_RNDN);
  mpfr_set_d(gradient[2*(sp.points.size()-1)], 0.0, MPFR_RNDN);
  mpfr_set_d(gradient[2*(sp.points.size()-1)+1], 0.0, MPFR_RNDN);
  for (int j = 1; j < sp.points.size() - 1; j++) {
    mpfr_set_d(expsumx, 0.0, MPFR_RNDN);
    mpfr_set_d(expsumy, 0.0, MPFR_RNDN);
    for (int i = 0; i < samples.size(); i++) {
      // query the curve and basis function
      sp.mpfr_evaluateParams(points, i, evalx, evaly);
      double basis = sp.evaluateBasisParams(j + 1, i);
      // compute dist differences
      mpfr_sub_d(ldiff, evalx, data[samples[i]].x, MPFR_RNDN);
      mpfr_sub_d(rdiff, evaly, data[samples[i]].y, MPFR_RNDN);
      // compute sqr distance
      mpfr_set(sqrd, ldiff, MPFR_RNDN);
      mpfr_set(sqrdr, rdiff, MPFR_RNDN);
      mpfr_mul(sqrd, sqrd, sqrd, MPFR_RNDN);
      mpfr_mul(sqrdr, sqrdr, sqrdr, MPFR_RNDN);
      mpfr_add(sqrd, sqrd, sqrdr, MPFR_RNDN);
      // increment the numerator
      mpfr_set(curx, sqrd, MPFR_RNDN);
      mpfr_exp(curx, curx, MPFR_RNDN);
      mpfr_mul(curx, curx, ldiff, MPFR_RNDN);
      mpfr_mul_d(curx, curx, 2 * basis, MPFR_RNDN);
      mpfr_set(cury, sqrd, MPFR_RNDN);
      mpfr_exp(cury, cury, MPFR_RNDN);
      mpfr_mul(cury, cury, rdiff, MPFR_RNDN);
      mpfr_mul_d(cury, cury, 2 * basis, MPFR_RNDN);
      mpfr_add(expsumx, expsumx, curx, MPFR_RNDN);
      mpfr_add(expsumy, expsumy, cury, MPFR_RNDN);
    }
    if (0.0 == mpfr_get_d(expsumx, MPFR_RNDN) || 0.0 == mpfr_get_d(expsumy, MPFR_RNDN)) {
      printf("j = %d, 0 expsum\n", j);
    }
    // the true gradient has these divided by e^cost.
    // we'll ignore that for efficiency and to avoid extremely
    // small values.
    mpfr_set(gradient[2*j], expsumx, MPFR_RNDN);
    mpfr_set(gradient[2*j+1], expsumy, MPFR_RNDN);
    if (mpfr_zero_p(gradient[2*j])) {
      printf("j = %d, gradient x actually 0\n", j);
    }
    if (mpfr_zero_p(gradient[2*j+1])) {
      printf("j = %d, gradient y actually 0\n", j);
    }
  }
}

static void clearArray(mpfr_t *arr, int len) {
  for (int i=0; i < len; i++) {
    mpfr_clear(arr[i]);
  }
}

void MyGLFrameWindow::logSumExp(int degree, int cps) {
  // initialize the various parameters
  int ocps = cps;
  if (cps > samples.size()) {
    cps = samples.size();
  }
  spline sp(degree);
  parameterize(sp, cps);
  sp.getFakeKnots();
  
  // construct initial solution
  mpfr_t *points = new mpfr_t[cps * 2];
  mpfr_t *prevPoints = new mpfr_t[cps * 2];
  for (int i = 0; i < cps-1; i++) {
    int ind = i * samples.size()/cps;
    sp.points.push_back(data[samples[ind]]);
    mpfr_init_set_d(points[2*i], data[samples[ind]].x, MPFR_RNDN);
    mpfr_init_set_d(points[2*i+1], data[samples[ind]].y, MPFR_RNDN);
    mpfr_inits(prevPoints[2*i], prevPoints[2*i+1], (mpfr_ptr)0);
  }
  sp.points.push_back(data[samples[samples.size()-1]]);
  mpfr_init_set_d(points[2*(cps-1)], data[samples[samples.size()-1]].x, MPFR_RNDN);
  mpfr_init_set_d(points[2*(cps-1)+1], data[samples[samples.size()-1]].y, MPFR_RNDN);
  mpfr_inits(prevPoints[2*(cps-1)], prevPoints[2*(cps-1)+1], (mpfr_ptr)0);

  mpfr_t gradient[cps * 2];
  for (int i = 0; i < cps * 2; i++) {
    mpfr_init(gradient[i]);
  }
  mpfr_t prevCost, curCost, gradMax, gamma, denom, num, cur, tmp;
  mpfr_inits(prevCost, curCost, gradMax, gamma, denom, num, cur, tmp, (mpfr_ptr)0);
  mpfr_set_d(prevCost, -1.0, MPFR_RNDN);
  logSumExpCost(sp, points, curCost);
  int iters;
  // perform gradient descent iterations
  for (iters = 0; ; iters++) {
    //printf("d = %d, p = %d, logSumExpCost: %f\n", degree, cps, mpfr_get_d(curCost, MPFR_RNDN));
    logSumExpGradient(sp, points, gradient);
    mpfr_set_d(gradMax, 0.0, MPFR_RNDN);
    for (int i = 2; i < 2*(cps-1); i++) {
      if (0 < mpfr_cmpabs(gradient[i], gradMax)) {
        mpfr_abs(gradMax, gradient[i], MPFR_RNDN);
      }
    }

    // We need to make gamma reasonably small
    //  so we don't overflow everywhere. Let's do this by normalizing the gradient.
    mpfr_set_d(gamma, 128.0, MPFR_RNDN);
    mpfr_div(gamma, gamma, gradMax, MPFR_RNDN);
    mpfr_t *tmp = prevPoints;
    prevPoints = points;
    points = tmp;

    mpfr_set(prevCost, curCost, MPFR_RNDN);
    int subiters = 0;
    const int MAX_SUBITERS = DEFAULT_PRECISION - 20;
    do {
      // modify the points
      for (int i = 0; i < 2 * cps; i++) {
        mpfr_mul(cur, gamma, gradient[i], MPFR_RNDN);
        mpfr_sub(points[i], prevPoints[i], cur, MPFR_RNDN);
      }
      sp.clearParamEval();
      logSumExpCost(sp, points, curCost);
      mpfr_div_d(gamma, gamma, 2.0, MPFR_RNDN);
      subiters++;
      if (subiters > MAX_SUBITERS) {
        printf("Too many gamma reductions\n");
        break;
      }
    } while(0 >= mpfr_cmp(prevCost, curCost));
    
    if (subiters > MAX_SUBITERS) {
      // if we could not find a better solution, break
      break;
    }
    if (0 == mpfr_cmp(prevCost, curCost)) {
      printf("Cost stayed the same.\n");
      break;
    }
    mpfr_sub(cur, prevCost, curCost, MPFR_RNDN);
    if (0 > mpfr_cmp(cur, MPFR_EPSILON)) {
      printf("Cost decrease was too small.\n");
      break;
    }
    if (MAX_ITERATIONS != -1 && iters >= MAX_ITERATIONS) {
      printf("Max iterations reached.\n");
      break;
    }
  }
  // copy points to the spline
  for (int i = 1; i < cps-1; i++) {
    sp.points[i].x = mpfr_get_d(points[2*i], MPFR_RNDN);
    sp.points[i].y = mpfr_get_d(points[2*i+1], MPFR_RNDN);
  }
  printf("LogSumExp computed in %d iterations with deg %d, ocps %d, cost %f\n", iters, degree, ocps, mpfr_get_d(curCost, MPFR_RNDN));
  // save the cost
  costs[degree][ocps] = mpfr_get_d(curCost, MPFR_RNDN);
  solutions[degree][ocps] = sp;

  // savings
  printf("Saved %d basis function evaluations.\n", basisSaved);
  printf("Saved %d curve evaluations.\n", evalSaved);
  basisSaved = 0;
  evalSaved = 0;

  // clean up
  clearArray(prevPoints, cps*2);
  clearArray(gradient, cps*2);
  clearArray(points, cps*2);
  delete [] prevPoints;
  delete [] points;
  mpfr_clears(prevCost, curCost, gradMax, gamma, denom, num, cur, tmp, (mpfr_ptr)0);
}

double MyGLFrameWindow::costFactor(int deg, int cps) {
  return degreeParam*deg + pointsParam*cps + 1;
}

int MyGLFrameWindow::minCPs(int deg) {
  return deg + 1 + deg*cusps.size();
}

double MyGLFrameWindow::leastSquaresCost(spline &sp) {
  assert(sp.params.size() == samples.size());
  double cost = 0;
  for (int i = 0; i < samples.size(); i++) {
    cost += squareDistance(data[samples[i]], sp.evaluate(sp.params[i]));
  }
  return cost;
}

double MyGLFrameWindow::manhattanCost(spline &sp) {
  double cost = 0;
  for (int i = 0; i < samples.size(); i++) {
    point eval = sp.evaluateParams(i);
    cost += abs(data[samples[i]].x - eval.x) + abs(data[samples[i]].y - eval.y);
  }
  return cost;
}

void MyGLFrameWindow::manhattanGradient(spline &sp, std::vector<point> &gradient) {
  gradient.clear();
  for (int j = 0; j < sp.points.size(); j++) {
    double sumx = 0.0, sumy = 0.0;
    for (int i = 0; i < sp.params.size(); i++) {
      point eval = sp.evaluateParams(i);
      double basis = sp.evaluateBasisParams(j + 1, i);
      sumx += sgn(eval.x - data[samples[i]].x) * basis;
      sumy += sgn(eval.y - data[samples[i]].y) * basis;
    }
    gradient.push_back(point(sumx, sumy));
  }
}

void MyGLFrameWindow::manhattan(int degree, int cps) {
  // initialize the various parameters
  int ocps = cps;
  if (cps > samples.size()) {
    cps = samples.size();
  }
  spline sp(degree);
  parameterize(sp, cps);
  sp.getFakeKnots();
  
  // construct initial solution
  std::vector<point> prevPoints;
  for (int i = 0; i < cps-1; i++) {
    int ind = i * samples.size()/cps;
    sp.points.push_back(data[samples[ind]]);
  }
  sp.points.push_back(data[samples[samples.size()-1]]);

  std::vector<point> gradient;

  double gradMax, gamma, denom, num, cur, tmp;
  double curCost = manhattanCost(sp);
  double prevCost = -1.0;
  int iters;
  // perform gradient descent iterations
  for (iters = 0; ; iters++) {
    //printf("d = %d, p = %d, logSumExpCost: %f\n", degree, cps, mpfr_get_d(curCost, MPFR_RNDN));
    manhattanGradient(sp, gradient);
    gradMax = 0;
    for (int i = 1; i < cps-1; i++) {
      if (abs(gradient[i].x) > gradMax) {
        gradMax = abs(gradient[i].x);
      }
      if (abs(gradient[i].y) > gradMax) {
        gradMax = abs(gradient[i].y);
      }
    }

    // We need to make gamma reasonably small
    //  so we don't overflow everywhere. Let's do this by normalizing the gradient.
    gamma = 128.0 / gradMax;
    prevPoints = sp.points;

  if (cps == 3 && degree == 2) {
    printf("wat\n");
  }

    prevCost = curCost;
    int subiters = 0;
    const int MAX_SUBITERS = 16;
    do {
      // modify the points
      for (int i = 1; i < cps - 1; i++) {
        sp.points[i] = prevPoints[i] - gamma * gradient[i];
      }
      sp.clearParamEval();
      curCost = manhattanCost(sp);
      gamma /= 2.0;
      subiters++;
      if (subiters > MAX_SUBITERS) {
        printf("Too many gamma reductions\n");
        break;
      }
    } while(curCost >= prevCost);
    
    if (subiters > MAX_SUBITERS) {
      // if we could not find a better solution, break
      break;
    }
    if (prevCost == curCost) {
      printf("Cost stayed the same.\n");
      break;
    }
    if (abs(prevCost - curCost) < EPSILON) {
      printf("Cost decrease was too small.\n");
      break;
    }
    if (MAX_ITERATIONS != -1 && iters >= MAX_ITERATIONS) {
      printf("Max iterations reached.\n");
      break;
    }
  }

  // savings
  printf("Saved %d basis function evaluations.\n", basisSaved);
  printf("Saved %d curve evaluations.\n", evalSaved);
  basisSaved = 0;
  evalSaved = 0;

  // save the cost
  costs[degree][ocps] = curCost;
  solutions[degree][ocps] = sp;
}

void MyGLFrameWindow::initializeCostMatrix(int max_cps) {
  for (int i = 0; i <= MAX_DEGREE+1; i++) {
    if (costs[i]) {
      delete [] costs[i];
      delete [] tried[i];
      delete [] solutions[i];
    }
    costs[i] = new double[max_cps+2];
    tried[i] = new bool[max_cps+2];
    solutions[i] = new spline[max_cps+2];
    for (int j = 0; j <= max_cps+1; j++) {
      if (j < minCPs(i) || i < MIN_DEGREE) {
        costs[i][j] = INVALID;
      }
      else {
        costs[i][j] = -1;
      }
      tried[i][j] = false;
    }
  }
}

void MyGLFrameWindow::approxCurve() {
  if (!detectCusps) {
    cusps.clear();
  }
  const int max_cps = MAX_CPS + MAX_DEGREE * cusps.size();

  // sample the input
  sampleData();

  if (samples.size() <= MIN_DEGREE + 1) {
    std::cerr << "Not enough data." << std::endl;
    return;
  }

  // compare with known curves
  spline knownSpline;
  int known = compareKnownCurves(knownSpline);
  if (known >= 0) {
    printf("Matched to known curve #%d\n", known);
    curve = knownSpline;
    return;
  }

  // initializes memoized costs
  initializeCostMatrix(max_cps);

  // select the approximation function
  void (MyGLFrameWindow::*solver)(int, int);
  if (objective == OBJ_LEAST_SQUARES) {
    solver = &MyGLFrameWindow::leastSquares;
  }
  else if (objective == OBJ_LOG_SUM_EXP) {
    solver = &MyGLFrameWindow::logSumExp;
  }
  else if (objective == OBJ_MANHATTAN) {
    solver = &MyGLFrameWindow::manhattan;
  }

  // do gradient descent
  int curDeg = MAX_DEGREE / 2;
  int curCPs = MAX_CPS / 2 + MAX_DEGREE * cusps.size();
  (this->*solver)(curDeg, curCPs);
  double curCost = costs[curDeg][curCPs];
  double curFactor = costFactor(curDeg, curCPs);
  while (1) {
    printf("curDeg: %d, curCPs: %d, curCost: %f (%f)\n", curDeg, curCPs, curCost, curFactor*curCost);
    tried[curDeg][curCPs] = true;
    // select the gradient by approximating the derivatives of the main cost
    //  function with the nearest valid integer degree, CPs pair.
    double dd = 0;
    double dp = 0;
    if (costs[curDeg+1][curCPs] == INVALID) {
      if (costs[curDeg-1][curCPs] < 0) {
        (this->*solver)(curDeg-1, curCPs);
      }
      dd += costs[curDeg][curCPs] - costs[curDeg-1][curCPs];
    }
    else {
      if (costs[curDeg+1][curCPs] < 0) {
        (this->*solver)(curDeg+1, curCPs);
      }
      dd += costs[curDeg+1][curCPs] - costs[curDeg][curCPs];
    }
    if (costs[curDeg][curCPs+1] == INVALID) {
      if (costs[curDeg][curCPs-1] < 0) {
        (this->*solver)(curDeg, curCPs-1);
      }
      dp += costs[curDeg][curCPs] - costs[curDeg][curCPs-1];
    }
    else {
      if (costs[curDeg][curCPs+1] < 0) {
        (this->*solver)(curDeg, curCPs+1);
      }
      dp += costs[curDeg][curCPs+1] - costs[curDeg][curCPs];
    }
    dd *= curFactor;
    dp *= curFactor;
    dd += degreeParam*curCost;
    dp += pointsParam*curCost;
    // check boundaries
    if (curDeg == MIN_DEGREE && dd > 0) {
      dd = 0;
    }
    if (minCPs(curDeg + 1) > curCPs && dd < 0) {
      dd = 0;
    }
    if (minCPs(curDeg + 1) > curCPs - 1 && dd < 0 && dp > 0) {
      dd = 0;
    }
    if (curCPs == minCPs(curDeg) && dp > 0) {
      dp = 0;
    }
    if (curDeg == MAX_DEGREE && dd < 0) {
      dd = 0;
    }
    if (curCPs == max_cps && dp < 0) {
      dp = 0;
    }
    // scale this gradient if it is too large
    double gradMax = abs(dd) > abs(dp) ? abs(dd) : abs(dp);
    if (gradMax < EPSILON) break;
    if (gradMax > APPROX_STEP_SIZE) {
      dd *= APPROX_STEP_SIZE/gradMax;
      dp *= APPROX_STEP_SIZE/gradMax;
    }
    int prevDeg = curDeg;
    int prevCPs = curCPs;
    if (abs(dd) > EPSILON) {
      curDeg -= sgn(dd);
    }
    if (abs(dp) > EPSILON) {
      curCPs -= sgn(dp);
    }
    assert(costs[curDeg][curCPs] != INVALID);
    if (tried[curDeg][curCPs]) {
      if (!tried[prevDeg][curCPs]) {
        curDeg = prevDeg;
      }
      else if (!tried[curDeg][prevCPs]) {
        curCPs = prevCPs;
      }
      else {
        break;
      }
    }
    curFactor = costFactor(curDeg, curCPs);
    if (costs[curDeg][curCPs] < 0) {
      (this->*solver)(curDeg, curCPs);
    }
    curCost = costs[curDeg][curCPs];
  }
  curCost *= curFactor;
  // find the best option we looked at (necessary in case of cycles)
  int mini = curDeg, minj = curCPs;
  for (int i = 1; i <= MAX_DEGREE; i++) {
    for (int j = minCPs(i); j <= max_cps; j++) {
      double fac = costFactor(i, j);
      if (tried[i][j] && fac*costs[i][j] < curCost) {
        assert(costs[i][j] >= 0);
        curCost = costs[i][j] * fac;
        mini = i;
        minj = j;
      }
    }
  }
  curve = solutions[mini][minj];

  /*printf("knots:\n");
  for (int i = 0; i < curve.knots.size(); i++) {
    printf("%d: %f\n", i, curve.knots[i]);
  }
  printf("end knots\n");
  printf("params:\n");
  for (int i = 0; i < curve.params.size(); i++) {
    printf("%d: %f\n", i, curve.params[i]);
  }
  printf("end params\n");*/
  printf("Final degree: %d, cps: %d, cost: %f\n", mini, minj, curCost);
}

int MyGLFrameWindow::compareKnownCurves(spline &sp) {
  if (knownCurves.size() == 0) {
    return -1;
  }
  double bestDistance = knownCurves[0].compare(data, samples, sp);
  int bestCurve = 0;
  for (int i = 1; i < knownCurves.size(); i++) {
    spline cursp;
    double distance = knownCurves[i].compare(data, samples, cursp);
    if (distance < bestDistance) {
      bestCurve = i;
      bestDistance = distance;
      sp = cursp;
    }
  }
  if (bestDistance <= knownCurveParam) {
    return bestCurve;
  }
  else {
    return -1;
  }
}

void MyGLFrameWindow::loadKnownCurves(std::istream &is) {
  while (1) {
    spline sp;
    if (!sp.read(is)) {
      break;
    }
    // need to translate curve to origin
    for (int i = 0; i < sp.points.size(); i++) {
      sp.points[i] = sp.points[i] - sp.points[0];
    }
    knownCurves.push_back(sp);
    // reverse the curve and add again
    //sp.reverse();
    //knownCurves.push_back(sp);
  }
  printf("Successfully read %zd known curves.\n", knownCurves.size());
}

void MyGLFrameWindow::printKnownCurves(const char *fname) {
  std::ofstream os(fname, std::ofstream::out);
  for (int i = 0; i < knownCurves.size(); i++) {
    knownCurves[i].print(os);
  }
}

void MyGLFrameWindow::addKnownCurve() {
  if (curve.degree > 0) {
    knownCurves.push_back(curve);
    //knownCurves.push_back(curve);
    // begin curve at origin
    point off = curve.points[0];
    for (int i = 0; i < curve.points.size(); i++) {
      knownCurves[knownCurves.size()-1].points[i] = knownCurves[knownCurves.size()-1].points[i] - off;
      //knownCurves[knownCurves.size()-2].points[i] = knownCurves[knownCurves.size()-2].points[i] - off;
    }
    // reverse the second curve
    //knownCurves[knownCurves.size()-1].reverse();
  }
}

int main(int argc, char* argv[]){
  mpfr_set_default_prec(DEFAULT_PRECISION);

  glwindow = new MyGLFrameWindow();

  mpfr_init_set_d(MPFR_EPSILON, 1.0, MPFR_RNDN);
  for (int i = 0; i < DEFAULT_PRECISION - 25; i++) {
    mpfr_div_d(MPFR_EPSILON, MPFR_EPSILON, 2.0, MPFR_RNDN);
  }
    
  mpfr_inits(evalx, evaly, sqrd, sqrdr, (mpfr_ptr)0);
  mpfr_inits(dx, dy, (mpfr_ptr)0);
  mpfr_inits(denom, expsumx, expsumy, ldiff, rdiff, curx, cury, (mpfr_ptr)0);

  if (argc >= 2) {
    std::ifstream is(argv[1]);
    glwindow->loadKnownCurves(is);
    is.close();
  }

  // can also add FL_STENCIL, FL_ALPHA, FL_STEREO, FL_DEPTH
  // can test if a flag supported with getGL().can_do
  glwindow->getGL().mode(FL_RGB |FL_DOUBLE);

  // set the menu items
  glwindow->getMenu().copy(mainMenuitems);

  glwindow->setupWidgets();

  glwindow->show();

  return Fl::run();
}