#include <math.h>
#include <stdio.h>
#include <assert.h>
#include <string.h>

#include <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;
}