#include <GL/gl.h>
#include <GL/glu.h>
#include "SDL.h"

#include "Engine.h"

using namespace std;

#ifdef DEBUG_GL_ERROR
#define checkGLError() checkGLErrorLine(__FUNCTION__, __LINE__)
static void checkGLErrorLine(const char * function, int line)
{
    GLenum err = glGetError();
    if (err != 0)
    {
        std::cerr << "gl error in " << function
            << ": " << err << " (0x" << std::hex << err << ") at line "
            << std::dec << line << std::endl;
    }
}
#else
#define checkGLError()
#endif

#define FP_EQ(a,b) (fabsf(a - b) < 0.0001)

/* used for objects loaded directly from model files */
Engine::Object::Object(bool is_static, bool is_reference, bool enable_blending,
        OdeWorld & world, WFObj * obj, float scale)
    : m_world(world)
{
    m_is_reference = is_reference;
    if (m_is_reference)
    {
        m_ode_object = NULL;
    }
    else
    {
        m_ode_object = world.createObject(is_static, scale);
        m_ode_object->setUserData(this);
    }
    m_is_static = is_static;
    m_enable_blending = enable_blending;
    m_display_list = obj->render(true, enable_blending);
    const float * obj_aabb = obj->getAABB();
    for (int i = 0; i < 6; i++)
        m_aabb[i] = scale * obj_aabb[i];
    m_display_list_refcnt = new int;
    *m_display_list_refcnt = 1;
    m_is_visible = true;
    m_scale = scale;
    m_is_scaled = ! (fabs(scale - 1.0) < 0.0001);
    m_is_managed = false;
    m_mass = 0.0;
    m_mass_is_set = false;
    m_gravity_mode = true;
}

/* used for "managed" objects with one geom */
Engine::Object::Object(bool is_static, bool is_reference, bool enable_blending,
                        OdeWorld & world, OdeWorld::GeomType geom_type,
                        refptr< std::vector<float> > args)
    : m_world(world)
{
    m_is_reference = is_reference;
    m_enable_blending = enable_blending;
    m_is_static = geom_type == OdeWorld::PLANE ? false : is_static;
    m_is_visible = true;
    m_scale = 1.0f;
    m_is_scaled = false;
    m_display_list = 0;
    m_display_list_refcnt = NULL;
    if (m_is_reference)
    {
        m_ode_object = NULL;
    }
    else
    {
        m_ode_object = world.createObject(m_is_static, m_scale);
        m_ode_object->setUserData(this);
    }
    m_is_managed = true;
    m_geom_type = geom_type;
    m_args = args;
    for (int i = 0; i < 4; i++)
        m_color[i] = 1.0f;
    m_texture = 0;
    m_texture_scale = 1.0f;
    createManagedObject();
    render();
    m_mass = 0.0;
    m_mass_is_set = false;
    m_gravity_mode = true;
}

/* used to clone objects */
Engine::Object::Object(const Engine::Object & orig)
    : m_world(orig.m_world)
{
    m_is_reference = false;
    m_is_visible = orig.m_is_visible;
    m_is_static = orig.m_is_static;
    m_enable_blending = orig.m_enable_blending;
    m_scale = orig.m_scale;
    m_is_scaled = orig.m_is_scaled;
    m_phy = orig.m_phy;
    m_is_managed = orig.m_is_managed;
    if (m_is_managed)
        m_display_list = 0;
    else
        m_display_list = orig.m_display_list;
    m_display_list_refcnt = orig.m_display_list_refcnt;
    if (m_display_list_refcnt != NULL)
        (*m_display_list_refcnt)++;
    m_geom_type = orig.m_geom_type;
    m_args = orig.m_args;
    for (int i = 0; i < 4; i++)
        m_color[i] = orig.m_color[i];
    m_texture = orig.m_texture;
    m_texture_scale = orig.m_texture_scale;
    if (orig.m_is_reference)
    {
        m_ode_object = m_world.createObject(m_is_static, m_scale);
        instantiatePhy();
    }
    else
    {
        m_ode_object = new OdeWorld::Object(*orig.m_ode_object);
    }
    m_ode_object->setUserData(this);
    if (m_is_managed)
    {
        createManagedObject();
        render();
    }
    m_mass = orig.m_mass;
    m_mass_is_set = orig.m_mass_is_set;
    if (m_mass_is_set)
    {
        setMass(orig.getMass());
    }
    m_gravity_mode = orig.m_gravity_mode;
    setGravityMode(m_gravity_mode);
}

