/**
 @file Entity_joint.cpp

 Dojo Game Engine

 Copyright 2005, Morgan McGuire
 All rights reserved.
 */
#include "dojo/Entity.h"
#include "dojo/ASFModel.h"
#include "dojo/Entity.h"
#include "dojo/World.h"
#include "dojo/odeHelper.h"

namespace dojo {

void Entity::addHinge(        
    const std::string&      jointName,
    const std::string&      part1Name,
    const std::string&      part2Name,
    const Vector3&          bsAxis,
    const Vector3&          bsAnchor,
    float                   loLimit,
    float                   hiLimit,
    float                   maxTorque,
    float                   scale) {
    
    alwaysAssertM(! inWorld, "Cannot add to an Entity that has been inserted into the World.");

    Part* part1 = partArray[part1Name];
    Part* part2 = partArray[part2Name];

    if (! isSimulated()) {
        // Can't put hinges on immobile objects
        return;
    }

    Joint* joint = new Joint(jointName, scale);

    static const int NO_GROUP = 0;

    joint->ID = dJointCreateHinge(World::world()->worldID(), NO_GROUP);

    Vector3 wsAxis = part1->frame().vectorToWorldSpace(bsAxis);
    Vector3 wsAnchor = part1->frame().pointToWorldSpace(bsAnchor);

    // Must attach before setting axis and anchor
    dJointAttach(joint->ID, part1->body, part2->body);
    
    dJointSetHingeAxis(joint->ID, wsAxis);
    dJointSetHingeAnchor(joint->ID, wsAnchor);
    debugAssert(maxTorque >= 0);
    dJointSetHingeParam(joint->ID, dParamFMax, maxTorque);

    // Disable joint limits
    dJointSetHingeParam(joint->ID, dParamLoStop, loLimit);
    dJointSetHingeParam(joint->ID, dParamHiStop, hiLimit);
    
    jointArray.append(joint->name, joint);
}


void Entity::setHingeTargetVelocity(
    const std::string& jointName,
    float targetVel) {

    Joint* joint = jointArray[jointName];

    debugAssertM(
        (dJointGetType(joint->ID) == dJointTypeHinge),
        format("Joint '%s' is not a hinge.", joint->name.c_str()));

    dJointSetHingeParam(joint->ID, dParamVel, targetVel);
}


void Entity::addUniversalJoint(        
    const std::string&      jointName,
    const std::string&      part1Name,
    const std::string&      part2Name,
    const Vector3&          bsAxis1,
    const Vector3&          bsAxis2,
    const Vector3&          bsAnchor,
	float                   maxTorque,
	const float				loLimit1,
	const float				loLimit2,
	const float				hiLimit1,
	const float				hiLimit2,
    float                   scale) {
    
    alwaysAssertM(! inWorld, "Cannot add to an Entity that has been inserted into the World.");

    Part* part1 = partArray[part1Name];
    Part* part2 = partArray[part2Name];

    if (! isSimulated()) {
        // Can't put hinges on immobile objects
        return;
    }

    Joint* joint = new Joint(jointName, scale);

    static const int NO_GROUP = 0;
    joint->ID = dJointCreateUniversal(World::world()->worldID(), NO_GROUP);

    Vector3 wsAxis1 = part1->frame().vectorToWorldSpace(bsAxis1);
    Vector3 wsAxis2 = part1->frame().vectorToWorldSpace(bsAxis2);
    Vector3 wsAnchor = part1->frame().pointToWorldSpace(bsAnchor);

    // Must attach before setting axis and anchor
    dJointAttach(joint->ID, part1->body, part2->body);
    
    dJointSetUniversalAxes(joint->ID, wsAxis1, wsAxis2);
    dJointSetUniversalAnchor(joint->ID, wsAnchor);
    debugAssert(maxTorque >= 0);
    dJointSetUniversalParam(joint->ID, dParamFMax, maxTorque);

     // Set joint limits
    dJointSetUniversalParam(joint->ID, dParamLoStop, loLimit1);
    dJointSetUniversalParam(joint->ID, dParamHiStop, hiLimit1);
	dJointSetUniversalParam(joint->ID, dParamLoStop2, loLimit2);
    dJointSetUniversalParam(joint->ID, dParamHiStop2, hiLimit2);

    jointArray.append(joint->name, joint);
}

    
void Entity::addBallJoint(        
    const std::string&      jointName,
    const std::string&      part1Name,
    const std::string&      part2Name,
    const Vector3&          bsAnchor,
	float                   maxTorque,
	const float				loLimit1,
	const float				loLimit2,
	const float				loLimit3,
	const float				hiLimit1,
	const float				hiLimit2,
	const float				hiLimit3,
    float                   scale) {
    
    alwaysAssertM(! inWorld, "Cannot add to an Entity that has been inserted into the World.");

    Part* part1 = partArray[part1Name];
    Part* part2 = partArray[part2Name];

    if (! isSimulated()) {
        // Can't put hinges on immobile objects
        return;
    }

    static const int NO_GROUP = 0;
    Joint* joint = new Joint(jointName, scale);

    joint->ID = dJointCreateBall(World::world()->worldID(), NO_GROUP);

    // Must attach before setting axis and anchor
    dJointAttach(joint->ID, part1->body, part2->body);
    
    Vector3 wsAnchor = part1->frame().pointToWorldSpace(bsAnchor);
    dJointSetBallAnchor(joint->ID, wsAnchor.x, wsAnchor.y, wsAnchor.z);

    if (maxTorque > 0.0f) {
        // Create an amotor to drive the joint
        joint->ID2 = dJointCreateAMotor(World::world()->worldID(), NO_GROUP);
        dJointSetAMotorParam(joint->ID2, dParamCFM, 10e-4);
        dJointSetAMotorNumAxes(joint->ID2, 3);
        dJointSetAMotorParam(joint->ID2, dParamFMax, maxTorque);

		// TODO: joint limits
    }

    jointArray.append(joint->name, joint);
}


Entity::Joint::Joint(const std::string& n, const float s) : name(n), size(s), ID(0), ID2(0) {
}


Entity::Joint::~Joint() {
    dJointDestroy(ID);
    if (ID2 != 0) {
        dJointDestroy(ID2);
    }
}


void Entity::Joint::render(RenderDevice* rd) const {

    static const Color3 color = Color3::red();

    switch (dJointGetType(ID)) {
    case dJointTypeHinge:
        {
            Vector3 anchor;
            Vector3 axis;
            dJointGetHingeAnchor(ID, anchor);
            dJointGetHingeAxis(ID, axis);
            const float s = size * 0.5;
            Draw::cylinder(Cylinder(anchor - axis * s, anchor + axis * s, s * 0.25), rd, 
                color, Color3::black());
        }
        break;

    case dJointTypeUniversal:
        {
            Vector3 anchor;
            Vector3 axis1;
            Vector3 axis2;
            dJointGetUniversalAnchor(ID, anchor);
            dJointGetUniversalAxes(ID, axis1, axis2);
            const float s = size * 0.5;
            Draw::cylinder(Cylinder(anchor - axis1 * s, anchor + axis1 * s, s * 0.25), rd, 
                color, Color3::black());
            Draw::cylinder(Cylinder(anchor - axis2 * s, anchor + axis2 * s, s * 0.25), rd, 
                color, Color3::black());
        }
        break;

    case dJointTypeBall:
        {
            Vector3 anchor;
            dJointGetBallAnchor(ID, anchor);
            const float s = size * 0.5;
            Draw::sphere(Sphere(anchor, s), rd, color, Color3::black());
        }
        break;

    default:
        debugAssert(false);
    }
}

} // namespace dojo
