/*************************************************************************
 *                                                                       *
 * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.       *
 * All rights reserved.  Email: russ@q12.org   Web: www.q12.org          *
 *                                                                       *
 * This library is free software; you can redistribute it and/or         *
 * modify it under the terms of EITHER:                                  *
 *   (1) The GNU Lesser General Public License as published by the Free  *
 *       Software Foundation; either version 2.1 of the License, or (at  *
 *       your option) any later version. The text of the GNU Lesser      *
 *       General Public License is included with this library in the     *
 *       file LICENSE.TXT.                                               *
 *   (2) The BSD-style license that is included with this library in     *
 *       the file LICENSE-BSD.TXT.                                       *
 *                                                                       *
 * This library 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 files    *
 * LICENSE.TXT and LICENSE-BSD.TXT for more details.                     *
 *                                                                       *
 *************************************************************************/

//#include <iostream>
#include <ode/ode.h>
#include <drawstuff/drawstuff.h>
#include "texturepath.h"

#ifdef dDOUBLE
#define dsDrawSphere dsDrawSphereD
#define dsDrawBox dsDrawBoxD
#define dsDrawTriangle dsDrawTriangleD
#define dsDrawLine dsDrawLineD
#endif



const dReal ball_radius = 0.4;
const dReal balls_sep = 2; // separation between the balls

/* Choose one test case
 */
#define TEST_CASE 0

#if TEST_CASE == 0
const dReal track_len = 10;
const dReal track_height = 1;
const dReal track_width = 0.1;
const dReal track_gauge = 1;
const dReal track_elevation = 2;
const dReal track_angle = 80 * M_PI/180.;
const dReal track_incl = 10 * M_PI/180.;
#elif TEST_CASE == 1
const dReal track_len = 10;
const dReal track_height = 1;
const dReal track_width = 0.1;
const dReal track_gauge = 1.9*ball_radius;
const dReal track_elevation = 2;
const dReal track_angle = 0 * M_PI/180.;
const dReal track_incl = 10 * M_PI/180.;
#elif TEST_CASE == 2
const dReal track_len = 10;
const dReal track_height = 1;
const dReal track_width = 0.1;
const dReal track_gauge = 1.9*ball_radius;
const dReal track_elevation = 2;
const dReal track_angle = 15 * M_PI/180.;
const dReal track_incl = 10 * M_PI/180.;
#elif TEST_CASE == 3
const dReal track_len = 10;
const dReal track_height = .7;
const dReal track_width = 0.1;
const dReal track_gauge = track_height*1.1;
const dReal track_elevation = 2;
const dReal track_angle = 90 * M_PI/180.;
const dReal track_incl = 10 * M_PI/180.;
#else
#error "TEST_CAST to a valid value!"
#endif



dWorldID world;
dSpaceID space;
dJointGroupID contact_group;
dGeomID ground;
dGeomID ball1_geom, ball2_geom;
dTriMeshDataID mesh_data;
dGeomID mesh_geom;

dBodyID ball1_body, ball2_body;

const unsigned n_box_verts = 8;
dVector3 box_verts[n_box_verts] = {
    {-track_len/2, -track_width/2,  track_height/2}, // 0
    { track_len/2, -track_width/2,  track_height/2}, // 1
    { track_len/2,  track_width/2,  track_height/2}, // 2
    {-track_len/2,  track_width/2,  track_height/2}, // 3
    { track_len/2, -track_width/2, -track_height/2}, // 4
    {-track_len/2, -track_width/2, -track_height/2}, // 5
    {-track_len/2,  track_width/2, -track_height/2}, // 6
    { track_len/2,  track_width/2, -track_height/2}  // 7
};

const unsigned n_box_faces = 12;
dTriIndex box_faces[n_box_faces * 3] = {
    0, 1, 2,
    0, 2, 3,
    1, 4, 7,
    1, 7, 2,
    4, 5, 6,
    4, 6, 7,
    5, 0, 3,
    5, 3, 6,
    3, 2, 7,
    3, 7, 6,
    0, 5, 4,
    0, 4, 1
};


