/*
 * SFRotation.cpp
 *
 * Copyright (C) 1999 Stephen F. White
 * 
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program 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 General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program (see the file "COPYING" for details); if 
 * not, write to the Free Software Foundation, Inc., 675 Mass Ave, 
 * Cambridge, MA 02139, USA.
 */

#include <stdio.h>
#include <math.h>
#include "stdafx.h"
#include "DuneApp.h"

#include "SFRotation.h"
#include "EulerAngles.h"

void SFRotation::generateQuaternion()
{ 
 _quaternion=Quaternion(Vec3f(_value[0], _value[1], _value[2]), _value[3]);
}

SFRotation::SFRotation(float x, float y, float z, float w)
{
    _value[0] = x; _value[1] = y; _value[2] = z; _value[3] = w;
    this->normalize();
    generateQuaternion();
    _eulerDirty = true;
}

SFRotation::SFRotation(const float *value)
{
    _value[0] = value[0];
    _value[1] = value[1];
    _value[2] = value[2];
    _value[3] = value[3];
    this->normalize();
    generateQuaternion();
    _eulerDirty = true;
}

SFRotation::SFRotation(Vec3f angles, int mode)
{
    setEulerAngles(angles, mode);
}

SFRotation::SFRotation(void)
{
    _value[0] = 0.0f;
    _value[1] = 0.0f;
    _value[2] = 1.0f;
    _value[3] = 0.0f;
    this->normalize();
    generateQuaternion();
    _eulerDirty = true;
}

void SFRotation::setValue(float v1, float v2, float v3, float v4)
{
    _value[0] = v1;
    _value[1] = v2;
    _value[2] = v3;
    _value[3] = v4;
}

void SFRotation::setValue(int index, float value)
{
    assert(index >= 0 && index < 4);

    _value[index] = value;
}

void SFRotation::normalize()
{
    float rlen = (float) sqrt(_value[0] * _value[0] + 
                              _value[1] * _value[1] +  
                              _value[2] * _value[2] );  
    if (rlen > 0.0f) { 
        rlen = 1.0f / rlen; 

        _value[0] *= rlen;
        _value[1] *= rlen;
        _value[2] *= rlen;
    } else {
        // according to ISO 14997 5.8:
        // SFRotation: The first three values specify a normalized rotation 
        // axis vector about which the rotation takes place. 

        // 0 0 0 ? is not normalized, therefore it would be a invalid SFRotation
        _value[0] = 0;
        _value[1] = 0;
        _value[2] = 1;
        _value[3] = 0;
    }
}

SFRotation::SFRotation(const Quaternion &q)
{
    Quaternion q2=q;
    if (fabs(q2.w) > 1)
       q2.normalize();
    _value[0] = q2.x;
    _value[1] = q2.y;
    _value[2] = q2.z;
    _value[3] = 2.0f * (float) acos(q2.w);
    this->normalize();
    generateQuaternion();
    _eulerDirty = true;
}

SFRotation::SFRotation(const Matrix &mat)
{
    HMatrix	hmat;
    mat.getValue(hmat);
    EulerAngles	a = Eul_FromHMatrix(hmat, EulOrdXYZs);

    setEulerAngles(Vec3f(a.x, a.y, a.z), EulOrdXYZs);
}

bool
SFRotation::equals(const FieldValue *value) const
{
    if (value->getType() == SFROTATION) {
	for (int i = 0; i < 4; i++)
	    if (((SFRotation *) value)->getValue()[i] != _value[i])
		return false;
	return true;
    }
    return false;
}

int SFRotation::write(int f, int /* indent */) const
{
    RET_ONERROR( mywritef(f, "%g %g %g %g\n", 
                          _value[0], _value[1], _value[2], _value[3]) )
    TheApp->incSelectionLinenumber();
    return 0;
}

Vec3f SFRotation::getEulerAngles(int order)
{
    if (_eulerDirty || _eulerOrder != order) {
	Vec3f		v(_value[0], _value[1], _value[2]);
	Quaternion	q(v, _value[3]);
	EulerAngles	a;
	a = Eul_FromQuat(q, order);
	_euler = Vec3f(RAD2DEG(a.x), RAD2DEG(a.y), RAD2DEG(a.z));
	_eulerOrder = order;
	_eulerDirty = false;
    }
    return _euler;
}

const Quaternion &
SFRotation::getQuat() const
{
    return _quaternion;
}

