#include <math.h></math.h>

#include <stdio.h></stdio.h>

#include <assert.h></assert.h>

#include <string.h></string.h>

#include <helianthus.h></helianthus.h>

#define MAX_POINTS  100

#define POINTS_FILE "output/ellipse-points.txt"

#define POINTS_SIZE 10

#define SCALE       100

typedef struct {

  double x;

  double y;

} Point;

typedef struct {

  union {

    struct { Point p; };

    struct { double x, y; };

  };

  double w;

} Point3;

typedef struct {

  union {

    struct { double m[9]; };

    struct { Point3 r[3]; };

    struct { Point3 r0, r1, r2; };

    struct { Point3 dx, dy, dw; };

    struct { double m00, m01, m02,

                    m10, m11, m12,

                    m20, m21, m22; };

  };

} Matrix;

Point *points[MAX_POINTS];

int pointsCnt;

Point *current;

double cmx, cmy;

Point e00, e01, e10, e11;

Matrix em;

Matrix em2;

Matrix em3;

Point* vecZero(Point *v)

  { memset(v, 0, sizeof(*v)); return v; }

Point* vecSet(Point *v, double x, double y)

  { v->x = x; v->y = y; return v; }

Point* vecCopy(Point *v, const Point *a)

  { memcpy(v, a, sizeof(*v)); return v; }

Point3* vec3Zero(Point3 *v)

  { memset(v, 0, sizeof(*v)); return v; }

Point3* vec3Set(Point3 *v, double x, double y, double w)

  { v->x = x; v->y = y; v->w = w; return v; }

Point3* vec3Copy(Point3 *v, const Point3 *a)

  { memcpy(v, a, sizeof(*v)); return v; }

Matrix* matZero(Matrix *m)

  { memset(m, 0, sizeof(*m)); return m; }

Matrix* matIdentity(Matrix *m)

  { matZero(m); m->dx.x = m->dy.y = m->dw.w = 1; return m; }

Matrix* matCopy(Matrix *m, const Matrix *a)

  { memcpy(m, a, sizeof(*m)); return m; }

Point* matMulVec(Point *v, const Matrix *a, const Point *b) {

  Point bb; if (v == b) b = vecCopy(&bb, b);

  double w = a->dx.w * b->x + a->dy.w * b->y + a->dw.w;

  w = fabs(w) > 1e-8 ? 1/w : 0;

  v->x = (a->dx.x * b->x + a->dy.x * b->y + a->dw.x)*w;

  v->y = (a->dx.y * b->x + a->dy.y * b->y + a->dw.y)*w;

  return v;

}

Point3* matMulVec3(Point3 *v, const Matrix *a, const Point3 *b) {

  Point3 bb; if (v == b) b = vec3Copy(&bb, b);

  v->x = a->dx.x * b->x + a->dy.x * b->y + a->dw.x * b->w;

  v->y = a->dx.y * b->x + a->dy.y * b->y + a->dw.y * b->w;

  v->w = a->dx.w * b->x + a->dy.w * b->y + a->dw.w * b->w;

  return v;

}

Matrix* matMul(Matrix *m, const Matrix *a, const Matrix *b) {

  Matrix aa; if (m == a) a = matCopy(&aa, a);

  Matrix bb; if (m == b) b = matCopy(&bb, b);

  matMulVec3(&m->dx, a, &b->dx);

  matMulVec3(&m->dy, a, &b->dy);

  matMulVec3(&m->dw, a, &b->dw);

  return m;

}

Matrix* matInv(Matrix *m, const Matrix *a) {

  Matrix aa; if (m == a) a = matCopy(&aa, a);

  m->m00 = a->m11*a->m22 - a->m12*a->m21;

  m->m10 = a->m12*a->m20 - a->m10*a->m22;

  m->m20 = a->m10*a->m21 - a->m11*a->m20;

  double d = m->m00*a->m00 + m->m10*a->m01 + m->m20*a->m02;

  if (!(fabs(d) > 1e-8)) return matZero(m);

  d = 1/d;

  m->m00 *= d; m->m10 *= d; m->m20 *= d;

  m->m01 = (a->m02*a->m21 - a->m01*a->m22)*d;

  m->m02 = (a->m01*a->m12 - a->m02*a->m11)*d;

  m->m11 = (a->m00*a->m22 - a->m02*a->m20)*d;

  m->m12 = (a->m02*a->m10 - a->m00*a->m12)*d;

  m->m21 = (a->m01*a->m20 - a->m00*a->m21)*d;

  m->m22 = (a->m00*a->m11 - a->m01*a->m10)*d;

  return m;

}

