#include <GL/gl.h>
#include <GL/glu.h>
#include <GL/glut.h>

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

#include "robot_main.h"
#include "robot_bodypart.h"
#include "robot_matrix.h"
#include "robot_vector.h"


// Robot initialization
// ====================

#define jsz1 0.45    // joint size 1
#define jsz2 0.35    // joint size 2

Cbodypart torso(PRIMITIVE_BOX, Cvector(0.0, 0.0, 1.0), Cvector(2.0, 4.0, 1.0), Cvector(0.0, 0.0, 0.0), Cvector(0.0, 0.0, 0.0), Cvector(0.0, 0.0, 0.0));

Cbodypart left_shoulder_joint(PRIMITIVE_SPHERE, Cvector(1.0, 1.0, 1.0), Cvector(jsz1, jsz1, jsz1), Cvector(-1.0, 2.0, 0.0), Cvector(0.0, 0.0, 0.0), Cvector(0.0, 0.0, 0.0));
Cbodypart left_shoulder(PRIMITIVE_BOX, Cvector(1.0, 0.0, 0.0), Cvector(0.5, 1.5, 0.5), Cvector(-0.25, 0.0, 0.0), Cvector(0.0, 0.0, 0.0), Cvector(0.0, -0.75, 0.0));

Cbodypart left_arm_joint(PRIMITIVE_SPHERE, Cvector(1.0, 1.0, 1.0), Cvector(jsz2, jsz2, jsz2), Cvector(0.0, -0.75, 0.0), Cvector(0.0, 0.0, 0.0), Cvector(0.0, 0.0, 0.0));
Cbodypart left_arm(PRIMITIVE_BOX, Cvector(1.0, 0.0, 0.0), Cvector(0.5, 1.5, 0.5), Cvector(0.0, 0.0, 0.0), Cvector(0.0, 0.0, 0.0), Cvector(0.0, -0.75, 0.0));
Cbodypart left_hand(PRIMITIVE_CONE, Cvector(1.0, 1.0, 0.0), Cvector(0.5, 0.5, 1.5), Cvector(0.0, -0.5, 0.5), Cvector(90.0, 0.0, 0.0), Cvector(0.0, -0.5, 0.0));


Cbodypart right_shoulder_joint(PRIMITIVE_SPHERE, Cvector(1.0, 1.0, 1.0), Cvector(jsz1, jsz1, jsz1), Cvector(1.0, 2.0, 0.0), Cvector(0.0, 0.0, 0.0), Cvector(0.0, 0.0, 0.0));
Cbodypart right_shoulder(PRIMITIVE_BOX, Cvector(1.0, 0.0, 0.0), Cvector(0.5, 1.5, 0.5), Cvector(0.25, 0.0, 0.0), Cvector(0.0, 0.0, 0.0), Cvector(0.0, -0.75, 0.0));

Cbodypart right_arm_joint(PRIMITIVE_SPHERE, Cvector(1.0, 1.0, 1.0), Cvector(jsz2, jsz2, jsz2), Cvector(0.0, -0.75, 0.0), Cvector(0.0, 0.0, 0.0), Cvector(0.0, 0.0, 0.0));
Cbodypart right_arm(PRIMITIVE_BOX, Cvector(1.0, 0.0, 0.0), Cvector(0.5, 1.5, 0.5), Cvector(0.0, 0.0, 0.0), Cvector(0.0, 0.0, 0.0), Cvector(0.0, -0.75, 0.0));
Cbodypart right_hand(PRIMITIVE_CONE, Cvector(1.0, 1.0, 0.0), Cvector(0.5, 0.5, 1.5), Cvector(0.0, -0.5, 0.5), Cvector(90.0, 0.0, 0.0), Cvector(0.0, -0.5, 0.0));


Cbodypart left_thigh_joint(PRIMITIVE_SPHERE, Cvector(1.0, 1.0, 1.0), Cvector(jsz1, jsz1, jsz1), Cvector(-1.0, -2.0, 0.0), Cvector(0.0, 0.0, 0.0), Cvector(0.0, 0.0, 0.0));
Cbodypart left_thigh(PRIMITIVE_BOX, Cvector(1.0, 0.0, 0.0), Cvector(0.5, 2.0, 0.5), Cvector(-0.25, 0.0, 0.0), Cvector(0.0, 0.0, 0.0), Cvector(0.0, -1.0, 0.0));

