#include <math.h>
#include <GL/gl.h>
#include <helianthus.h>


#include "matrix.inc.h"


typedef struct {
  int colors[6];
  Matrix position;
  Vector3 mousePos[6];
} Cube;


const double colors[7][4] = {
  { 0.25, 0.25, 0.25, 1 }, // gray
  { 1   , 1   , 1   , 1 }, // white
  { 0   , 0.5 , 0   , 1 }, // green
  { 1   , 0.5 , 0   , 1 }, // orange
  { 1   , 1   , 0   , 1 }, // yellow
  { 0   , 0   , 1   , 1 }, // blue
  { 1   , 0   , 0   , 1 }, // red
};

Cube cubes[27];

Matrix position;
double mx, my;

int turning;
Vector3 turnAxis;
double turnLayer;
double turnAngle;


void randomTurn() {
  vector3Set(&turnAxis, 0, 0, 0);
  turnAxis.a[randomNumber(0, 2)] = randomNumber(0, 1)*2 - 1;
  turnLayer = randomNumber(-1, 1);
  turnAngle = 0;
  turning = TRUE;
}


void captureMouse(Vector3 *position, double z) {
  vector3Set(position, 0, 0, 100);

  double mx = mouseX();
  double my = mouseY();
  double w = windowGetWidth();
  double h = windowGetHeight();

  mx = mx/w*2 - 1;
  my = 1 - my/h*2;
  Vector4 p0, p1;
  vector4Set(&p0, mx, my, -1, 1);
  vector4Set(&p1, mx, my,  1, 1);

  Matrix back, transform, projection, modelview;
  glGetDoublev(GL_PROJECTION_MATRIX, projection.a);
  glGetDoublev(GL_MODELVIEW_MATRIX, modelview.a);
  matrixMultMatrix(&transform, &modelview, &projection);
  matrixInvert(&back, &transform);

  vector4MultMatrix(&p0, &p0, &back);
  vector4MultMatrix(&p1, &p1, &back);
  if (fabs(p0.w) <= 1e-6 || fabs(p1.w) <= 1e-6)
    return;
  vector4PespectiveDivide(&p0, &p0);
  vector4PespectiveDivide(&p1, &p1);

  double dz = p1.z - p0.z;
  if (fabs(dz) <= 1e-6)
    return;

  double l = (z - p0.z)/dz;
  double x = (p1.x - p0.x)*l + p0.x;
  double y = (p1.y - p0.y)*l + p0.y;

  vector4Set(&p0, x, y, z, 1);
  vector4MultMatrix(&p0, &p0, &transform);
  if (fabs(p0.w) <= 1e-6)
    return;
  vector4PespectiveDivide(&p0, &p0);
  vector3Set(position, x, y, p0.z);
}


void drawCube(Cube *cube) {
  const double s = 0.5;
  const double ss = 0.8*s;
  const double sq2 = sqrt(2);
  const double sq3 = sqrt(3);

  glPushMatrix();
  if (turning && fabs(vector3Dot(&cube->position.ow.xyz, &turnAxis) - turnLayer) < 0.5)
    glRotated(turnAngle, turnAxis.x, turnAxis.y, turnAxis.z);
  glMultMatrixd(cube->position.a);

  for(int i = 0; i < 6; ++i) {
    glColor4dv(colors[cube->colors[i]]);
    glBegin(GL_QUADS);
    glNormal3d(0, 0, -1);
    glVertex3d(-ss, -ss, -s);
    glVertex3d(-ss,  ss, -s);
    glVertex3d( ss,  ss, -s);
    glVertex3d( ss, -ss, -s);
    glEnd();

    glColor4dv(colors[0]);
    glBegin(GL_QUADS);
    glNormal3d(-sq2, 0, -sq2);
    glVertex3d(-s,  -ss, -ss);
    glVertex3d(-s,   ss, -ss);
    glVertex3d(-ss,  ss,  -s);
    glVertex3d(-ss, -ss,  -s);
    glNormal3d(0, -sq2, -sq2);
    glVertex3d(-ss,  -s, -ss);
    glVertex3d(-ss, -ss,  -s);
    glVertex3d( ss, -ss,  -s);
    glVertex3d( ss,  -s, -ss);
    glEnd();

    glBegin(GL_TRIANGLES);
    glNormal3d(-sq3, -sq3, -sq3);
    glVertex3d(-ss, -ss, -s);
    glVertex3d(-ss, -s, -ss);
    glVertex3d(-s, -ss, -ss);
    glEnd();
    if (i == 0) {
      glBegin(GL_TRIANGLES);
      glNormal3d(-sq3, sq3, -sq3);
      glVertex3d(-s, ss, -ss);
      glVertex3d(-ss, s, -ss);
      glVertex3d(-ss, ss, -s);
      glEnd();
    }
    if (i == 1) {
      glBegin(GL_TRIANGLES);
      glNormal3d(sq3, -sq3, -sq3);
      glVertex3d(s, -ss, -ss);
      glVertex3d(ss, -s, -ss);
      glVertex3d(ss, -ss, -s);
      glEnd();
    }

    glRotated(90, i%2, -((i+1)%2), 0);
  }
  glPopMatrix();
}