Engine::Object::~Object()
{
    if (m_ode_object != NULL)
        delete m_ode_object;
    if (m_display_list_refcnt != NULL)
    {
        (*m_display_list_refcnt)--;
        if (*m_display_list_refcnt < 1)
        {
            /* we hold the last reference to the OpenGL display list */
            delete m_display_list_refcnt;
            glDeleteLists(m_display_list, 1);
        }
    }
    else
    {
        glDeleteLists(m_display_list, 1);
    }
}

void Engine::Object::createManagedObject()
{
    switch (m_geom_type)
    {
        case OdeWorld::BOX: {
            while (m_args->size() < 9)
                m_args->push_back(0);
            if (m_ode_object != NULL)
                m_ode_object->addBox(m_args);
            float aabb[6] = {-(*m_args)[0], -(*m_args)[1], -(*m_args)[2],
                             (*m_args)[0], (*m_args)[1], (*m_args)[2]};
            for (int i = 0; i < 6; i++)
                aabb[i] /= 2.0f;
            memcpy(m_aabb, aabb, sizeof(m_aabb));
            break;
        }
        case OdeWorld::SPHERE: {
            while (m_args->size() < 4)
                m_args->push_back(0);
            if (m_ode_object != NULL)
                m_ode_object->addSphere(m_args);
            float aabb[6] = {-(*m_args)[0], -(*m_args)[0], -(*m_args)[0],
                             (*m_args)[0], (*m_args)[0], (*m_args)[0]};
            memcpy(m_aabb, aabb, sizeof(m_aabb));
            break;
        }
        case OdeWorld::PLANE: {
            while (m_args->size() < 4 || m_args->size() == 5)
                m_args->push_back(0);
            if (m_ode_object != NULL)
                m_ode_object->addPlane(m_args);
            for (int i = 0; i < 6; i++)
                m_aabb[i] = 0.0f;
            break;
        }
        case OdeWorld::CYLINDER: {
            while (m_args->size() < 8)
                m_args->push_back(0);
            if (m_ode_object != NULL)
                m_ode_object->addCylinder(m_args);
            float aabb[6] = {-(*m_args)[0], -(*m_args)[0], -(*m_args)[1],
                             (*m_args)[0], (*m_args)[0], (*m_args)[1]};
            memcpy(m_aabb, aabb, sizeof(m_aabb));
            break;
        }
        case OdeWorld::CAPSULE: {
            while (m_args->size() < 8)
                m_args->push_back(0);
            if (m_ode_object != NULL)
                m_ode_object->addCapsule(m_args);
            float aabb[6] = {-(*m_args)[0], -(*m_args)[0], -(*m_args)[1],
                             (*m_args)[0], (*m_args)[0], (*m_args)[1]};
            memcpy(m_aabb, aabb, sizeof(m_aabb));
            break;
        }
    }
    if (m_ode_object != NULL)
    {
        m_ode_object->finalize();
    }
    if (!m_is_reference)
    {
        render();
    }
}