Cbodypart left_leg_joint(PRIMITIVE_SPHERE, Cvector(1.0, 1.0, 1.0), Cvector(jsz2, jsz2, jsz2), Cvector(0.0, -1.0, 0.0), Cvector(0.0, 0.0, 0.0), Cvector(0.0, 0.0, 0.0));
Cbodypart left_leg(PRIMITIVE_BOX, Cvector(1.0, 0.0, 0.0), Cvector(0.5, 2.0, 0.5), Cvector(0.0, 0.0, 0.0), Cvector(0.0, 0.0, 0.0), Cvector(0.0, -1.0, 0.0));
Cbodypart left_foot(PRIMITIVE_TORUS, Cvector(1.0, 1.0, 0.0), Cvector(0.5, 0.5, 1.0), Cvector(0.0, -0.5, 0.5), Cvector(90.0, 0.0, 0.0), Cvector(0.0, -0.5, 0.0));


Cbodypart right_thigh_joint(PRIMITIVE_SPHERE, Cvector(1.0, 1.0, 1.0), Cvector(jsz1, jsz1, jsz1), Cvector(1.0, -2.0, 0.0), Cvector(0.0, 0.0, 0.0), Cvector(0.0, 0.0, 0.0));
Cbodypart right_thigh(PRIMITIVE_BOX, Cvector(1.0, 0.0, 0.0), Cvector(0.5, 2.0, 0.5), Cvector(0.25, 0.0, 0.0), Cvector(0.0, 0.0, 0.0), Cvector(0.0, -1.0, 0.0));

Cbodypart right_leg_joint(PRIMITIVE_SPHERE, Cvector(1.0, 1.0, 1.0), Cvector(jsz2, jsz2, jsz2), Cvector(0.0, -1.0, 0.0), Cvector(0.0, 0.0, 0.0), Cvector(0.0 ,0.0 ,0.0));
Cbodypart right_leg(PRIMITIVE_BOX, Cvector(1.0, 0.0, 0.0), Cvector(0.5, 2.0, 0.5), Cvector(0.0, 0.0, 0.0), Cvector(0.0, 0.0, 0.0), Cvector(0.0 ,-1.0, 0.0));
Cbodypart right_foot(PRIMITIVE_TORUS, Cvector(1.0, 1.0, 0.0), Cvector(0.5, 0.5, 1.0), Cvector(0.0, -0.5, 0.5), Cvector(90.0, 0.0, 0.0), Cvector(0.0, -0.5, 0.0));


Cbodypart head(PRIMITIVE_TEAPOT, Cvector(0.0, 1.0, 0.0), Cvector(1.0, 1.5, 1.0), Cvector(0.0, 2.0, 0.0), Cvector(0.0, 90.0, 0.0), Cvector(0.0, 0.75, 0.0));
Cbodypart eye_left(PRIMITIVE_SPHERE, Cvector(0.5, 0.25, 1.0), Cvector(0.25, 0.25, 0.25), Cvector(0.65, 0.5, 0.5), Cvector(0.0, 90.0, 0.0), Cvector(0.0, 0.0, 0.0));
Cbodypart eye_right(PRIMITIVE_SPHERE, Cvector(0.5, 0.25, 1.0), Cvector(0.25, 0.25, 0.25), Cvector(0.65, 0.5, -0.5), Cvector(0.0, 90.0, 0.0), Cvector(0.0, 0.0, 0.0));


// Lights Initialiaztion
// =====================

GLfloat LightAmbient[] = {0.5f, 0.5f, 0.5f, 1.0f};
GLfloat LightDiffuse[] = {1.0f, 1.0f, 1.0f, 1.0f};
GLfloat LightPosition[] = {0.0f, 0.0f, 4.0f, 1.0f};

// Display lists
// =============

GLuint box_solid;
GLuint box_wire;
GLuint sphere_solid;
GLuint sphere_wire;
GLuint cone_solid;
GLuint cone_wire;
GLuint torus_solid;
GLuint torus_wire;
GLuint teapot_solid;
GLuint teapot_wire;

int nframe = 0;         // number of current frame running
bool solid = true;      // solid polygons or wireframe?
bool tmode = true;      // teapot mode ON/OFF (goes too slow with too many robots)