void init() {
  background(colorByName("black"));

  Cube *c = cubes;
  for(int ix = -1; ix <= 1; ++ix)
  for(int iy = -1; iy <= 1; ++iy)
  for(int iz = -1; iz <= 1; ++iz, ++c) {
    if (iz < 0) c->colors[0] = 0+1;
    if (ix > 0) c->colors[1] = 1+1;
    if (iy > 0) c->colors[2] = 2+1;
    if (iz > 0) c->colors[3] = 3+1;
    if (ix < 0) c->colors[4] = 4+1;
    if (iy < 0) c->colors[5] = 5+1;
    c->position.ox.x = c->position.oy.y = c->position.oz.z = c->position.ow.w = 1;
    c->position.ow.x = ix;
    c->position.ow.y = iy;
    c->position.ow.z = iz;
  }

  matrixSetIdentity(&position);
  randomTurn();
}


void draw() {
  saveState();

  if (mouseDown("right")) {
    double ay = mouseX() - mx;
    double ax = mouseY() - my;
    Matrix rotation;
    matrixSetRotation(&rotation, ax, 1, 0, 0);
    matrixMultMatrix(&position, &position, &rotation);
    matrixSetRotation(&rotation, ay, 0, 1, 0);
    matrixMultMatrix(&position, &position, &rotation);
  }
  if (mouseDown("middle")) {
    double az = mouseX() - mx;
    Matrix rotation;
    matrixSetRotation(&rotation, az, 0, 0, 1);
    matrixMultMatrix(&position, &position, &rotation);
  }
  mx = mouseX(); my = mouseY();

  if (turning) {
    turnAngle += windowGetFrameTime()*90;
    if (turnAngle > 90) {
      turnAngle = 90;
      Matrix turningMatrix;
      matrixSetRotation(&turningMatrix, turnAngle, turnAxis.x, turnAxis.y, turnAxis.z);
      for(int i = 0; i < 27; ++i) {
        if (fabs(vector3Dot(&cubes[i].position.ow.xyz, &turnAxis) - turnLayer) < 0.5)
          matrixMultMatrix(&cubes[i].position, &cubes[i].position, &turningMatrix);
      }
      turning = FALSE;
      randomTurn();
    }
  }

  glClear(GL_DEPTH_BUFFER_BIT);
  glEnable(GL_DEPTH_TEST);
  glEnable(GL_CULL_FACE);
  glEnable(GL_COLOR_MATERIAL);
  glEnable(GL_LIGHTING);
  glEnable(GL_LIGHT0);

  Matrix perspective;
  matrixSetPerspective(&perspective, 100.0, windowGetWidth()/(double)windowGetHeight(), 0.01, 100);
  glMatrixMode(GL_PROJECTION);
  glLoadMatrixd(perspective.a);
  glMatrixMode(GL_MODELVIEW);
  glLoadIdentity();
  glTranslated(0, 0, -5);
  glMultMatrixd(position.a);

  for(int i = 0; i < 27; ++i)
    drawCube(&cubes[i]);

  glDisable(GL_DEPTH_TEST);
  restoreState();
}


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