/**
 * @file lljointsolverrp3.cpp
 * @brief Implementation of Joint Solver in 3D Real Projective space (RP3). See: https://en.wikipedia.org/wiki/Real_projective_space
 *
 * $LicenseInfo:firstyear=2001&license=viewerlgpl$
 * Second Life Viewer Source Code
 * Copyright (C) 2010, Linden Research, Inc.
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation;
 * version 2.1 of the License only.
 *
 * 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 GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
 *
 * Linden Research, Inc., 945 Battery Street, San Francisco, CA  94111  USA
 * $/LicenseInfo$
 */

//-----------------------------------------------------------------------------
// Header Files
//-----------------------------------------------------------------------------
#include "linden_common.h"

#include "lljointsolverrp3.h"

#include "llmath.h"

#define F_EPSILON 0.00001f

#if LL_RELEASE
    #define DEBUG_JOINT_SOLVER 0
#else
    #define DEBUG_JOINT_SOLVER 1
#endif

//-----------------------------------------------------------------------------
// Constructor
//-----------------------------------------------------------------------------
LLJointSolverRP3::LLJointSolverRP3()
{
    mJointA = NULL;
    mJointB = NULL;
    mJointC = NULL;
    mJointGoal = NULL;
    mLengthAB = 1.0f;
    mLengthBC = 1.0f;
    mPoleVector.setVec( 1.0f, 0.0f, 0.0f );
    mbUseBAxis = false;
    mTwist = 0.0f;
    mFirstTime = true;
}


//-----------------------------------------------------------------------------
// Destructor
//-----------------------------------------------------------------------------
/*virtual*/ LLJointSolverRP3::~LLJointSolverRP3()
{
}


//-----------------------------------------------------------------------------
// setupJoints()
//-----------------------------------------------------------------------------
void LLJointSolverRP3::setupJoints( LLJoint* jointA,
                                    LLJoint* jointB,
                                    LLJoint* jointC,
                                    LLJoint* jointGoal )
{
    mJointA = jointA;
    mJointB = jointB;
    mJointC = jointC;
    mJointGoal = jointGoal;

    mLengthAB = mJointB->getPosition().magVec();
    mLengthBC = mJointC->getPosition().magVec();

    mJointABaseRotation = jointA->getRotation();
    mJointBBaseRotation = jointB->getRotation();
}


//-----------------------------------------------------------------------------
// getPoleVector()
//-----------------------------------------------------------------------------
const LLVector3& LLJointSolverRP3::getPoleVector()
{
    return mPoleVector;
}


//-----------------------------------------------------------------------------
// setPoleVector()
//-----------------------------------------------------------------------------
void LLJointSolverRP3::setPoleVector( const LLVector3& poleVector )
{
    mPoleVector = poleVector;
    mPoleVector.normVec();
}


//-----------------------------------------------------------------------------
// setPoleVector()
//-----------------------------------------------------------------------------
void LLJointSolverRP3::setBAxis( const LLVector3& bAxis )
{
    mBAxis = bAxis;
    mBAxis.normVec();
    mbUseBAxis = true;
}

//-----------------------------------------------------------------------------
// getTwist()
//-----------------------------------------------------------------------------
F32 LLJointSolverRP3::getTwist()
{
    return mTwist;
}


//-----------------------------------------------------------------------------
// setTwist()
//-----------------------------------------------------------------------------
void LLJointSolverRP3::setTwist( F32 twist )
{
    mTwist = twist;
}