const unsigned n_track_verts = n_box_verts * 2;
const unsigned n_track_faces = n_box_faces * 2;

dVector3 track_verts[n_track_verts];
dTriIndex track_faces[n_track_faces * 3];



void resetBall(dBodyID b, unsigned idx)
{
    dBodySetPosition(b,
                     0.5*track_len*cos(track_incl) // Z
                     - 0.5*track_height*sin(track_incl)
                     - ball_radius, // X
                     balls_sep*idx, // Y
                     track_elevation + ball_radius// Z
                     + 0.5*track_len*sin(track_incl)
                     + 0.5*track_height*cos(track_incl));
    dMatrix3 r = {1, 0, 0, 0,
                  0, 1, 0, 0,
                  0, 0, 1, 0};
    dBodySetRotation(b, r);
    dBodySetLinearVel(b, 0, 0, 0);
    dBodySetAngularVel(b, 0, 0, 0);

}


void resetSim()
{
    resetBall(ball1_body, 0);
    resetBall(ball2_body, 1);
}


void start()
{
    dAllocateODEDataForThread(dAllocateMaskAll);

    world = dWorldCreate();
    dWorldSetGravity (world,0,0,-9.8);

    contact_group = dJointGroupCreate(0);

    space = dSimpleSpaceCreate (0);


    // first, the ground plane
    // it has to coincide with the plane we have in drawstuff
    ground = dCreatePlane(space, 0, 0, 1, 0);


    // now a ball
    dMass m;
    dMassSetSphere(&m, 0.1, ball_radius);

    ball1_geom = dCreateSphere(space, ball_radius);
    ball1_body = dBodyCreate(world);
    dGeomSetBody(ball1_geom, ball1_body);
    dBodySetMass(ball1_body, &m);

    ball2_geom = dCreateSphere(space, ball_radius);
    ball2_body = dBodyCreate(world);
    dGeomSetBody(ball2_geom, ball2_body);
    dBodySetMass(ball2_body, &m);




    // tracks made out of boxes
    dGeomID trk;
    dMatrix3 r1, r2, r3;
    dVector3 ro = {0, -(0.5*track_gauge + 0.5*track_width), track_elevation};
    dMatrix3 s1, s2, s3;
    dVector3 so = {0, 0.5*track_gauge + 0.5*track_width, track_elevation};

    dRFromAxisAndAngle(r1, 1, 0, 0,  track_angle);
    dRFromAxisAndAngle(r2, 0, 1, 0, -track_incl);
    dMultiply0_333(r3, r2, r1);

    dRFromAxisAndAngle(s1, 1, 0, 0, -track_angle);
    dRFromAxisAndAngle(s2, 0, 1, 0, -track_incl);
    dMultiply0_333(s3, s2, s1);

    trk = dCreateBox(space, track_len, track_width, track_height);
    dGeomSetPosition(trk, ro[0], ro[1] + balls_sep, ro[2]);
    dGeomSetRotation(trk, r3);

    trk = dCreateBox(space, track_len, track_width, track_height);
    dGeomSetPosition(trk, so[0], so[1] + balls_sep, so[2]);
    dGeomSetRotation(trk, s3);



    

    // tracks made out of trimesh
    for (unsigned i=0; i<n_box_verts; ++i) {
        dVector3 p;
        dMultiply0_331(p, s3, box_verts[i]);
        dAddVectors3(p, p, so);
        dCopyVector3(track_verts[i], p);
    }
    // trimesh tracks 2, transform all vertices by s3
    for (unsigned i=0; i<n_box_verts; ++i) {
        dVector3 p;
        dMultiply0_331(p, r3, box_verts[i]);
        dAddVectors3(p, p, ro);
        dCopyVector3(track_verts[n_box_verts + i], p);
    }

    // copy face indices
    for (unsigned i=0; i<n_box_faces; ++i)
        for (unsigned j=0; j<3; ++j) // each face index
            track_faces[3*i+j] = box_faces[3*i+j];
    for (unsigned i=0; i<n_box_faces; ++i)
        for (unsigned j=0; j<3; ++j) // each face index
            track_faces[3*(i + n_box_faces)+j] = box_faces[3*i+j] + n_box_verts;

    mesh_data = dGeomTriMeshDataCreate();
    dGeomTriMeshDataBuildSimple(mesh_data,
                                track_verts[0], n_track_verts,
                                track_faces, 3*n_track_faces);
    mesh_geom = dCreateTriMesh(space, mesh_data, 0, 0, 0);





    resetSim();
    

    // initial camera position
    static float xyz[3] = {-5.9414,-0.4804,2.9800};
    static float hpr[3] = {32.5000,-10.0000,0.0000};
    dsSetViewpoint (xyz,hpr);

    dsSetSphereQuality(3);
}


