/*************************************************************************
 *                                                                       *
 * 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/odeconfig.h>
#include "pr.h"
#include "joint_internal.h"



//****************************************************************************
// Prismatic and Rotoide

dxJointPR::dxJointPR( dxWorld *w ) :
    dxJoint( w )
{
    // Default Position
    // Z^
    //  | Body 1       P      R          Body2
    //  |+---------+   _      _         +-----------+
    //  ||         |----|----(_)--------+           |
    //  |+---------+   -                +-----------+
    //  |
    // X.-----------------------------------------> Y
    // N.B. X is comming out of the page
    dSetZero( anchor2, 4 );

    dSetZero( axisR1, 4 );
    axisR1[0] = 1;
    dSetZero( axisR2, 4 );
    axisR2[0] = 1;

    dSetZero( axisP1, 4 );
    axisP1[1] = 1;
    dSetZero( qrel, 4 );
    dSetZero( offset, 4 );

    limotR.init( world );
    limotP.init( world );
}


dReal dJointGetPRPosition( dJointID j )
{
    dxJointPR* joint = ( dxJointPR* ) j;
    dUASSERT( joint, "bad joint argument" );
    checktype( joint, PR );

    dVector3 q;
    // get the offset in global coordinates
    dMultiply0_331( q, joint->node[0].body->posr.R, joint->offset );

    if ( joint->node[1].body )
    {
        dVector3 anchor2;

        // get the anchor2 in global coordinates
        dMultiply0_331( anchor2, joint->node[1].body->posr.R, joint->anchor2 );

        q[0] = (( joint->node[0].body->posr.pos[0] + q[0] ) -
            ( joint->node[1].body->posr.pos[0] + anchor2[0] ) );
        q[1] = (( joint->node[0].body->posr.pos[1] + q[1] ) -
            ( joint->node[1].body->posr.pos[1] + anchor2[1] ) );
        q[2] = (( joint->node[0].body->posr.pos[2] + q[2] ) -
            ( joint->node[1].body->posr.pos[2] + anchor2[2] ) );

    }
    else
    {
        //N.B. When there is no body 2 the joint->anchor2 is already in
        //     global coordinates

        q[0] = (( joint->node[0].body->posr.pos[0] + q[0] ) -
            ( joint->anchor2[0] ) );
        q[1] = (( joint->node[0].body->posr.pos[1] + q[1] ) -
            ( joint->anchor2[1] ) );
        q[2] = (( joint->node[0].body->posr.pos[2] + q[2] ) -
            ( joint->anchor2[2] ) );

        if ( joint->flags & dJOINT_REVERSE )
        {
            q[0] = -q[0];
            q[1] = -q[1];
            q[2] = -q[2];
        }
    }

    dVector3 axP;
    // get prismatic axis in global coordinates
    dMultiply0_331( axP, joint->node[0].body->posr.R, joint->axisP1 );

    return dCalcVectorDot3( axP, q );
}

dReal dJointGetPRPositionRate( dJointID j )
{
    dxJointPR* joint = ( dxJointPR* ) j;
    dUASSERT( joint, "bad joint argument" );
    checktype( joint, PR );
    // get axis1 in global coordinates
    dVector3 ax1;
    dMultiply0_331( ax1, joint->node[0].body->posr.R, joint->axisP1 );

    if ( joint->node[1].body )
    {
        dVector3 lv2;
        dBodyGetRelPointVel( joint->node[1].body, joint->anchor2[0], joint->anchor2[1], joint->anchor2[2], lv2 );
        return dCalcVectorDot3( ax1, joint->node[0].body->lvel ) - dCalcVectorDot3( ax1, lv2 );
    }
    else
    {
        dReal rate = dCalcVectorDot3( ax1, joint->node[0].body->lvel );
        return ( (joint->flags & dJOINT_REVERSE) ? -rate : rate);
    }
}



dReal dJointGetPRAngle( dJointID j )
{
    dxJointPR* joint = ( dxJointPR* )j;
    dAASSERT( joint );
    checktype( joint, PR );
    if ( joint->node[0].body )
    {
        dReal ang = getHingeAngle( joint->node[0].body,
            joint->node[1].body,
            joint->axisR1,
            joint->qrel );
        if ( joint->flags & dJOINT_REVERSE )
            return -ang;
        else
            return ang;
    }
    else return 0;
}



dReal dJointGetPRAngleRate( dJointID j )
{
    dxJointPR* joint = ( dxJointPR* )j;
    dAASSERT( joint );
    checktype( joint, PR );
    if ( joint->node[0].body )
    {
        dVector3 axis;
        dMultiply0_331( axis, joint->node[0].body->posr.R, joint->axisR1 );
        dReal rate = dCalcVectorDot3( axis, joint->node[0].body->avel );
        if ( joint->node[1].body ) rate -= dCalcVectorDot3( axis, joint->node[1].body->avel );
        if ( joint->flags & dJOINT_REVERSE ) rate = -rate;
        return rate;
    }
    else return 0;
}




void 
dxJointPR::getSureMaxInfo( SureMaxInfo* info )
{
    info->max_m = 6;
}



void
dxJointPR::getInfo1( dxJoint::Info1 *info )
{
    info->nub = 4;
    info->m = 4;


    // see if we're at a joint limit.
    limotP.limit = 0;
    if (( limotP.lostop > -dInfinity || limotP.histop < dInfinity ) &&
        limotP.lostop <= limotP.histop )
    {
        // measure joint position
        dReal pos = dJointGetPRPosition( this );
        limotP.testRotationalLimit( pos );  // N.B. The function is ill named
    }

    // powered needs an extra constraint row
    if ( limotP.limit || limotP.fmax > 0 ) info->m++;


    // see if we're at a joint limit.
    limotR.limit = 0;
    if (( limotR.lostop >= -M_PI || limotR.histop <= M_PI ) &&
        limotR.lostop <= limotR.histop )
    {
        dReal angle = getHingeAngle( node[0].body,
            node[1].body,
            axisR1, qrel );
        limotR.testRotationalLimit( angle );
    }

    // powered morit or at limits needs an extra constraint row
    if ( limotR.limit || limotR.fmax > 0 ) info->m++;

}



void
dxJointPR::getInfo2( dReal worldFPS, dReal worldERP, 
    int rowskip, dReal *J1, dReal *J2,
    int pairskip, dReal *pairRhsCfm, dReal *pairLoHi, 
    int *findex )
{
    dReal k = worldFPS * worldERP;


    dVector3 q;  // plane space of axP and after that axR

    // pull out pos and R for both bodies. also get the `connection'
    // vector pos2-pos1.

    dReal *pos2 = NULL, *R2 = NULL;
    
    dReal *pos1 = node[0].body->posr.pos;
    dReal *R1 = node[0].body->posr.R;

    dxBody *body1 = node[1].body;

    if ( body1 )
    {
        pos2 = body1->posr.pos;
        R2 = body1->posr.R;
    }


    dVector3 axP; // Axis of the prismatic joint in global frame
    dMultiply0_331( axP, R1, axisP1 );

    // distance between the body1 and the anchor2 in global frame
    // Calculated in the same way as the offset
    dVector3 wanchor2 = {0, 0, 0}, dist;

    if ( body1 )
    {
        // Calculate anchor2 in world coordinate
        dMultiply0_331( wanchor2, R2, anchor2 );
        dist[0] = wanchor2[0] + pos2[0] - pos1[0];
        dist[1] = wanchor2[1] + pos2[1] - pos1[1];
        dist[2] = wanchor2[2] + pos2[2] - pos1[2];
    }
    else
    {
        if ( (flags & dJOINT_REVERSE) != 0 )
        {
            dSubtractVectors3(dist, pos1, anchor2); // Invert the value
        }
        else
        {
            dSubtractVectors3(dist, anchor2, pos1); // Invert the value
        }
    }


    // ======================================================================
    // Work on the Rotoide part (i.e. row 0, 1 and maybe 4 if rotoide powered

    // Set the two rotoide rows. The rotoide axis should be the only unconstrained
    // rotational axis, the angular velocity of the two bodies perpendicular to
    // the rotoide axis should be equal. Thus the constraint equations are
    //    p*w1 - p*w2 = 0
    //    q*w1 - q*w2 = 0
    // where p and q are unit vectors normal to the rotoide axis, and w1 and w2
    // are the angular velocity vectors of the two bodies.
    dVector3 ax2;
    dVector3 ax1;
    dMultiply0_331( ax1, R1, axisR1 );
    dCalcVectorCross3( q , ax1, axP );

    dCopyVector3(J1 + GI2__JA_MIN, axP);

    if ( body1 )
    {
        dCopyNegatedVector3(J2 + GI2__JA_MIN, axP);
    }

    dCopyVector3(J1 + rowskip + GI2__JA_MIN, q);

    if ( body1 )
    {
        dCopyNegatedVector3(J2 + rowskip + GI2__JA_MIN, q);
    }

    // Compute the right hand side of the constraint equation set. Relative
    // body velocities along p and q to bring the rotoide back into alignment.
    // ax1,ax2 are the unit length rotoide axes of body1 and body2 in world frame.
    // We need to rotate both bodies along the axis u = (ax1 x ax2).
    // if `theta' is the angle between ax1 and ax2, we need an angular velocity
    // along u to cover angle erp*theta in one step :
    //   |angular_velocity| = angle/time = erp*theta / stepsize
    //                      = (erp*fps) * theta
    //    angular_velocity  = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
    //                      = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
    // ...as ax1 and ax2 are unit length. if theta is smallish,
    // theta ~= sin(theta), so
    //    angular_velocity  = (erp*fps) * (ax1 x ax2)
    // ax1 x ax2 is in the plane space of ax1, so we project the angular
    // velocity to p and q to find the right hand side.

    if ( body1 )
    {
        dMultiply0_331( ax2, R2, axisR2 );
    }
    else
    {
        dCopyVector3(ax2, axisR2);
    }

    dVector3 b;
    dCalcVectorCross3( b, ax1, ax2 );
    pairRhsCfm[GI2_RHS] = k * dCalcVectorDot3( b, axP );
    pairRhsCfm[pairskip + GI2_RHS] = k * dCalcVectorDot3( b, q );



    // ==========================
    // Work on the Prismatic part (i.e row 2,3 and 4 if only the prismatic is powered
    // or 5 if rotoide and prismatic powered

    // two rows. we want: vel2 = vel1 + w1 x c ... but this would
    // result in three equations, so we project along the planespace vectors
    // so that sliding along the prismatic axis is disregarded. for symmetry we
    // also substitute (w1+w2)/2 for w1, as w1 is supposed to equal w2.

    // p1 + R1 dist' = p2 + R2 anchor2' ## OLD ## p1 + R1 anchor1' = p2 + R2 dist'
    // v1 + w1 x R1 dist' + v_p = v2 + w2 x R2 anchor2'## OLD  v1 + w1 x R1 anchor1' = v2 + w2 x R2 dist' + v_p
    // v_p is speed of prismatic joint (i.e. elongation rate)
    // Since the constraints are perpendicular to v_p we have:
    // p dot v_p = 0 and q dot v_p = 0
    // ax1 dot ( v1 + w1 x dist = v2 + w2 x anchor2 )
    // q dot ( v1 + w1 x dist = v2 + w2 x anchor2 )
    // ==
    // ax1 . v1 + ax1 . w1 x dist = ax1 . v2 + ax1 . w2 x anchor2 ## OLD ## ax1 . v1 + ax1 . w1 x anchor1 = ax1 . v2 + ax1 . w2 x dist
    // since a . (b x c) = - b . (a x c) = - (a x c) . b
    // and a x b = - b x a
    // ax1 . v1 - ax1 x dist . w1 - ax1 . v2 - (- ax1 x anchor2 . w2) = 0
    // ax1 . v1 + dist x ax1 . w1 - ax1 . v2 - anchor2 x ax1 . w2 = 0
    // Coeff for 1er line of: J1l => ax1, J2l => -ax1
    // Coeff for 2er line of: J1l => q, J2l => -q
    // Coeff for 1er line of: J1a => dist x ax1, J2a => - anchor2 x ax1
    // Coeff for 2er line of: J1a => dist x q,   J2a => - anchor2 x q

    int currRowSkip = 2 * rowskip;
    {
        dCopyVector3( J1 + currRowSkip + GI2__JL_MIN, ax1 );
        dCalcVectorCross3( J1 + currRowSkip + GI2__JA_MIN, dist, ax1 );

        if ( body1 )
        {
            dCopyNegatedVector3( J2 + currRowSkip + GI2__JL_MIN, ax1 );
            // ax2 x anchor2 instead of anchor2 x ax2 since we want the negative value
            dCalcVectorCross3( J2 + currRowSkip + GI2__JA_MIN, ax2, wanchor2 );   // since ax1 == ax2
        }
    }

    currRowSkip += rowskip;
    {
        dCopyVector3( J1 + currRowSkip + GI2__JL_MIN, q );
        dCalcVectorCross3(J1 + currRowSkip + GI2__JA_MIN, dist, q );

        if ( body1 )
        {
            dCopyNegatedVector3( J2 + currRowSkip + GI2__JL_MIN, q);
            // The cross product is in reverse order since we want the negative value
            dCalcVectorCross3( J2 + currRowSkip + GI2__JA_MIN, q, wanchor2 );
        }
    }

    // We want to make correction for motion not in the line of the axisP
    // We calculate the displacement w.r.t. the anchor pt.
    //
    // compute the elements 2 and 3 of right hand side.
    // we want to align the offset point (in body 2's frame) with the center of body 1.
    // The position should be the same when we are not along the prismatic axis
    dVector3 err;
    dMultiply0_331( err, R1, offset );
    dSubtractVectors3(err, dist, err);

    int currPairSkip = 2 * pairskip;
    {
        pairRhsCfm[currPairSkip + GI2_RHS] = k * dCalcVectorDot3( ax1, err );
    }

    currPairSkip += pairskip;
    {
        pairRhsCfm[currPairSkip + GI2_RHS] = k * dCalcVectorDot3( q, err );
    }

    currRowSkip += rowskip; currPairSkip += pairskip;

    if (  body1 || (flags & dJOINT_REVERSE) == 0 )
    {
        if (limotP.addLimot ( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, axP, 0 ))
        {
            currRowSkip += rowskip; currPairSkip += pairskip;
        }
    }
    else
    {
        dVector3 rAxP;
        dCopyNegatedVector3(rAxP, axP);

        if (limotP.addLimot ( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, rAxP, 0 ))
        {
            currRowSkip += rowskip; currPairSkip += pairskip;
        }
    }

    limotR.addLimot ( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, ax1, 1 );
}


// compute initial relative rotation body1 -> body2, or env -> body1
void
dxJointPR::computeInitialRelativeRotation()
{
    if ( node[0].body )
    {
        if ( node[1].body )
        {
            dQMultiply1( qrel, node[0].body->q, node[1].body->q );
        }
        else
        {
            // set joint->qrel to the transpose of the first body q
            qrel[0] = node[0].body->q[0];
            for ( int i = 1; i < 4; i++ )
                qrel[i] = -node[0].body->q[i];
            // WARNING do we need the - in -joint->node[0].body->q[i]; or not
        }
    }
}

void dJointSetPRAnchor( dJointID j, dReal x, dReal y, dReal z )
{
    dxJointPR* joint = ( dxJointPR* ) j;
    dUASSERT( joint, "bad joint argument" );
    checktype( joint, PR );
    setAnchors( joint, x, y, z, joint->offset, joint->anchor2 );
}


void dJointSetPRAxis1( dJointID j, dReal x, dReal y, dReal z )
{
    dxJointPR* joint = ( dxJointPR* ) j;
    dUASSERT( joint, "bad joint argument" );
    checktype( joint, PR );

    setAxes( joint, x, y, z, joint->axisP1, 0 );

    joint->computeInitialRelativeRotation();
}


void dJointSetPRAxis2( dJointID j, dReal x, dReal y, dReal z )
{
    dxJointPR* joint = ( dxJointPR* ) j;
    dUASSERT( joint, "bad joint argument" );
    checktype( joint, PR );
    setAxes( joint, x, y, z, joint->axisR1, joint->axisR2 );
    joint->computeInitialRelativeRotation();
}


void dJointSetPRParam( dJointID j, int parameter, dReal value )
{
    dxJointPR* joint = ( dxJointPR* ) j;
    dUASSERT( joint, "bad joint argument" );
    checktype( joint, PR );
    if (( parameter & 0xff00 ) == 0x100 )
    {
        joint->limotR.set( parameter & 0xff, value );  // Take only lower part of the
    }                                              // parameter alue
    else
    {
        joint->limotP.set( parameter, value );
    }
}

void dJointGetPRAnchor( dJointID j, dVector3 result )
{
    dxJointPR* joint = ( dxJointPR* ) j;
    dUASSERT( joint, "bad joint argument" );
    dUASSERT( result, "bad result argument" );
    checktype( joint, PR );

    if ( joint->node[1].body )
        getAnchor2( joint, result, joint->anchor2 );
    else
    {
        result[0] = joint->anchor2[0];
        result[1] = joint->anchor2[1];
        result[2] = joint->anchor2[2];
    }
}

void dJointGetPRAxis1( dJointID j, dVector3 result )
{
    dxJointPR* joint = ( dxJointPR* ) j;
    dUASSERT( joint, "bad joint argument" );
    dUASSERT( result, "bad result argument" );
    checktype( joint, PR );
    getAxis( joint, result, joint->axisP1 );
}

void dJointGetPRAxis2( dJointID j, dVector3 result )
{
    dxJointPR* joint = ( dxJointPR* ) j;
    dUASSERT( joint, "bad joint argument" );
    dUASSERT( result, "bad result argument" );
    checktype( joint, PR );
    getAxis( joint, result, joint->axisR1 );
}

dReal dJointGetPRParam( dJointID j, int parameter )
{
    dxJointPR* joint = ( dxJointPR* ) j;
    dUASSERT( joint, "bad joint argument" );
    checktype( joint, PR );
    if (( parameter & 0xff00 ) == 0x100 )
    {
        return joint->limotR.get( parameter & 0xff );
    }
    else
    {
        return joint->limotP.get( parameter );
    }
}

void dJointAddPRTorque( dJointID j, dReal torque )
{
    dxJointPR* joint = ( dxJointPR* ) j;
    dVector3 axis;
    dAASSERT( joint );
    checktype( joint, PR );

    if ( joint->flags & dJOINT_REVERSE )
        torque = -torque;

    getAxis( joint, axis, joint->axisR1 );
    axis[0] *= torque;
    axis[1] *= torque;
    axis[2] *= torque;

    if ( joint->node[0].body != 0 )
        dBodyAddTorque( joint->node[0].body, axis[0], axis[1], axis[2] );
    if ( joint->node[1].body != 0 )
        dBodyAddTorque( joint->node[1].body, -axis[0], -axis[1], -axis[2] );
}


dJointType
dxJointPR::type() const
{
    return dJointTypePR;
}

sizeint
dxJointPR::size() const
{
    return sizeof( *this );
}


void
dxJointPR::setRelativeValues()
{
    dVector3 anchor;
    dJointGetPRAnchor(this, anchor);
    setAnchors( this, anchor[0], anchor[1], anchor[2], offset, anchor2 );

    dVector3 axis;
    dJointGetPRAxis1(this, axis);
    setAxes( this, axis[0], axis[1], axis[2], axisP1, 0 );

    dJointGetPRAxis2(this, axis);
    setAxes( this, axis[0], axis[1], axis[2], axisR1, axisR2 );

    computeInitialRelativeRotation();
}