//-----------------------------------------------------------------------------
// solve()
//-----------------------------------------------------------------------------
void LLJointSolverRP3::solve()
{

    //-------------------------------------------------------------------------
    // setup joints in their base rotations
    //-------------------------------------------------------------------------
    mJointA->setRotation( mJointABaseRotation );
    mJointB->setRotation( mJointBBaseRotation );

    //-------------------------------------------------------------------------
    // get joint positions in world space
    //-------------------------------------------------------------------------
    LLVector3 aPos = mJointA->getWorldPosition();
    LLVector3 bPos = mJointB->getWorldPosition();
    LLVector3 cPos = mJointC->getWorldPosition();
    LLVector3 gPos = mJointGoal->getWorldPosition();

#if DEBUG_JOINT_SOLVER
    LL_DEBUGS("JointSolver") << "LLJointSolverRP3::solve()" << LL_NEWLINE
                            << "bPosLocal = " << mJointB->getPosition() << LL_NEWLINE
                            << "cPosLocal = " << mJointC->getPosition() << LL_NEWLINE
                            << "bRotLocal = " << mJointB->getRotation() << LL_NEWLINE
                            << "cRotLocal = " << mJointC->getRotation() << LL_NEWLINE
                            << "aPos : " << aPos << LL_NEWLINE
                            << "bPos : " << bPos << LL_NEWLINE
                            << "cPos : " << cPos << LL_NEWLINE
                            << "gPos : " << gPos << LL_ENDL;
#endif

    //-------------------------------------------------------------------------
    // get the poleVector in world space
    //-------------------------------------------------------------------------
    LLMatrix4 worldJointAParentMat;
    if ( mJointA->getParent() )
    {
        worldJointAParentMat = mJointA->getParent()->getWorldMatrix();
    }
    LLVector3 poleVec = rotate_vector( mPoleVector, worldJointAParentMat );

    //-------------------------------------------------------------------------
    // compute the following:
    // vector from A to B
    // vector from B to C
    // vector from A to C
    // vector from A to G (goal)
    //-------------------------------------------------------------------------
    LLVector3 abVec = bPos - aPos;
    LLVector3 bcVec = cPos - bPos;
    LLVector3 acVec = cPos - aPos;
    LLVector3 agVec = gPos - aPos;

    //-------------------------------------------------------------------------
    // compute needed lengths of those vectors
    //-------------------------------------------------------------------------
    F32 abLen = abVec.magVec();
    F32 bcLen = bcVec.magVec();
    F32 agLen = agVec.magVec();

    //-------------------------------------------------------------------------
    // compute component vector of (A->B) orthogonal to (A->C)
    //-------------------------------------------------------------------------
    LLVector3 abacCompOrthoVec = abVec - acVec * ((abVec * acVec)/(acVec * acVec));

#if DEBUG_JOINT_SOLVER
    LL_DEBUGS("JointSolver") << "abVec : " << abVec << LL_NEWLINE
        << "bcVec : " << bcVec << LL_NEWLINE
        << "acVec : " << acVec << LL_NEWLINE
        << "agVec : " << agVec << LL_NEWLINE
        << "abLen : " << abLen << LL_NEWLINE
        << "bcLen : " << bcLen << LL_NEWLINE
        << "agLen : " << agLen << LL_NEWLINE
        << "abacCompOrthoVec : " << abacCompOrthoVec << LL_ENDL;
#endif

    //-------------------------------------------------------------------------
    // compute the normal of the original ABC plane (and store for later)
    //-------------------------------------------------------------------------
    LLVector3 abcNorm;
    if (!mbUseBAxis)
    {
        if( are_parallel(abVec, bcVec, 0.001f) )
        {
            // the current solution is maxed out, so we use the axis that is
            // orthogonal to both poleVec and A->B
            if ( are_parallel(poleVec, abVec, 0.001f) )
            {
                // ACK! the problem is singular
                if ( are_parallel(poleVec, agVec, 0.001f) )
                {
                    // the solutions is also singular
                    return;
                }
                else
                {
                    abcNorm = poleVec % agVec;
                }
            }
            else
            {
                abcNorm = poleVec % abVec;
            }
        }
        else
        {
            abcNorm = abVec % bcVec;
        }
    }
    else
    {
        abcNorm = mBAxis * mJointB->getWorldRotation();
    }

    //-------------------------------------------------------------------------
    // compute rotation of B
    //-------------------------------------------------------------------------
    // angle between A->B and B->C
    F32 abbcAng = angle_between(abVec, bcVec);

    // vector orthogonal to A->B and B->C
    LLVector3 abbcOrthoVec = abVec % bcVec;
    if (abbcOrthoVec.magVecSquared() < 0.001f)
    {
        abbcOrthoVec = poleVec % abVec;
        abacCompOrthoVec = poleVec;
    }
    abbcOrthoVec.normVec();

    F32 agLenSq = agLen * agLen;

    // angle arm for extension
    F32 cosTheta =  (agLenSq - abLen*abLen - bcLen*bcLen) / (2.0f * abLen * bcLen);
    if (cosTheta > 1.0f)
        cosTheta = 1.0f;
    else if (cosTheta < -1.0f)
        cosTheta = -1.0f;

    F32 theta = acos(cosTheta);

    LLQuaternion bRot(theta - abbcAng, abbcOrthoVec);

#if DEBUG_JOINT_SOLVER
    LL_DEBUGS("JointSolver") << "abbcAng      : " << abbcAng << LL_NEWLINE
                            << "abbcOrthoVec : " << abbcOrthoVec << LL_NEWLINE
                            << "agLenSq      : " << agLenSq << LL_NEWLINE
                            << "cosTheta     : " << cosTheta << LL_NEWLINE
                            << "theta        : " << theta << LL_NEWLINE
                            << "bRot         : " << bRot << LL_NEWLINE
                            << "theta abbcAng theta-abbcAng: "
                                << theta*180.0/F_PI << " "
                                << abbcAng*180.0f/F_PI << " "
                                << (theta - abbcAng)*180.0f/F_PI
    << LL_ENDL;
#endif

    //-------------------------------------------------------------------------
    // compute rotation that rotates new A->C to A->G
    //-------------------------------------------------------------------------
    // rotate B->C by bRot
    bcVec = bcVec * bRot;

    // update A->C
    acVec = abVec + bcVec;

    LLQuaternion cgRot;
    cgRot.shortestArc( acVec, agVec );

#if DEBUG_JOINT_SOLVER
    LL_DEBUGS("JointSolver") << "bcVec : " << bcVec << LL_NEWLINE
                            << "acVec : " << acVec << LL_NEWLINE
                            << "cgRot : " << cgRot << LL_ENDL;
#endif

    // update A->B and B->C with rotation from C to G
    abVec = abVec * cgRot;
    bcVec = bcVec * cgRot;
    abcNorm = abcNorm * cgRot;
    acVec = abVec + bcVec;

    //-------------------------------------------------------------------------
    // compute the normal of the APG plane
    //-------------------------------------------------------------------------
    if (are_parallel(agVec, poleVec, 0.001f))
    {
        // the solution plane is undefined ==> we're done
        return;
    }
    LLVector3 apgNorm = poleVec % agVec;
    apgNorm.normVec();

    if (!mbUseBAxis)
    {
        //---------------------------------------------------------------------
        // compute the normal of the new ABC plane
        // (only necessary if we're NOT using mBAxis)
        //---------------------------------------------------------------------
        if( are_parallel(abVec, bcVec, 0.001f) )
        {
            // G is either too close or too far away
            // we'll use the old ABCnormal
        }
        else
        {
            abcNorm = abVec % bcVec;
        }
        abcNorm.normVec();
    }

    //-------------------------------------------------------------------------
    // calcuate plane rotation
    //-------------------------------------------------------------------------
    LLQuaternion pRot;
    if ( are_parallel( abcNorm, apgNorm, 0.001f) )
    {
        if (abcNorm * apgNorm < 0.0f)
        {
            // we must be PI radians off ==> rotate by PI around agVec
            pRot.setQuat(F_PI, agVec);
        }
        else
        {
            // we're done
        }
    }
    else
    {
        pRot.shortestArc( abcNorm, apgNorm );
    }

    //-------------------------------------------------------------------------
    // compute twist rotation
    //-------------------------------------------------------------------------
    LLQuaternion twistRot( mTwist, agVec );

#if DEBUG_JOINT_SOLVER
    LL_DEBUGS("JointSolver") << "abcNorm = " << abcNorm << LL_NEWLINE
                            << "apgNorm = " << apgNorm << LL_NEWLINE
                            << "pRot = " << pRot << LL_NEWLINE
                            << "twist    : " << mTwist*180.0/F_PI << LL_NEWLINE
                            << "twistRot : " << twistRot << LL_ENDL;
#endif

    //-------------------------------------------------------------------------
    // compute rotation of A
    //-------------------------------------------------------------------------
    LLQuaternion aRot = cgRot * pRot * twistRot;

    //-------------------------------------------------------------------------
    // apply the rotations
    //-------------------------------------------------------------------------
    mJointB->setWorldRotation( mJointB->getWorldRotation() * bRot );
    mJointA->setWorldRotation( mJointA->getWorldRotation() * aRot );
}


// End