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

#include <gl gl.h=""></gl>

#include <helianthus.h></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();

}