/* render a managed object */
/* prerequisite: m_args->size() is big enough */
void Engine::Object::render()
{
    if (!m_is_managed || m_is_reference)
        return;
    GLUquadric * quad = gluNewQuadric();
    if (m_display_list <= 0)
    {
        m_display_list = glGenLists(1);
    }
    checkGLError();
    glNewList(m_display_list, GL_COMPILE);
    int attrib_flags = GL_ENABLE_BIT;
    if (m_enable_blending)
    {
        attrib_flags |= GL_COLOR_BUFFER_BIT | GL_TEXTURE_BIT;
    }
    glPushAttrib(attrib_flags);
    if (m_enable_blending)
    {
        glEnable(GL_BLEND);
        glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
        glTexEnvi(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_REPLACE);
    }
    if (m_texture != 0)
    {
        gluQuadricTexture(quad, 1);
        glEnable(GL_TEXTURE_2D);
        glBindTexture(GL_TEXTURE_2D, m_texture);
    }
    else
    {
        glDisable(GL_TEXTURE_2D);
        glMaterialfv(GL_FRONT, GL_AMBIENT_AND_DIFFUSE, m_color);
    }
    switch (m_geom_type)
    {
        case OdeWorld::BOX: {
                const static struct {
                    float norm[3];
                    float verts[4][3];
                } sides[] = {
                    { {1, 0, 0},        /* right */
                        {{1, -1, -1}, {1, 1, -1}, {1, 1, 1}, {1, -1, 1}} },
                    { {-1, 0, 0},       /* left */
                        {{-1, 1, -1}, {-1, -1, -1}, {-1, -1, 1}, {-1, 1, 1}} },
                    { {0, 1, 0},        /* back */
                        {{1, 1, -1}, {-1, 1, -1}, {-1, 1, 1}, {1, 1, 1}} },
                    { {0, -1, 0},       /* front */
                        {{-1, -1, -1}, {1, -1, -1}, {1, -1, 1}, {-1, -1, 1}} },
                    { {0, 0, 1},        /* top */
                        {{-1, -1, 1}, {1, -1, 1}, {1, 1, 1}, {-1, 1, 1}} },
                    { {0, 0, -1},       /* bottom */
                        {{1, -1, -1}, {-1, -1, -1}, {-1, 1, -1}, {1, 1, -1}} }
                };
                const float tex_coords[4][2] = {
                    {0, 0}, {1, 0}, {1, 1}, {0, 1}
                };
                glBegin(GL_QUADS);
                float width = (*m_args)[0];
                float depth = (*m_args)[1];
                float height = (*m_args)[2];
                for (unsigned int s = 0;
                        s < (sizeof(sides)/sizeof(sides[0]));
                        s++)
                {
                    glNormal3fv(sides[s].norm);
                    for (int v = 0; v < 4; v++)
                    {
                        if (m_texture != 0)
                        {
                            glTexCoord2f(tex_coords[v][0] * m_texture_scale,
                                    tex_coords[v][1] * m_texture_scale);
                        }
                        glVertex3f(sides[s].verts[v][0] * width / 2.0,
                                sides[s].verts[v][1] * depth / 2.0,
                                sides[s].verts[v][2] * height / 2.0);
                    }
                }
                glEnd();
            }
            break;
        case OdeWorld::SPHERE:
            gluSphere(quad, (*m_args)[0], 16, 8);
            break;
        case OdeWorld::PLANE: {
            dVector3 normal;
            dVector3 rotated_x;
            dVector3 rotated_y;
            float x_o, y_o, z_o;
            if (m_args->size() == 6)
            {
                dMatrix3 r;
                dRFromEulerAngles(r, (*m_args)[3], (*m_args)[4], (*m_args)[5]);
                dVector3 default_plane_direction = {0, 0, 1, 0};
                dVector3 default_x = {1, 0, 0, 0};
                dVector3 default_y = {0, 1, 0, 0};
                dMultiply0(normal, r, default_plane_direction, 3, 3, 1);
                dMultiply0(rotated_x, r, default_x, 3, 3, 1);
                dMultiply0(rotated_y, r, default_y, 3, 3, 1);
                x_o = (*m_args)[0];
                y_o = (*m_args)[1];
                z_o = (*m_args)[2];
            }
            else
            {
                normal[0] = (*m_args)[0];
                normal[1] = (*m_args)[1];
                normal[2] = (*m_args)[2];
                float d = (*m_args)[3];
                float len = normal[0] * normal[0]
                            + normal[1] * normal[1]
                            + normal[2] * normal[2];
                if (! FP_EQ(len, 1.0))          /* normalize if necessary */
                {
                    len = sqrtf(len);
                    normal[0] /= len;
                    normal[1] /= len;
                    normal[2] /= len;
                    d /= len;
                }
                int min_component = 0;
                float comp = 10000.0f;
                for (int i = 0; i < 3; i++)
                {
                    if (normal[i] < comp)
                    {
                        min_component = i;
                        comp = normal[i];
                    }
                }
                dVector3 cross_vec;
                for (int i = 0; i < 3; i++)
                    cross_vec[i] = (i == min_component) ? 1 : 0;
                dCROSS(rotated_x, =, normal, cross_vec);
                dCROSS(rotated_y, =, normal, rotated_x);
                x_o = normal[0] * d;
                y_o = normal[1] * d;
                z_o = normal[2] * d;
            }

            /* plane equation: ax + by + cz = d, plane normal: (a, b, c) */

            const int num_sections = 10;
            const float section_size = 100.0f;
#ifdef dSINGLE
            glNormal3fv(normal);
#else
            glNormal3dv(normal);
#endif
            for (int x = 0; x < num_sections; x++)
            {
                glBegin(GL_QUAD_STRIP);
                for (int y = 0; y <= num_sections; y++)
                {
                    float half_span = num_sections * section_size / 2.0;
                    float x_c = x * section_size - half_span;
                    float y_c = y * section_size - half_span;
                    glTexCoord2f(0.0, y * m_texture_scale);
                    glVertex3f(x_o + rotated_x[0] * x_c + rotated_y[0] * y_c,
                            y_o + rotated_x[1] * x_c + rotated_y[1] * y_c,
                            z_o + rotated_x[2] * x_c + rotated_y[2] * y_c);
                    x_c += section_size;
                    glTexCoord2f(1.0, y * m_texture_scale);
                    glVertex3f(x_o + rotated_x[0] * x_c + rotated_y[0] * y_c,
                            y_o + rotated_x[1] * x_c + rotated_y[1] * y_c,
                            z_o + rotated_x[2] * x_c + rotated_y[2] * y_c);
                }
                glEnd();
            }
            }
            break;
        case OdeWorld::CYLINDER: {
            glPushMatrix();
            float half_span = (*m_args)[1] / 2.0;
            glTranslatef(0, 0, -half_span);
            gluCylinder(quad, (*m_args)[0], (*m_args)[0], (*m_args)[1], 12, 1);
            gluDisk(quad, 0.0, (*m_args)[0], 12, 4);
            glTranslatef(0, 0, 2 * half_span);
            gluDisk(quad, 0.0, (*m_args)[0], 12, 4);
            glPopMatrix();
            }
            break;
        case OdeWorld::CAPSULE: {
            glPushAttrib(GL_TRANSFORM_BIT);      /* save current clip planes */
            double halfheight = (*m_args)[1] / 2.0;
            glPushMatrix();
            glTranslatef(0, 0, -halfheight);
            gluCylinder(quad, (*m_args)[0], (*m_args)[0], (*m_args)[1], 12, 1);

            double clip_plane[] = {0, 0, -1, halfheight};
            glClipPlane(GL_CLIP_PLANE0, clip_plane);
            glEnable(GL_CLIP_PLANE0);
            gluSphere(quad, (*m_args)[0], 12, 8);
            glTranslatef(0, 0, 2*halfheight);
            clip_plane[2] = 1.0;
            glClipPlane(GL_CLIP_PLANE0, clip_plane);
            gluSphere(quad, (*m_args)[0], 12, 8);
            glPopMatrix();
            glPopAttrib();
            }
            break;
    }
    gluDeleteQuadric(quad);
    glPopAttrib();
    glEndList();
    checkGLError();
}

