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

#include <helianthus.h>


int printed = 0;
int started = 0;
double mx, my, ax, ay;


void vec3set(double *v, double x, double y, double z)
  { v[0] = x, v[1] = y, v[2] = z; }
void vec3cpy(double *v, const double *vv)
  { memcpy(v, vv, sizeof(*v)*3); }
void vec3add(double *v, const double *vv)
  { v[0] += vv[0]; v[1] += vv[1]; v[2] += vv[2]; }
void vec3sub(double *v, const double *vv)
  { v[0] -= vv[0]; v[1] -= vv[1]; v[2] -= vv[2]; }
void vec3mul(double *v, double x)
  { v[0] *= x; v[1] *= x; v[2] *= x; }
double vec3dot(const double *a, const double *b)
  { return a[0]*b[0] + a[1]*b[1] + a[2]*b[2]; }
void vec3trans(double *v, const double *m) {
  vec3set(v, v[0]*m[0] + v[1]*m[3] + v[2]*m[6],
             v[0]*m[1] + v[1]*m[4] + v[2]*m[7],
             v[0]*m[2] + v[1]*m[5] + v[2]*m[8] );
}

void mat3cpy(double *m, double *mm)
  { memcpy(m, mm, sizeof(*m)*9); }
void mat3one(double *m) {
  m[1] = m[2] = m[3] = m[5] = m[6] = m[7] = 0;
  m[0] = m[4] = m[8] = 1;
}
void mat3rotX(double *m, double a) {
  rotateVector(&m[1], &m[2], a);
  rotateVector(&m[4], &m[5], a);
  rotateVector(&m[7], &m[8], a);
}
void mat3rotY(double *m, double a) {
  rotateVector(&m[2], &m[0], a);
  rotateVector(&m[5], &m[3], a);
  rotateVector(&m[8], &m[6], a);
}
void mat3rotZ(double *m, double a) {
  rotateVector(&m[0], &m[1], a);
  rotateVector(&m[3], &m[4], a);
  rotateVector(&m[6], &m[7], a);
}


void lineTo3d(const double *m, double x, double y, double z) {
  double v[] = {x, y, z};
  vec3trans(v, m);
  lineTo(v[0], v[1]);
}
void lineTo3dv(const double *m, const double *v)
  { lineTo3d(m, v[0], v[1], v[2]); }


void func(double *v, double x, double y) {
  double angle  = 1.5*PI*y;
  double radius = 4*(1-y) + 2*y;
  double size   = (1-y)*0.25;

  double ss = sin(angle), cc = cos(angle);

  double a = 2*PI*(x-0.5)*size + PI/2, s = sin(a), c = cos(a);
  vec3set(v, c*radius, s*cc*radius, s*ss*radius);
  rotateVector(&v[0], &v[1], 45);
}


void tangent(double *v, double x, double y, double dx, double dy) {
  double p[3];
  func(p, x-dx, y-dy);
  func(v, x+dx, y+dy);
  vec3sub(v, p);
  double l = sqrt(vec3dot(v, v));
  vec3mul(v, 1/l);
}


void drawCubic(const double *m, const double *vv) {
  for(int i = 0; i <= 16; ++i) {
    double l = i/16.0, k = 1-l;
    double v[3] = {};
    double vc[4][3];
    memcpy(vc, vv, sizeof(vc));
    vec3mul(vc[0], k*k*k);
    vec3mul(vc[1], 3*k*k*l);
    vec3mul(vc[2], 3*l*l*k);
    vec3mul(vc[3], l*l*l);
    vec3add(v, vc[0]);
    vec3add(v, vc[1]);
    vec3add(v, vc[2]);
    vec3add(v, vc[3]);
    lineTo3dv(m, v);
  }
  strokePath();
}


void makeCubicBase(double *vv, double *p0, double *t0, double *p1, double *t1) {
  double d[3], tt0[3], tt1[3];
  vec3cpy(d, p1);
  vec3sub(d, p0);
  double ld = sqrt(vec3dot(d, d));

  double l0 = sqrt(vec3dot(t0, t0));
  double l1 = sqrt(vec3dot(t1, t1));
  double k = ld/3;
  vec3cpy(tt0, t0); vec3mul(tt0, k/l0);
  vec3cpy(tt1, t1); vec3mul(tt1, k/l1);

  double pp0[3], pp1[3];
  vec3cpy(pp0, p0); vec3add(pp0, tt0);
  vec3cpy(pp1, p1); vec3sub(pp1, tt1);

  vec3cpy(&vv[0], p0);
  vec3cpy(&vv[3], pp0);
  vec3cpy(&vv[6], pp1);
  vec3cpy(&vv[9], p1);

  if (!printed) {
    if (started)
      printf("                   \"");
    else
      printf("\" [%.2f][%.2f][%.2f]", vv[0]+4, vv[1]+4, vv[2]+4);

    for(int i = 1; i < 4; ++i) {
      printf("-[%.2f][%.2f][%.2f]", vv[3*i+0]+4, vv[3*i+1]+4, vv[3*i+2]+4);
    }
    printf("\"\n");
  }
  started = 1;
}



