/*************************************************************************
 *                                                                       *
 * 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 <ode/ode.h>
#include <drawstuff/drawstuff.h>
#include "texturepath.h"

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


dWorldID world;
dSpaceID space;
dBodyID body1;
dBodyID body2;
dJointID joint1, joint2;

void start()
{
    world = dWorldCreate();
    dWorldSetGravity (world,0,0,-9.8);

    dWorldSetDamping(world, 1e-4, 1e-5);
//    dWorldSetERP(world, 1);

    space = dSimpleSpaceCreate (0);
    
    body1 = dBodyCreate(world);
    body2 = dBodyCreate(world);

    dBodySetPosition(body1, 0, 0, 3);
    dBodySetPosition(body2, 0, 0, 1);


    dGeomID g;
    dMass mass;
    
    g = dCreateBox(space, 0.2, 0.2, 1);
    dGeomSetBody(g, body1);
    dMassSetBox(&mass, 1, 0.2, 0.2, 1);
    dBodySetMass(body1, &mass);
    
    g = dCreateBox(space, 0.2, 0.2, 1);
    dGeomSetBody(g, body2);
    dMassSetBox(&mass, 1, 0.2, 0.2, 1);
    dBodySetMass(body2, &mass);

    joint1 = dJointCreateDBall(world, 0);
    dJointAttach(joint1, body1, 0);
    dJointSetDBallAnchor1(joint1, 0, 0, 3.5);
    dJointSetDBallAnchor2(joint1, 0, 0, 4.5);

    joint2 = dJointCreateDBall(world, 0);
    dJointAttach(joint2, body1, body2);
    dJointSetDBallAnchor1(joint2, 0, 0, 2.5);
    dJointSetDBallAnchor2(joint2, 0, 0, 1.5);


    // initial camera position
    static float xyz[3] = {3.8966, -2.0614, 4.0300};
    static float hpr[3] = {153.5, -16.5, 0};
    dsSetViewpoint (xyz,hpr);
}

void stop()
{
    dSpaceDestroy(space);

    dWorldDestroy(world);
}


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, 1);
            dsSetTexture (DS_CHECKERED);
            dsDrawSphere(pos, rot, dGeomSphereGetRadius(g));
            break;
        case dBoxClass:
        {
            dVector3 lengths;
            dsSetColorAlpha(1, 1, 0, 1);
            dsSetTexture (DS_WOOD);
            dGeomBoxGetLengths(g, lengths);
            dsDrawBox(pos, rot, lengths);
            break;
        }
        
        default:
        {}
    }
}

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

        static dReal t = 0;

        const dReal step = 0.005;
        const unsigned nsteps = 4;

        for (unsigned i=0; i<nsteps; ++i) {

            dReal f = sin(t*1.2)*0.8;
            dBodyAddForceAtRelPos(body1,
                                  f, 0, 0, 
                                  0, 0, -0.5); // at the lower end

            dReal g = sin(t*0.7)*0.8;
            dBodyAddForceAtRelPos(body2,
                                  0, g, 0, 
                                  0, 0, -0.5); // at the lower end
            t += step;

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

        drawGeom(g);
    }

    dVector3 a11, a12;
    dJointGetDBallAnchor1(joint1, a11);
    dJointGetDBallAnchor2(joint1, a12);
    dsSetColor(1, 0, 0);
    dsDrawLine(a11, a12);

    //printf("Error 1: %f\n", fabs(dJointGetDBallDistance(joint1) - dCalcPointsDistance3(a11, a12)));

    dVector3 a21, a22;
    dJointGetDBallAnchor1(joint2, a21);
    dJointGetDBallAnchor2(joint2, a22);
    dsSetColor(0, 1, 0);
    dsDrawLine(a21, a22);

    //printf("Error 2: %f\n", fabs(dJointGetDBallDistance(joint2) - dCalcPointsDistance3(a21, a22)));
}



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 = 0;
    fn.stop = stop;
    fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
    
    // create world
    dInitODE();

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

    dCloseODE();
    return 0;
}