void Engine::Object::setPosition(double x, double y, double z)
{
    if (m_ode_object != NULL)
        m_ode_object->setPosition(x, y, z);
}

void Engine::Object::getPosition(double * x, double * y, double * z)
{
    if (m_ode_object != NULL)
        m_ode_object->getPosition(x, y, z);
    else
        *x = *y = *z = 0.0;
}

void Engine::Object::loadPhy(FileLoader * fl, const FileLoader::Path & path)
{
    m_phy = new PhyObj();
    m_phy->load(fl, path);
    if (!m_is_managed)
        instantiatePhy();
}

void Engine::Object::instantiatePhy()
{
    if (m_ode_object != NULL && !m_is_managed && !m_phy.isNull())
    {
        for (int i = 0, sz = m_phy->getNumGeoms(); i < sz; i++)
        {
            refptr<PhyObj::Geom> geom = m_phy->getGeom(i);
            refptr< vector<float> > args = geom->getArgs();
            switch (geom->getType())
            {
                case PhyObj::BOX:
                    m_ode_object->addBox(args);
                    break;
                case PhyObj::SPHERE:
                    m_ode_object->addSphere(args);
                    break;
                case PhyObj::CAPSULE:
                    m_ode_object->addCapsule(args);
                    break;
                case PhyObj::PLANE:
                    m_ode_object->addPlane(args);
                    break;
                default:
                    break;
            }
        }
        m_ode_object->finalize();
    }
}

void Engine::Object::setTexture(GLuint tex)
{
    if (m_is_managed)
    {
        m_texture = tex;
        render();
    }
}

void Engine::Object::draw()
{
    if (!m_is_reference && m_is_visible)
    {
        checkGLError();
        const dReal * pos = m_ode_object->getPosition();
        const dReal * rot = m_ode_object->getRotation();
        bool transform = (pos != NULL && rot != NULL);

        if (transform)
            OdeWorld::pushTransform(pos, rot);

        if (m_is_scaled)
        {
            glPushAttrib(GL_TRANSFORM_BIT);
            glEnable(GL_NORMALIZE);
            glPushMatrix();
            glScalef(m_scale, m_scale, m_scale);
        }

        checkGLError();
        glCallList(m_display_list);
        checkGLError();

        if (m_is_scaled)
        {
            glPopMatrix();
            glPopAttrib();
        }

        if (transform)
            glPopMatrix();
        checkGLError();
    }
}

dReal Engine::Object::getMass() const
{
    return (m_ode_object != NULL) ? m_ode_object->getMass() : m_mass;
}

void Engine::Object::setMass(dReal mass)
{
    m_mass = mass;
    m_mass_is_set = true;
    if (m_ode_object != NULL)
    {
        m_ode_object->setMass(mass);
    }
}