void drawBodypart(Cbodypart bp)
{
    if (solid)
    {
        glColorMaterial(GL_FRONT, GL_AMBIENT);
        glColor3f(0.0f, 0.0f, 0.0f);
        glColorMaterial(GL_FRONT, GL_DIFFUSE);
        glColor3f(bp.color_r, bp.color_g, bp.color_b);
    }

    bp.InitTransformationMatrix();
    glMultMatrixf(bp.transformation_matrix.GetGLmatrixPointer());

    glPushMatrix();
    glMultMatrixf(bp.scale_matrix.GetGLmatrixPointer());

    int shape_type = bp.shape_type;

    if (!tmode)
    {
        if (shape_type == PRIMITIVE_TEAPOT)
            shape_type = PRIMITIVE_SPHERE;
    }

    switch(shape_type)
    {
        case PRIMITIVE_BOX:
            if (solid)
                glCallList(box_solid);
            else
                glCallList(box_wire);
        break;

        case PRIMITIVE_SPHERE:
            if (solid)
                glCallList(sphere_solid);
            else
                glCallList(sphere_wire);
        break;
                    
        case PRIMITIVE_CONE:
            if (solid)
                glCallList(cone_solid);
            else
                glCallList(cone_wire);
        break;
        
        case PRIMITIVE_TORUS:
            if (solid)
                glCallList(torus_solid);
            else
                glCallList(torus_wire);
        break;
        
        case PRIMITIVE_TEAPOT:
            if (solid)
                glCallList(teapot_solid);
            else
                glCallList(teapot_wire);
        break;
        
        default:
        break;
    }

    glPopMatrix();
}

void drawRobot()
{
    glMatrixMode(GL_MODELVIEW);

    drawBodypart(torso);

    glPushMatrix();
        drawBodypart(left_shoulder_joint);
        drawBodypart(left_shoulder);

        drawBodypart(left_arm_joint);
        drawBodypart(left_arm);
        drawBodypart(left_hand);
    glPopMatrix();

    glPushMatrix();
        drawBodypart(right_shoulder_joint);
        drawBodypart(right_shoulder);

        drawBodypart(right_arm_joint);
        drawBodypart(right_arm);
        drawBodypart(right_hand);
    glPopMatrix();


    glPushMatrix();
        drawBodypart(left_thigh_joint);
        drawBodypart(left_thigh);

        drawBodypart(left_leg_joint);
        drawBodypart(left_leg);
        drawBodypart(left_foot);
    glPopMatrix();

    glPushMatrix();
        drawBodypart(right_thigh_joint);
        drawBodypart(right_thigh);

        drawBodypart(right_leg_joint);
        drawBodypart(right_leg);
        drawBodypart(right_foot);
    glPopMatrix();

    glPushMatrix();
        drawBodypart(head);

        glPushMatrix();
            drawBodypart(eye_left);
        glPopMatrix();

        glPushMatrix();
            drawBodypart(eye_right);
        glPopMatrix();
    glPopMatrix();
}




void drawGround(float x_anim, float z_anim)
{
    float floor_y = -18.0;

    int ground_step_x = 4;
    int ground_step_z = 4;

    int ground_width = (int)(128 * ground_step_x);
    int ground_height = (int)(128 * ground_step_z);

    float ground_y[ground_width][ground_height];

    int half_height = (int)(ground_height / 2.0);
    int half_width = (int)(ground_width / 2.0);

    int i, j;

    j = 0;
    for (int z=-half_height; z<half_height; z+=ground_step_z)
    {
        i = 0;
        for (int x=-half_width; x<half_width; x+=ground_step_x)
        {
            float xp = x + x_anim;
            float zp = z + z_anim;
            ground_y[i % ground_width][j % ground_height] = floor_y + sin(xp / 8.0) * 3.0 + sin((xp + zp) / 5.0) * 2.0 + sin((xp + sin((xp - zp) / 4.0) * 3.0) / 7.0) * 1.5;
            i++;
        }
        j++;
    }

    glBegin(GL_QUADS);

    j = 0;
    for (int z=-half_height; z<half_height; z+=ground_step_z)
    {
        i = 0;
        for (int x=-half_width; x<half_width; x+=ground_step_x)
        {
            float d = sqrt(x*x + z*z) * 0.01;
            float r = 0.5 / d;
            float g = 0.25 / d;
            float b = 0.15 / d;
            if (r > 1.0) r = 1.0;
            if (g > 1.0) g = 1.0;
            if (b > 1.0) b = 1.0;
            glColor3f(r, g, b);

            float g01 = ground_y[i][j];
            float g11 = ground_y[(i+1) % ground_width][j];
            float g10 = ground_y[(i+1) % ground_width][(j+1) % ground_height];
            float g00 = ground_y[i][(j+1) % ground_height];

            float dx0 = (float)(fabs(g10 - g00) * 0.05);
            float dx1 = (float)(fabs(g11 - g01) * 0.05);
            float dy0 = (float)(fabs(g01 - g00) * 0.05);
            float dy1 = (float)(fabs(g11 - g10) * 0.05);

            glNormal3f(dx1, 1.0, dy0);
			glVertex3f(x, g00, z + ground_step_z);

            glNormal3f(dx1, 1.0, dy1);
			glVertex3f(x + ground_step_x, g10, z + ground_step_z);

            glNormal3f(dx0, 1.0, dy1);
			glVertex3f(x + ground_step_x, g11, z);

            glNormal3f(dx0, 1.0, dy0);
			glVertex3f(x, g01, z);
			i++;
        }
        j++;
    }
    glEnd();
}