void nearCallback(void *, dGeomID a, dGeomID b)
{
    const unsigned max_contacts = 8;
    dContact contacts[max_contacts];
    
    if (!dGeomGetBody(a) && !dGeomGetBody(b))
        return; // don't handle static geom collisions

    int n = dCollide(a, b, max_contacts, &contacts[0].geom, sizeof(dContact));
    //clog << "got " << n << " contacts" << endl;

    /* Simple contact merging:
     * If we have contacts that are too close with the same normal, keep only
     * the one with maximum depth.
     * The epsilon that defines what "too close" means can be a heuristic.
     */
    int new_n = 0;
    dReal epsilon = 1e-1; // default
    /* If we know one of the geoms is a sphere, we can base the epsilon on the
     *  sphere's radius.
     */
    dGeomID s = 0;
    if ((dGeomGetClass(a) == dSphereClass && (s = a)) || 
        (dGeomGetClass(b) == dSphereClass && (s = b))) {
        epsilon = dGeomSphereGetRadius(s) * 0.3;
    }


    for (int i=0; i<n; ++i) {

        // this block draws the contact points before merging, in red
        dMatrix3 r;
        dRSetIdentity(r);
        dsSetColor(1, 0, 0);
        dsSetTexture(DS_NONE);
        dsDrawSphere(contacts[i].geom.pos, r, 0.008);

        // let's offset the line a bit to avoid drawing overlap issues
        float xyzf[3], hprf[3];
        dsGetViewpoint(xyzf, hprf);
        dVector3 xyz = {dReal(xyzf[0]), dReal(xyzf[1]), dReal(xyzf[2])};
        dVector3 v;
        dSubtractVectors3(v, contacts[i].geom.pos, xyz);
        dVector3 c;
        dCalcVectorCross3(c, v, contacts[i].geom.pos);
        dNormalize3(c);
        dVector3 pos1;
        dAddScaledVectors3(pos1, contacts[i].geom.pos, c, 1, 0.005);
        dVector3 pos2;
        dAddScaledVectors3(pos2, pos1, contacts[i].geom.normal, 1, 0.05);
        dsDrawLine(pos1, pos2);
        // end of contacts drawing code



        int closest_point = i;
        for (int j=0; j<new_n; ++j) {
            dReal alignment = dCalcVectorDot3(contacts[i].geom.normal, contacts[j].geom.normal);
            if (alignment > 0.99 // about 8 degrees of difference
                &&
                dCalcPointsDistance3(contacts[i].geom.pos, contacts[j].geom.pos) < epsilon) {
                // they are too close
                closest_point = j;
                //clog << "found close points: " << j << " and " << i << endl;
                break;
            }
        }
        
        if (closest_point != i) {
            // we discard one of the points
            if (contacts[i].geom.depth > contacts[closest_point].geom.depth)
                // the new point is deeper, copy it over closest_point
                contacts[closest_point] = contacts[i];
        } else
            contacts[new_n++] = contacts[i]; // the point is preserved
    }
    //clog << "reduced from " << n << " to " << new_n << endl;
    n = new_n;

    for (int i=0; i<n; ++i) {
        contacts[i].surface.mode = dContactBounce | dContactApprox1 | dContactSoftERP;
        contacts[i].surface.mu = 10;
        contacts[i].surface.bounce = 0.2;
        contacts[i].surface.bounce_vel = 0;
        contacts[i].surface.soft_erp = 1e-3;
        //clog << "depth: " << contacts[i].geom.depth << endl;


        dJointID contact = dJointCreateContact(world, contact_group, &contacts[i]);
        dJointAttach(contact, dGeomGetBody(a), dGeomGetBody(b));

        dMatrix3 r;
        dRSetIdentity(r);
        dsSetColor(0, 0, 1);
        dsSetTexture(DS_NONE);
        dsDrawSphere(contacts[i].geom.pos, r, 0.01);
        dsSetColor(0, 1, 0);
        dVector3 pos2;
        dAddScaledVectors3(pos2, contacts[i].geom.pos, contacts[i].geom.normal, 1, 0.1);
        dsDrawLine(contacts[i].geom.pos, pos2);
    }
    //clog << "----" << endl;
}