void
SFRotation::setEulerAngles(Vec3f angles, int order)
{
    Quat	q;
    EulerAngles	a;

    a.x = DEG2RAD(angles.x);
    a.y = DEG2RAD(angles.y);
    a.z = DEG2RAD(angles.z);
    a.w = (float) order;

    q = Eul_ToQuat(a);
    _value[0] = q.x;
    _value[1] = q.y;
    _value[2] = q.z;
    _value[3] = 2.0f * (float) acos(q.w);
    _quaternion = Quaternion(q.x, q.y, q.z, q.w);
    this->normalize();
    _euler = angles;
    _eulerOrder = order;
    _eulerDirty = false;
}

Vec3f
SFRotation::operator *(const Vec3f &v) const
{
    return _quaternion * v;
}

MyString
SFRotation::getEcmaScriptComment(MyString name, int flags) const
{
    const char *indent = ((FieldValue *)this)->getEcmaScriptIndent(flags);
    MyString ret;
    ret = "";
    if (TheApp->GetEcmaScriptAddAllowedValues()) {
        ret += indent;
        ret += "// allowed values:\n";

        ret += indent;
        ret += "   // 4 floating point numbers\n";
        ret += indent;
        ret += "   // 3 numbers: normalized axis vector";
        ret += " + 1 number: angle (0 to 2 * pi)\n";
    }
    if (TheApp->GetEcmaScriptAddAllowedComponents()) {
        ret += indent;
        ret += "// allowed components:\n";

        ret += indent;
        ret += "   // x:     ";
        ret += name;
        ret += ".x or ";
        ret += name;
        ret += "[0]\n";

        ret += indent;
        ret += "   // y:     ";
        ret += name;
        ret += ".y or ";
        ret += name;
        ret += "[1]\n";

        ret += indent;
        ret += "   // z:     ";
        ret += name;
        ret += ".z or ";
        ret += name;
        ret += "[2]\n";

        ret += indent;
        ret += "   // angle: ";
        ret += name;
        ret += ".a or ";
        ret += name;
        ret += "[3]\n";
    }
    if (TheApp->GetEcmaScriptAddAvailableFunctions()) {
        ret += indent;
        ret += "// available functions:\n";
        if (flags != EL_EVENT_IN) {
            ret += indent;
            ret += "   // ";
            ret += name;
            ret += " = new SFRotation(float_x, float_y, float_z, float_a);\n";

            ret += indent;
            ret += "   // ";
            ret += name;
            ret += " = new SFRotation(sfvec3f_axis, float_angle);\n";

            ret += indent;
            ret += "   // ";
            ret += name;
            ret += " = new SFRotation(sfvec3f_fromVector, sfvec3f_toVector);\n";

            ret += indent;
            ret += "   // ";
            ret += name;
            ret += " = sfrotation_rot.inverse();\n";

            ret += indent;
            ret += "   // ";
            ret += name;
            ret += " = sfrotation_rot.multiply(sfrotation_rot2);\n";

            ret += indent;
            ret += "   // ";
            ret += name;
            ret += " = sfrotation_rot.slerp(sfrotation_dest,float_t);\n";
       }
       if (flags != EL_EVENT_OUT) {
            ret += indent;
            ret += "   // sfvec3f_axis = ";
            ret += name;
            ret += ".getAxis();\n";

            ret += indent;
            ret += "   // sfvec3f_vec = ";
            ret += name;
            ret += ".multVec(sfvec3f_vec);\n";

            ret += indent;
            ret += "   // ";
            ret += name;
            ret += ".setAxis(sfvec3f_vec);\n";

            ret += indent;
            ret += "   // string_str = ";
            ret += name;
            ret += ".toString();\n";
       }
    }
    if (TheApp->GetEcmaScriptAddExampleUsage()) {
        ret += indent;
        ret += "// example usage:\n";
        if (flags != EL_EVENT_IN) {
             ret += indent;
             ret += "   // ";
             ret += name;
             ret += " = new SFRotation(0, 0, 1, Math.PI / 4.0);\n";

             ret += indent;
             ret += "   // ";
             ret += name;
             ret += ".a = Math.PI / 2.0;\n";
        } 
        if (flags != EL_EVENT_OUT) {
             ret += indent;
             ret += "   // float_x = ";
             ret += name;
             ret += ".x;\n";
        }
        if (flags == EL_FIELD_DEF) {
             ret += indent;
             ret += "   // ";
             ret += name;
             ret += " = ";
             ret += name;
             ret += ".inverse();\n";
        }
    }
    return ret;
}

void
SFRotation::flip(int index) 
{
    _value[index] *= -1.0; 
    _value[3] *= -1.0; 
}