float rx = 0.0, ry = 180.0, rz = 0.0;
float anim_x = 0.0, anim_z = 0.0;
float zoom = 0.0;

int rnum = 1;

void display()
{
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    glLoadIdentity();

    gluLookAt(  0.0, 0.0, 64.0 + zoom,
                0.0, 0.0, 0.0,
                0.0, 1.0, 0.0);

    glRotatef(rx, 1.0, 0.0, 0.0);
    glRotatef(ry, 0.0, 1.0, 0.0);
    glRotatef(rz, 0.0, 0.0, 1.0);

    drawGround(anim_x, anim_z);

    float space = 16.0f;
    for (int j=-rnum; j<=rnum; j++)
    {
        for (int i=-rnum; i<=rnum; i++)
        {
            float xp = 256 * i + anim_x;
            float zp = 256 * j + anim_z;
            float zz = sin(xp / 8.0) * 3.0 + sin((xp + zp) / 5.0) * 2.0 + sin((xp + sin((xp - zp) / 4.0) * 3.0) / 7.0) * 1.5;

            glPushMatrix();
            glTranslatef(i * space, 0.0f + zz, j * space);
            glScalef(3.0, 3.0, 3.0);
            drawRobot();
            glPopMatrix();
        }
    }

    glutSwapBuffers();
}

bool plane_anim = true;

void animate(float anim)
{
    if (plane_anim)
    {    
        anim_x = sin(anim / 2.0) * 6.0;
        anim_z = 12.0 * anim;
    }

    float an0 = sin(anim) * 45;
    float an1 = sin(anim*1.5) * 35;
    float an2 = sin(anim*0.75) * 75;
    float an3 = sin(anim*2.5) * 25;

    left_shoulder.rotate_x = an0;
    left_arm.rotate_x = an1;
    if (left_arm.rotate_x < 0) left_arm.rotate_x = 0;

    right_shoulder.rotate_x = an2;
    right_arm.rotate_x = an3;
    if (right_arm.rotate_x < 0) right_arm.rotate_x = 0;

    left_thigh.rotate_x = an0;
    left_leg.rotate_x = an2;
    if (left_leg.rotate_x > 0) left_leg.rotate_x = 0;

    right_thigh.rotate_x = an1;
    right_leg.rotate_x = an3;
    if (right_leg.rotate_x > 0) right_leg.rotate_x = 0;

    display();
}


void keyboard_actions(unsigned char key, int x, int y)
{
    if (key==27)
        exit (0);

    if (key=='w' || key=='W')
        solid = !solid;

    if (key=='a' || key=='A')
        plane_anim = !plane_anim;

    if (key=='t' || key=='T')
        tmode = !tmode;

    if (key=='+')
    {
        rnum++;
        if (rnum > 4) rnum = 4;
    }

    if (key=='-')
    {
        rnum--;
        if (rnum < 0) rnum = 0;
    }
}


int previous_mouse_x = 0;
int previous_mouse_y = 0;
int button_type = -1;

void mouse_movement(int x, int y)
{
    previous_mouse_x = x;
    previous_mouse_y = y;
}