void makeCubic(
  double *vv,
  double x0, double y0, double dx0, double dy0,
  double x1, double y1, double dx1, double dy1 )
{
  double p0[3], t0[3], p1[3], t1[3];
  func(p0, x0, y0);
  func(p1, x1, y1);
  tangent(t0, x0, y0, dx0, dy0);
  tangent(t1, x1, y1, dx1, dy1);
  makeCubicBase(vv, p0, t0, p1, t1);
}


void makeCubicBase2(const double *m, double *vv, double *p0, double *t0, double *p1, double *t1) {
  makeCubicBase(vv, p0, t0, p1, t1);
  drawCubic(m, vv);
}

void makeCubic2(
  const double *m, double *vv,
  double x0, double y0, double dx0, double dy0,
  double x1, double y1, double dx1, double dy1 )
{
  makeCubic(vv, x0, y0, dx0, dy0, x1, y1, dx1, dy1);
  drawCubic(m, vv);
}



void init() { }


void draw() {
  double w = windowGetWidth();
  double h = windowGetHeight();

  saveState();
  translate(w/2, h/2);

  int ml = mouseDown("left");
  int mr = mouseDown("right");

  double pmx = mx, pmy = my;
  mx = mouseX(), my = mouseY();
  if (ml || mr) { ax += pmx - mx; ay += my - pmy; }
  ax = fmod(ax, 360);
  ay = fmod(ay, 360);

  double step = 128/8.0;
  double mat[9] = {step, 0, 0, 0, 0, step, 0, -step, 0};
  mat3rotY(mat, ax);
  mat3rotX(mat, ay);

  double axislen = 1;
  strokeWidth(2);
  stroke(COLOR_RED);   line(0, 0, mat[0]*axislen, mat[1]*axislen);
  stroke(COLOR_BLACK); line(0, 0, mat[3]*axislen, mat[4]*axislen);
  stroke(COLOR_BLUE);  line(0, 0, mat[6]*axislen, mat[7]*axislen);
  strokeWidth(1);
  stroke(COLOR_BLACK);

  noFill();
  for(int i = 0; i <= 16; ++i) {
    for(int j = 0; j <= 16; ++j) {
      double v[3];
      func(v, i/16.0, j/16.0);
      vec3trans(v, mat);
      point(v[0], v[1]);
    }
  }

  double curve[12];
  double p0[3], t0[3], p1[3], t1[3];

  started = 0;
  vec3set(p0, 4, 0, -4); vec3set(t0, 0, 1, 0);
  func(p1, 0, 0); tangent(t1, 0, 0, 0, 0.001);
  makeCubicBase2(mat, curve, p0, t0, p1, t1);
  for(int i = 0; i < 6; ++i)
    makeCubic2(mat, curve, 0, i/6.0, 0, 0.001, 0, (i+1)/6.0, 0, 0.001 );

  for(int i = 0; i < 6; ++i)
    makeCubic2(mat, curve, 1, 1-i/6.0, 0, -0.001, 1, 1-(i+1)/6.0, 0, -0.001 );
  func(p0, 1, 0); tangent(t0, 1, 0, 0, -0.001);
  vec3set(p1, 0, -4, -4); vec3set(t1, 1, 0, 0);
  makeCubicBase2(mat, curve, p0, t0, p1, t1);

  started = 0;
  makeCubic2(mat, curve, 0, 1/3.0, 0.001, 0, 1, 1/3.0, 0.001, 0);
  started = 0;
  makeCubic2(mat, curve, 0, 2/3.0, 0.001, 0, 1, 2/3.0, 0.001, 0);

  lineTo3d(mat, 4,  0, -4);
  lineTo3d(mat, 4, -4, -4);
  lineTo3d(mat, 0, -4, -4);
  lineTo3d(mat, 0,  0, -4);
  closePath();

  printed = 1;

  restoreState();
}


int main() {
  windowSetVariableFrameRate();
  windowSetResizable(TRUE);
  windowSetInit(&init);
  windowSetDraw(&draw);
  windowRun();
}