int equal(double a, double b)

  { return fabs(a-b) < 1e-4; }

void fixEllipseMatrix() {

  matZero(&em2);

  Matrix m;

  matInv(&m, &em);

  double A = m.r0.x*m.r0.x + m.r0.y*m.r0.y - m.r0.w*m.r0.w;

  double B = m.r1.x*m.r1.x + m.r1.y*m.r1.y - m.r1.w*m.r1.w;

  double C = m.r2.x*m.r2.x + m.r2.y*m.r2.y - m.r2.w*m.r2.w;

  double D = m.r0.x*m.r1.x + m.r0.y*m.r1.y - m.r0.w*m.r1.w;

  double E = m.r0.x*m.r2.x + m.r0.y*m.r2.y - m.r0.w*m.r2.w;

  double F = m.r1.x*m.r2.x + m.r1.y*m.r2.y - m.r1.w*m.r2.w;

  //printf( "A: %f, B: %f, C: %f, D: %f, E: %f, F: %f\n",

  //         A    , B    , C    , D    , E    , F        );

  matZero(&m);

  if (!(fabs(A) > 1e-8))

    return;

  double X0 = A;

  double X1 = D*D/X0;

  double X2 = E*E/X0;

  double Y1 = (B - X1);

  double Y2 = (C - X2);

  double k = sqrt(X1*X2);

  if (D*E < 0) k = -k;

  k = F - k;

  k = k*k - Y1*Y2;

  if (!(fabs(k) > 1e-8))

    return;

  k = Y1/k;

  m.r0.x = sqrt(X0*k);

  m.r1.x = sqrt(X1*k);

  m.r1.y = sqrt(Y1*k);

  m.r2.x = sqrt(X2*k);

  m.r2.y = sqrt(Y2*k + 1);

  m.r2.w = 1;

  if (D*k < 0) m.r1.x = -m.r1.x;

  if (E*k < 0) m.r2.x = -m.r2.x;

  if (F*k < m.r1.x * m.r2.x) m.r2.y = -m.r2.y;

  //assert(equal( A*k , m.r0.x*m.r0.x + m.r0.y*m.r0.y - m.r0.w*m.r0.w ));

  //assert(equal( B*k , m.r1.x*m.r1.x + m.r1.y*m.r1.y - m.r1.w*m.r1.w ));

  //assert(equal( C*k , m.r2.x*m.r2.x + m.r2.y*m.r2.y - m.r2.w*m.r2.w ));

  //assert(equal( D*k , m.r0.x*m.r1.x + m.r0.y*m.r1.y - m.r0.w*m.r1.w ));

  //assert(equal( E*k , m.r0.x*m.r2.x + m.r0.y*m.r2.y - m.r0.w*m.r2.w ));

  //assert(equal( F*k , m.r1.x*m.r2.x + m.r1.y*m.r2.y - m.r1.w*m.r2.w ));

  matInv(&em2, &m);

}

void fixEllipseMatrix2() {

  matZero(&em3);

  double a = em2.m00;

  double b = em2.m01;

  double c = em2.m10;

  double d = em2.m11;

  //printf("a: %f, b: %f, c: %f, d: %f\n", a, b, c, d);

  double g = a*c + b*d;

  if (!(fabs(g) > 1e-8))

    { em3 = em2; return; }

  g = (a*a + b*b - c*c - d*d)/g;

  double G = g*g + 4;

  //printf("g: %f, G: %f\n", g, G);

  double D = G*G - 4*G;

  assert(D >= 0);

  D = sqrt(D);

  assert(G > D);

  double e = (G + D)/(2*G);

  double f = 1 - e;

  assert(0 <= e && e <= 1);

  assert(0 <= f && f <= 1);

  e = sqrt(e);

  f = sqrt(f);

  if (g < 0) f = -f;

  //printf("e: %f, f: %f\n", e, f);

  Matrix m = {};

  m.m00 =  e;

  m.m01 =  f;

  m.m10 = -f;

  m.m11 =  e;

  m.m22 =  1;

  matMul(&em3, &em2, &m);

}