void mouse_action_movement(int x, int y)
{
    if (button_type == GLUT_LEFT_BUTTON)
    {
        rx += (y - previous_mouse_y) / 4.0;
        ry += (x - previous_mouse_x) / 4.0;

        if (rx < 0) rx = 0;
        if (rx > 180) rx = 180;
    }

    if (button_type == GLUT_RIGHT_BUTTON)
    {
        zoom += (y - previous_mouse_y) / 8.0;
        if (zoom < -32.0) zoom = -32.0;
        if (zoom > 64.0) zoom = 64.0;
    }

    previous_mouse_x = x;
    previous_mouse_y = y;
}

void mouse_button_actions(int button, int state, int x, int y)
{
    button_type = button;
}

void timer(int value)
{
    animate((float)nframe / 5.0);
    glutTimerFunc(1000 / 50, timer, 1); // 50fps
    nframe++;
}

void initGlut()
{
    glutInitWindowSize(ScreenWidth, ScreenHeight); 
    glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH);
    glutCreateWindow("Walking Dancing Teabots");

    glutDisplayFunc(display);
    glutKeyboardFunc(keyboard_actions);
    glutMotionFunc(mouse_action_movement);
    glutPassiveMotionFunc(mouse_movement);
    glutMouseFunc(mouse_button_actions);
    glutTimerFunc(1000 / 50, timer, 1); // 50fps
}


void initDisplayLists()
{
    box_solid=glGenLists(1);
    box_wire=glGenLists(1);

    glNewList(box_solid, GL_COMPILE);
        glutSolidCube(1);
    glEndList();

    glNewList(box_wire, GL_COMPILE);
        glutWireCube(1);
    glEndList();


    sphere_solid=glGenLists(1);
    sphere_wire=glGenLists(1);

    glNewList(sphere_solid, GL_COMPILE);
        glutSolidSphere(1, 16, 16);
    glEndList();

    glNewList(sphere_wire, GL_COMPILE);
        glutWireSphere(1, 16, 16);
    glEndList();


    cone_solid=glGenLists(1);
    cone_wire=glGenLists(1);

    glNewList(cone_solid, GL_COMPILE);
        glutSolidCone(1, 1, 32, 1);
    glEndList();

    glNewList(cone_wire, GL_COMPILE);
        glutWireCone(1, 1, 16, 8);
    glEndList();


    torus_solid=glGenLists(1);
    torus_wire=glGenLists(1);

    glNewList(torus_solid, GL_COMPILE);
        glutSolidTorus(0.5, 1, 32, 64);
    glEndList();

    glNewList(torus_wire, GL_COMPILE);
        glutWireTorus(0.5, 1, 16, 32);
    glEndList();


    teapot_solid=glGenLists(1);
    teapot_wire=glGenLists(1);

    glNewList(teapot_solid, GL_COMPILE);
        glutSolidTeapot(1);
    glEndList();

    glNewList(teapot_wire, GL_COMPILE);
        glutWireTeapot(1);
    glEndList();
}

void initGl()
{
    glMatrixMode(GL_PROJECTION);
    gluPerspective(64.0, ScreenWidth/ScreenHeight, 1.0, 4096.0);

    glMatrixMode(GL_MODELVIEW);
    glLoadIdentity();

    glEnable(GL_DEPTH_TEST);
	glEnable(GL_TEXTURE_2D);
    glShadeModel(GL_SMOOTH);
    glEnable(GL_LIGHTING);
    glEnable(GL_COLOR_MATERIAL);

    glLightfv(GL_LIGHT1, GL_AMBIENT, LightAmbient);
	glLightfv(GL_LIGHT1, GL_DIFFUSE, LightDiffuse);
	glLightfv(GL_LIGHT1, GL_POSITION,LightPosition);
	glEnable(GL_LIGHT1);
}


int main(int argc, char **argv)
{
	glutInit(&argc, argv);

    printf("Press the left mouse button and move to rotate the camera\n");
    printf("Press the right mouse button and move to zoom in/out the scene\n");
    printf("\nKeys: \t'W' for wireframe, 'A' to start/stop animation, 'T' for teapot mode\n\t '+/-' to increase/reduce the robot population\n\n");

    initGlut();
    initGl();
    initDisplayLists();

    glutMainLoop();
    return 0;
}