void stop()
{
    dGeomDestroy(mesh_geom);
    dGeomTriMeshDataDestroy(mesh_data);

    dBodyDestroy(ball1_body);
    dBodyDestroy(ball2_body);

    dGeomDestroy(ground);

    dJointGroupDestroy(contact_group);

    dSpaceDestroy(space); // will destroy all geoms

    dWorldDestroy(world);
}


static void command (int cmd)
{
    switch (cmd) {
        case ' ':
            resetSim();
            break;
    }
}


void drawGeom(dGeomID g)
{
    int gclass = dGeomGetClass(g);
    const dReal *pos = dGeomGetPosition(g);
    const dReal *rot = dGeomGetRotation(g);

    switch (gclass) {
        case dSphereClass:
            dsSetColorAlpha(0, 0.75, 0.5, 0.5);
            dsSetTexture (DS_CHECKERED);
            dsDrawSphere(pos, rot, dGeomSphereGetRadius(g));
            break;
        case dBoxClass:
        {
            dVector3 lengths;
            dsSetColorAlpha(1, 1, 0, 0.5);
            dsSetTexture (DS_WOOD);
            dGeomBoxGetLengths(g, lengths);
            dsDrawBox(pos, rot, lengths);
            break;
        }
        case dTriMeshClass: 
        {
            int numi = dGeomTriMeshGetTriangleCount(g);

            for (int i=0; i<numi; ++i) {
                dVector3 v0, v1, v2;
                dGeomTriMeshGetTriangle(g, i, &v0, &v1, &v2);

                dsSetTexture (DS_WOOD);

                dsSetDrawMode(DS_WIREFRAME);
                dsSetColorAlpha(0, 0, 0, 1.0);
                dsDrawTriangle(pos, rot, v0, v1, v2, true);

                dsSetDrawMode(DS_POLYFILL);
                dsSetColorAlpha(1, 1, 0, 0.5);
                dsDrawTriangle(pos, rot, v0, v1, v2, true);
            }
            break;
        }
        
        default:
        {}
    }
}



void simLoop (int pause)
{
    if (!pause) {

        const dReal step = 0.02;
        const unsigned nsteps = 1;

        for (unsigned i=0; i<nsteps; ++i) {
            dSpaceCollide(space, 0, nearCallback);
            dWorldQuickStep(world, step);
            dJointGroupEmpty(contact_group);
        }
    } else {
        dSpaceCollide(space, 0, nearCallback);
        dJointGroupEmpty(contact_group);
    }
    
    // now we draw everything
    unsigned ngeoms = dSpaceGetNumGeoms(space);
    for (unsigned i=0; i<ngeoms; ++i) {
        dGeomID g = dSpaceGetGeom(space, i);

        if (g == ground)
            continue; // drawstuff is already drawing it for us

        drawGeom(g);
    }
    
    if (dBodyGetPosition(ball1_body)[0] < -track_len)
        resetSim();
}


int main (int argc, char **argv)
{
    // setup pointers to drawstuff callback functions
    dsFunctions fn;
    fn.version = DS_VERSION;
    fn.start = &start;
    fn.step = &simLoop;
    fn.command = &command;
    fn.stop = stop;
    fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
    
    // create world
    dInitODE();

    // run demo
    dsSimulationLoop (argc, argv, 800, 600, &fn);

    dCloseODE();
    return 0;
}