/****************************************************************************
 * Copyright (C) 2009      Daniel Chappuis                                  *
 ****************************************************************************
 * This file is part of ReactPhysics3D.                                     *
 *                                                                          *
 * ReactPhysics3D is free software: you can redistribute it and/or modify   *
 * it under the terms of the GNU Lesser General Public License as published *
 * by the Free Software Foundation, either version 3 of the License, or     *
 * (at your option) any later version.                                      *
 *                                                                          *
 * ReactPhysics3D is distributed in the hope that it will be useful,        *
 * but WITHOUT ANY WARRANTY; without even the implied warranty of           *
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the             *
 * GNU Lesser General Public License for more details.                      *
 *                                                                          *
 * You should have received a copy of the GNU Lesser General Public License *
 * along with ReactPhysics3D. If not, see <http://www.gnu.org/licenses/>.   *
 ***************************************************************************/

// Libraries
#include "Objects.h"

//#include <windows.h>            // To avoid an error due to the #include <GL/glut.h>
#include <GL/freeglut.h>
#include <GL/gl.h>
#include <GL/glu.h>
#include <cmath>
#include <iostream>

// ----- Class Object ----- //

// Constructor of the class Object
Object::Object(const Vector3D& position, const Quaternion& orientation, double mass, const Matrix3x3& inertiaTensor, OBB* obb)
       :rigidBody(new RigidBody(position, orientation, mass, inertiaTensor, obb)) {

}

// Destructor of the class Object
Object::~Object() {
    // Delete the rigid body object
    delete rigidBody;
}

// Return the pointer to the rigid body
RigidBody* Object::getRigidBody() {
    return rigidBody;
}

// ----- Class Cube ----- //

// Static attributes
const Matrix3x3 Cube::inertiaTensor;

// Constructor of the class Cube
Cube::Cube(const Vector3D& position, const Quaternion& orientation, float size, double mass)
     :Object(position, orientation, mass, Matrix3x3(1.0/12.0*mass*2*size*size, 0.0, 0.0,  
                                        0.0, 1.0/12.0*mass*2*size*size, 0.0,                  
                                        0.0, 0.0, 1.0/12.0*mass*2*size*size), new OBB(position, Vector3D(1.0, 0.0, 0.0), Vector3D(0.0, 1.0, 0.0), Vector3D(0.0, 0.0, 1.0),
                                                                                             size/2.0, size/2.0, size/2)) { // TODO : Construct automatically the OBB and AABB using the computeFromVertices() method
    this->size = size;
}

// Destructor of the classe Cube
Cube::~Cube() {

}

// Method to draw the cube
void Cube::draw() const {

    // Get the interpolated state of the rigid body
    Vector3D position = rigidBody->getInterpolatedPosition();
    Quaternion orientation = rigidBody->getInterpolatedOrientation();

    // Position of the cube
    double x = position.getX();
    double y = position.getY();
    double z = position.getZ();

    // Orientation of the cube
    Vector3D orientationAxis;
    double orientationAngle;
    orientation.getRotationAngleAxis(orientationAngle, orientationAxis);

    // Translation of the cube to its position
    glTranslatef(x, y, z);

    // Rotation of the cube according to its orientation
    glRotatef(orientationAngle/PI*180.0, orientationAxis.getX(), orientationAxis.getY(), orientationAxis.getZ());

    // Draw the cube
    glutSolidCube(size);
}


// ----- Class Plane ----- //

// Constructor of the class Plane
Plane::Plane(const Vector3D& position, const Quaternion& orientation, float width, float height, const Vector3D& d1, const Vector3D& d2, double mass)
      :Object(position, orientation, mass, Matrix3x3(1.0/12.0*mass*height*height, 0.0, 0.0,
                                        0.0, 1.0/12.0*mass*(width*width+height*height), 0.0,
                                        0.0, 0.0, 1.0/12.0*mass*width*width), new OBB(position, Vector3D(1.0, 0.0, 0.0), Vector3D(0.0, 1.0, 0.0), Vector3D(0.0, 0.0, 1.0),
                                                                                             width/2.0, 0.5, height/2.0)) { // TODO : Construct automatically the OBB and AABB using the computeFromVertices() method
    this->width = width;
    this->height = height;
    this->d1 = d1;
    this->d2 = d2;

    // By default Planes in the demo cannot move
    //rigidBody->setIsMotionEnabled(false);

    // Compute the unit normal vector of the plane by a cross product
    normalVector = d1.crossProduct(d2).getUnit();
}

// Destructor of the class Plane
Plane::~Plane() {

}

// Method used to draw the plane
void Plane::draw() const {

       // Get the interpolated state of the rigid body
    Vector3D position = rigidBody->getInterpolatedPosition();
    Quaternion orientation = rigidBody->getInterpolatedOrientation();

    // Position of the cube
    double x = position.getX();
    double y = position.getY();
    double z = position.getZ();

       // Translation of the cube to its position
       glTranslatef(x, y, z);

       float halfWidth = width / 2.0;
       float halfHeight = height / 2.0;

       // Draw the plane
       glBegin(GL_POLYGON);
            glColor3f(1.0, 1.0, 1.0);
            glVertex3f(x + d1.getX() * halfWidth + d2.getX() * halfHeight , y + d1.getY() * halfWidth +  d2.getY() * halfHeight
                        , z + d1.getZ() * halfWidth + d2.getZ() * halfHeight);
            glNormal3f(normalVector.getX(), normalVector.getY(), normalVector.getZ());
            glVertex3f(x + d1.getX() * halfWidth - d2.getX() * halfHeight , y + d1.getY() * halfWidth -  d2.getY() * halfHeight
                        , z + d1.getZ() * halfWidth - d2.getZ() * halfHeight);
            glNormal3f(normalVector.getX(), normalVector.getY(), normalVector.getZ());
            glVertex3f(x - d1.getX() * halfWidth - d2.getX() * halfHeight , y - d1.getY() * halfWidth -  d2.getY() * halfHeight
                        , z - d1.getZ() * halfWidth - d2.getZ() * halfHeight);
            glNormal3f(normalVector.getX(), normalVector.getY(), normalVector.getZ());
            glVertex3f(x - d1.getX() * halfWidth + d2.getX() * halfHeight , y - d1.getY() * halfWidth +  d2.getY() * halfHeight
                        , z - d1.getZ() * halfWidth + d2.getZ() * halfHeight);
       glEnd();
}