//-*****************************************************************************
//
// Copyright (c) 2009-2012,
//  Sony Pictures Imageworks, Inc. and
//  Industrial Light & Magic, a division of Lucasfilm Entertainment Company Ltd.
//
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
// *       Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
// *       Redistributions in binary form must reproduce the above
// copyright notice, this list of conditions and the following disclaimer
// in the documentation and/or other materials provided with the
// distribution.
// *       Neither the name of Sony Pictures Imageworks, nor
// Industrial Light & Magic nor the names of their contributors may be used
// to endorse or promote products derived from this software without specific
// prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
//-*****************************************************************************

#include <Alembic/AbcGeom/XformSample.h>
#include <Alembic/AbcGeom/XformOp.h>

#include <ImathMatrix.h>
#include <ImathMatrixAlgo.h>
#include <ImathQuat.h>

#include <math.h>
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif


namespace Alembic {
namespace AbcGeom {
namespace ALEMBIC_VERSION_NS {

//-*****************************************************************************
XformSample::XformSample()
{
    m_setWithOpStack = 0;
    m_inherits = true;
    m_opIndex = 0;
    m_hasBeenRead = false;
}

//-*****************************************************************************
std::size_t XformSample::addOp( XformOp iOp, const Abc::V3d &iVal )
{
    for ( size_t i = 0 ; i < 3 ; ++i )
    {
        iOp.setChannelValue( i, iVal[i] );
    }

    if ( ! m_hasBeenRead )
    {
        ABCA_ASSERT( m_setWithOpStack == 0 || m_setWithOpStack == 1,
                     "Cannot mix addOp() and set<Foo>() methods." );

        m_setWithOpStack = 1;

        m_ops.push_back( iOp );

        return m_ops.size() - 1;
    }
    else
    {
        std::size_t ret = m_opIndex;

        ABCA_ASSERT( m_setWithOpStack == 1,
                     "Cannot mix addOp() and set<Foo>() methods." );

        ABCA_ASSERT( iOp.getType() == m_ops[ret].getType(),
                     "Cannot update mismatched op-type in already-setted "
                     << "XformSample!" );

        m_ops[ret] = iOp;
        ++m_opIndex;
        m_opIndex = m_opIndex % m_ops.size();

        return ret;
    }
}

//-*****************************************************************************
std::size_t XformSample::addOp( XformOp iOp, const Abc::V3d &iAxis,
                                const double iAngleInDegrees )

{
    for ( size_t i = 0 ; i < 3 ; ++i )
    {
        iOp.setChannelValue( i, iAxis[i] );
    }
    iOp.setChannelValue( 3, iAngleInDegrees );

    if ( ! m_hasBeenRead )
    {
        ABCA_ASSERT( m_setWithOpStack == 0 || m_setWithOpStack == 1,
                     "Cannot mix addOp() and set<Foo>() methods." );

        m_setWithOpStack = 1;

        m_ops.push_back( iOp );

        return m_ops.size() - 1;
    }
    else
    {
        std::size_t ret = m_opIndex;

        ABCA_ASSERT( iOp.getType() == m_ops[ret].getType(),
                     "Cannot update mismatched op-type in already-setted "
                     << "XformSample!" );

        ABCA_ASSERT( m_setWithOpStack == 1,
                     "Cannot mix addOp() and set<Foo>() methods." );

        m_ops[ret] = iOp;
        ++m_opIndex;
        m_opIndex = m_opIndex % m_ops.size();

        return ret;
    }
}

//-*****************************************************************************
std::size_t XformSample::addOp( XformOp iOp,
                                const double iSingleAxisRotationInDegrees )

{
    iOp.setChannelValue( 0, iSingleAxisRotationInDegrees );

    if ( ! m_hasBeenRead )
    {
        ABCA_ASSERT( m_setWithOpStack == 0 || m_setWithOpStack == 1,
                     "Cannot mix addOp() and set<Foo>() methods." );

        m_setWithOpStack = 1;

        m_ops.push_back( iOp );

        return m_ops.size() - 1;
    }
    else
    {
        std::size_t ret = m_opIndex;

        ABCA_ASSERT( iOp.getType() == m_ops[ret].getType(),
                     "Cannot update mismatched op-type in already-setted "
                     << "XformSample!" );

        ABCA_ASSERT( m_setWithOpStack == 1,
                     "Cannot mix addOp() and set<Foo>() methods." );

        m_ops[ret] = iOp;
        ++m_opIndex;
        m_opIndex = m_opIndex % m_ops.size();

        return ret;
    }
}

//-*****************************************************************************
std::size_t XformSample::addOp( XformOp iOp, const Abc::M44d &iVal )
{
    for ( size_t i = 0 ; i < 4 ; ++i )
    {
        for ( size_t j = 0 ; j < 4 ; ++j )
        {
            iOp.setChannelValue( ( i * 4 ) + j, iVal.x[i][j] );
        }
    }

    if ( ! m_hasBeenRead )
    {
        ABCA_ASSERT( m_setWithOpStack == 0 || m_setWithOpStack == 1,
                     "Cannot mix addOp() and set<Foo>() methods." );

        m_setWithOpStack = 1;

        m_ops.push_back( iOp );

        return m_ops.size() - 1;
    }
    else
    {
        std::size_t ret = m_opIndex;

        ABCA_ASSERT( iOp.getType() == m_ops[ret].getType(),
                     "Cannot update mismatched op-type in already-setted "
                     << "XformSample!" );

        ABCA_ASSERT( m_setWithOpStack == 1,
                     "Cannot mix addOp() and set<Foo>() methods." );

        m_ops[ret] = iOp;
        ++m_opIndex;
        m_opIndex = m_opIndex % m_ops.size();

        return ret;
    }
}

//-*****************************************************************************
std::size_t XformSample::addOp( const XformOp &iOp )
{
    if ( ! m_hasBeenRead )
    {
        ABCA_ASSERT( m_setWithOpStack == 0 || m_setWithOpStack == 1,
                     "Cannot mix addOp() and set<Foo>() methods." );

        m_setWithOpStack = 1;

        m_ops.push_back( iOp );

        return m_ops.size() - 1;
    }
    else
    {
        std::size_t ret = m_opIndex;

        ABCA_ASSERT( iOp.getType() == m_ops[ret].getType(),
                     "Cannot update mismatched op-type in already-setted "
                     << "XformSample!" );

        ABCA_ASSERT( m_setWithOpStack == 1,
                     "Cannot mix addOp() and set<Foo>() methods." );

        m_ops[ret] = iOp;
        ++m_opIndex;
        m_opIndex = m_opIndex % m_ops.size();

        return ret;
    }
}

//-*****************************************************************************
XformOp XformSample::getOp( std::size_t iIndex ) const
{
    return m_ops[iIndex];
}

//-*****************************************************************************
XformOp &XformSample::operator[]( const std::size_t &iIndex )
{
    return m_ops[iIndex];
}

//-*****************************************************************************
const XformOp &XformSample::operator[]( const std::size_t &iIndex ) const
{
    return m_ops[iIndex];
}

//-*****************************************************************************
std::size_t XformSample::getNumOps() const
{
    return m_ops.size();
}

//-*****************************************************************************
std::size_t XformSample::getNumOpChannels() const
{
    std::size_t ret = 0;
    for ( size_t i = 0 ; i < m_ops.size() ; ++i )
    {
        ret += m_ops[i].getNumChannels();
    }

    return ret;
}

//-*****************************************************************************
void XformSample::setInheritsXforms( bool iInherits )
{
    m_inherits = iInherits;
}

//-*****************************************************************************
bool XformSample::getInheritsXforms() const
{
    return m_inherits;
}

//-*****************************************************************************
void XformSample::setTranslation( const Abc::V3d &iTrans )
{
    XformOp op( kTranslateOperation, kTranslateHint );

    for ( size_t i = 0 ; i < 3 ; ++i )
    {
        op.setChannelValue( i, iTrans[i] );
    }

    if ( ! m_hasBeenRead )
    {
        ABCA_ASSERT( m_setWithOpStack == 0 || m_setWithOpStack == 2,
                     "Cannot mix addOp() and set<Foo>() methods." );

        m_setWithOpStack = 2;

        m_ops.push_back( op );
    }
    else
    {
        std::size_t ret = m_opIndex;

        ABCA_ASSERT( m_setWithOpStack == 2,
                     "Cannot mix addOp() and set<Foo>() methods." );

        ABCA_ASSERT( op.getType() == m_ops[ret].getType(),
                     "Cannot update mismatched op-type in already-setted "
                     << "XformSample!" );

        m_ops[ret] = op;
        ++m_opIndex;
        m_opIndex = m_opIndex % m_ops.size();
    }
}

//-*****************************************************************************
void XformSample::setRotation( const Abc::V3d &iAxis,
                               const double iAngleInDegrees )
{
    XformOp op( kRotateOperation, kRotateHint );

    for ( size_t i = 0 ; i < 3 ; ++i )
    {
        op.setChannelValue( i, iAxis[i] );
    }
    op.setChannelValue( 3, iAngleInDegrees );

    if ( ! m_hasBeenRead )
    {
        ABCA_ASSERT( m_setWithOpStack == 0 || m_setWithOpStack == 2,
                     "Cannot mix addOp() and set<Foo>() methods." );

        m_setWithOpStack = 2;

        m_ops.push_back( op );
    }
    else
    {
        std::size_t ret = m_opIndex;

        ABCA_ASSERT( m_setWithOpStack == 2,
                     "Cannot mix addOp() and set<Foo>() methods." );

        ABCA_ASSERT( op.getType() == m_ops[ret].getType(),
                     "Cannot update mismatched op-type in already-setted "
                     << "XformSample!" );

        m_ops[ret] = op;
        ++m_opIndex;
        m_opIndex = m_opIndex % m_ops.size();
    }
}

//-*****************************************************************************
void XformSample::setScale( const Abc::V3d &iScale )
{
    XformOp op( kScaleOperation, kScaleHint );

    for ( size_t i = 0 ; i < 3 ; ++i )
    {
        op.setChannelValue( i, iScale[i] );
    }

    if ( ! m_hasBeenRead )
    {
        ABCA_ASSERT( m_setWithOpStack == 0 || m_setWithOpStack == 2,
                     "Cannot mix addOp() and set<Foo>() methods." );

        m_setWithOpStack = 2;

        m_ops.push_back( op );
    }
    else
    {
        std::size_t ret = m_opIndex;

        ABCA_ASSERT( m_setWithOpStack == 2,
                     "Cannot mix addOp() and set<Foo>() methods." );

        ABCA_ASSERT( op.getType() == m_ops[ret].getType(),
                     "Cannot update mismatched op-type in already-setted "
                     << "XformSample!" );

        m_ops[ret] = op;
        ++m_opIndex;
        m_opIndex = m_opIndex % m_ops.size();
    }
}

//-*****************************************************************************
void XformSample::setXRotation( const double iAngleInDegrees )
{
    XformOp op( kRotateXOperation, kRotateHint );

    op.setChannelValue( 0, iAngleInDegrees );

    if ( ! m_hasBeenRead )
    {
        ABCA_ASSERT( m_setWithOpStack == 0 || m_setWithOpStack == 2,
                     "Cannot mix addOp() and set<Foo>() methods." );

        m_setWithOpStack = 2;

        m_ops.push_back( op );
    }
    else
    {
        std::size_t ret = m_opIndex;

        ABCA_ASSERT( m_setWithOpStack == 2,
                     "Cannot mix addOp() and set<Foo>() methods." );

        ABCA_ASSERT( op.getType() == m_ops[ret].getType(),
                     "Cannot update mismatched op-type in already-setted "
                     << "XformSample!" );

        m_ops[ret] = op;
        ++m_opIndex;
        m_opIndex = m_opIndex % m_ops.size();
    }
}

//-*****************************************************************************
void XformSample::setYRotation( const double iAngleInDegrees )
{
    XformOp op( kRotateYOperation, kRotateHint );

    op.setChannelValue( 0, iAngleInDegrees );

    if ( ! m_hasBeenRead )
    {
        ABCA_ASSERT( m_setWithOpStack == 0 || m_setWithOpStack == 2,
                     "Cannot mix addOp() and set<Foo>() methods." );

        m_setWithOpStack = 2;

        m_ops.push_back( op );
    }
    else
    {
        std::size_t ret = m_opIndex;

        ABCA_ASSERT( m_setWithOpStack == 2,
                     "Cannot mix addOp() and set<Foo>() methods." );

        ABCA_ASSERT( op.getType() == m_ops[ret].getType(),
                     "Cannot update mismatched op-type in already-setted "
                     << "XformSample!" );

        m_ops[ret] = op;
        ++m_opIndex;
        m_opIndex = m_opIndex % m_ops.size();
    }
}

//-*****************************************************************************
void XformSample::setZRotation( const double iAngleInDegrees )
{
    XformOp op( kRotateZOperation, kRotateHint );

    op.setChannelValue( 0, iAngleInDegrees );

    if ( ! m_hasBeenRead )
    {
        ABCA_ASSERT( m_setWithOpStack == 0 || m_setWithOpStack == 2,
                     "Cannot mix addOp() and set<Foo>() methods." );

        m_setWithOpStack = 2;

        m_ops.push_back( op );
    }
    else
    {
        std::size_t ret = m_opIndex;

        ABCA_ASSERT( m_setWithOpStack == 2,
                     "Cannot mix addOp() and set<Foo>() methods." );

        ABCA_ASSERT( op.getType() == m_ops[ret].getType(),
                     "Cannot update mismatched op-type in already-setted "
                     << "XformSample!" );

        m_ops[ret] = op;
        ++m_opIndex;
        m_opIndex = m_opIndex % m_ops.size();
    }
}

//-*****************************************************************************
void XformSample::setMatrix( const Abc::M44d &iMatrix )
{
    XformOp op( kMatrixOperation, kMatrixHint );

    for ( size_t i = 0 ; i < 4 ; ++i )
    {
        for ( size_t j = 0 ; j < 4 ; ++j )
        {
            op.setChannelValue( ( i * 4 ) + j, iMatrix.x[i][j] );
        }
    }

    if ( ! m_hasBeenRead )
    {
        ABCA_ASSERT( m_setWithOpStack == 0 || m_setWithOpStack == 2,
                     "Cannot mix addOp() and set<Foo>() methods." );

        m_setWithOpStack = 2;

        m_ops.push_back( op );
    }
    else
    {
        std::size_t ret = m_opIndex;

        ABCA_ASSERT( m_setWithOpStack == 2,
                     "Cannot mix addOp() and set<Foo>() methods." );

        ABCA_ASSERT( op.getType() == m_ops[ret].getType(),
                     "Cannot update mismatched op-type in already-setted "
                     << "XformSample!" );

        m_ops[ret] = op;
        ++m_opIndex;
        m_opIndex = m_opIndex % m_ops.size();
    }
}

//-*****************************************************************************
Abc::M44d XformSample::getMatrix() const
{
    Abc::M44d ret;
    ret.makeIdentity();

    for ( std::size_t i = 0 ; i < m_ops.size() ; ++i )
    {
        Abc::M44d m;
        m.makeIdentity();

        XformOp op = m_ops[i];

        XformOperationType otype = op.getType();

        if ( otype == kMatrixOperation )
        {
            for ( std::size_t j = 0 ; j < 4 ; ++j )
            {
                for ( std::size_t k = 0 ; k < 4 ; ++k )
                {
                    m.x[j][k] = op.getChannelValue( ( 4 * j ) + k );
                }
            }
        }
        else if ( otype == kRotateXOperation )
        {
            m.setAxisAngle( Abc::V3d( 1.0, 0.0, 0.0 ),
                            DegreesToRadians( op.getChannelValue( 0 ) ) );
        }
        else if ( otype == kRotateYOperation )
        {
            m.setAxisAngle( Abc::V3d( 0.0, 1.0, 0.0 ),
                            DegreesToRadians( op.getChannelValue( 0 ) ) );
        }
        else if ( otype == kRotateZOperation )
        {
            m.setAxisAngle( Abc::V3d( 0.0, 0.0, 1.0 ),
                            DegreesToRadians( op.getChannelValue( 0 ) ) );
        }
        else
        {
            Abc::V3d vec( op.getChannelValue( 0 ),
                          op.getChannelValue( 1 ),
                          op.getChannelValue( 2 ) );

            if ( otype == kScaleOperation )
            {
                m.setScale( vec );
            }
            else if ( otype == kTranslateOperation )
            {
                m.setTranslation( vec );
            }
            else if ( otype == kRotateOperation )
            {
                m.setAxisAngle( vec,
                                DegreesToRadians( op.getChannelValue( 3 ) ) );
            }

        }
        ret = m * ret;
    }

    return ret;
}

//-*****************************************************************************
Abc::V3d XformSample::getTranslation() const
{
    Abc::M44d m = this->getMatrix();
    return m.translation();
}

//-*****************************************************************************
Abc::V3d XformSample::getScale() const
{
    Abc::V3d scl;
    Imath::extractScaling( this->getMatrix(), scl );
    return scl;
}

//-*****************************************************************************
Abc::V3d XformSample::getAxis() const
{
    Imath::Quatd q = Imath::extractQuat( this->getMatrix() );

    return q.axis();
}

//-*****************************************************************************
double XformSample::getAngle() const
{
    Imath::Quatd q = Imath::extractQuat( this->getMatrix() );

    return RadiansToDegrees( q.angle() );
}

//-*****************************************************************************
double XformSample::getXRotation() const
{
    Abc::V3d rot;
    Imath::extractEulerXYZ( this->getMatrix(), rot );

    return RadiansToDegrees( rot[0] );
}

//-*****************************************************************************
double XformSample::getYRotation() const
{
    Abc::V3d rot;
    Imath::extractEulerXYZ( this->getMatrix(), rot );

    return RadiansToDegrees( rot[1] );
}

//-*****************************************************************************
double XformSample::getZRotation() const
{
    Abc::V3d rot;
    Imath::extractEulerXYZ( this->getMatrix(), rot );

    return RadiansToDegrees( rot[2] );
}

//-*****************************************************************************
void XformSample::freezeTopology()
{
    m_hasBeenRead = true;
}

//-*****************************************************************************
void XformSample::clear()
{
    m_hasBeenRead = false;
    m_ops.resize( 0 );
    m_setWithOpStack = 0;
    m_opIndex = 0;
    m_inherits = true;
}

//-*****************************************************************************
void XformSample::reset()
{
    this->clear();
}

//-*****************************************************************************
bool XformSample::isTopologyEqual( const XformSample & iSample )
{
    if (getNumOps() != iSample.getNumOps())
    {
        return false;
    }

    std::vector<XformOp>::const_iterator opA, opB;
    for ( opA = m_ops.begin(), opB = iSample.m_ops.begin(); opA != m_ops.end();
          ++opA, ++opB )
    {
        if ( opA->getType() != opB->getType() )
        {
            return false;
        }
    }

    return true;
}

} // End namespace ALEMBIC_VERSION_NS
} // End namespace AbcGeom
} // End namespace Alembic