void updateEllipseMatrix() {

  double A = e11.x - e01.x;

  double B = e11.y - e01.y;

  double C = e11.x - e10.x;

  double D = e11.y - e10.y;

  double E = e01.x + e10.x - e00.x - e11.x;

  double F = e01.y + e10.y - e00.y - e11.y;

  double k = A*D - B*C;

  if (fabs(k) < 1e-6) return;

  k = 1/k;

  em.dx.w = (D*E - C*F)*k;

  em.dy.w = (A*F - B*E)*k;

  em.dw.w = 1;

  em.dx.x = e01.x*(em.dx.w + 1) - e00.x;

  em.dx.y = e01.y*(em.dx.w + 1) - e00.y;

  em.dy.x = e10.x*(em.dy.w + 1) - e00.x;

  em.dy.y = e10.y*(em.dy.w + 1) - e00.y;

  em.dw.x = e00.x;

  em.dw.y = e00.y;

  Matrix m = {};

  m.dx.x = m.dy.y = 0.5;

  m.dw.x = m.dw.y = 0.5;

  m.dw.w = 1;

  matMul(&em, &em, &m);

  fixEllipseMatrix();

  fixEllipseMatrix2();

}

void drawEllipse(Matrix *m) {

  for(double a = 0; a < 2*PI; a += 0.05) {

    Point p = { cos(a), sin(a) };

    matMulVec(&p, m, &p);

    lineTo(p.x, p.y);

  }

  closePath();

  Point p[] = { {-1, 0}, {1, 0}, {0, -1}, {0, 1} };

  for(int i = 0; i < 4; ++i)

    matMulVec(&p[i], m, &p[i]);

  line(p[0].x, p[0].y, p[1].x, p[1].y);

  line(p[2].x, p[2].y, p[3].x, p[3].y);

}

void drawEllipseBounds() {

  stroke(colorByRGBA(0, 0, 0, 0.5));

  moveTo(e00.x, e00.y);

  lineTo(e01.x, e01.y);

  lineTo(e11.x, e11.y);

  lineTo(e10.x, e10.y);

  closePath();

}

void savePoints() {

  FILE *f = fopen(POINTS_FILE, "w");

  for(int i = 0; i < pointsCnt; ++i)

    fprintf(f, "%f %f\n", points[i]->x, points[i]->y);

}

void loadPoints() {

  FILE *f = fopen(POINTS_FILE, "r");

  for(int i = 0; i < pointsCnt; ++i)

    fscanf(f, "%lf %lf", &points[i]->x, &points[i]->y);

}

void init() {

  points[pointsCnt++] = &e00;

  points[pointsCnt++] = &e01;

  points[pointsCnt++] = &e10;

  points[pointsCnt++] = &e11;

  loadPoints();

}

void deinit() {

  savePoints();

}

void draw() {

  double w = windowGetWidth();

  double h = windowGetHeight();

  saveState();

  translate(w/2, h/2);

  zoom(SCALE);

  strokeWidth(1.0/SCALE);

  double pointRadius = POINTS_SIZE/2.0/SCALE;

  double pointRadius2 = pointRadius*pointRadius;

  double mx = mouseTransformedX();

  double my = mouseTransformedY();

  if (mouseWentDown("left")) {

    current = NULL;

    for(int i = pointsCnt-1; i >= 0; --i) {

      cmx = points[i]->x - mx;

      cmy = points[i]->y - my;

      if (cmx*cmx + cmy*cmy <= pointRadius2) {

        current = points[i];

        break;

      }

    }

  } else

  if (current && mouseDown("left")) {

    current->x = mx + cmx;

    current->y = my + cmy;

  } else {

    current = NULL;

  }

  noFill();

  updateEllipseMatrix();

  stroke(colorByRGBA(1, 0, 0, 0.5));

  drawEllipse(&em);

  stroke(colorByRGBA(0, 1, 0, 0.5));

  drawEllipse(&em2);

  stroke(colorByRGBA(0, 0, 1, 0.5));

  drawEllipse(&em3);

  stroke(colorByRGBA(0, 0, 0, 0.5));

  drawEllipseBounds();

  fill(colorByRGBA(0, 0, 1, 0.5));

  for(int i = 0; i < pointsCnt; ++i)

    circle(points[i]->x, points[i]->y, pointRadius);

  restoreState();

}

int main() {

  windowSetVariableFrameRate();

  windowSetResizable(TRUE);

  windowSetInit(&init);

  windowSetDeinit(&deinit);

  windowSetDraw(&draw);

  windowRun();

  return 0;

}