#include "c4d_quaternion.h"
#ifndef __API_INTERN__
	#include "c4d_tools.h"
#endif
#include "ge_math.h"

void Quaternion::SetHPB(const LVector &hpb)
{
	Quaternion qh,qp,qb;
		
	qh.SetAxis(LVector(0,1,0),hpb.x);
	qp.SetAxis(LVector(1,0,0),hpb.y);
	qb.SetAxis(LVector(0,0,1),hpb.z);

	*this=QMul(qb,QMul(qp,qh));
}

Quaternion QAdd(const Quaternion &q1, const Quaternion &q2)
{
	Quaternion r;

  r.w = q1.w+q2.w;
	r.v = q1.v+q2.v;
	return r;
}

Quaternion QSub(const Quaternion &q1, const Quaternion &q2)
{
	Quaternion r;

  r.w = q1.w-q2.w;
	r.v = q1.v-q2.v;
	return r;
}

Quaternion QMul(const Quaternion &q1, const Quaternion &q2)
{
	Quaternion r;

  r.w = (q1.w*q2.w) - (q1.v*q2.v);
	r.v = q1.w*q2.v + q2.w*q1.v + (q1.v%q2.v);
	return r;
}

Quaternion QMul(const Quaternion &q, LReal s)
{
	Quaternion r=q;

	r.w *= s;
	r.v *= s;

	return r;
}


Quaternion QInvert(const Quaternion &q)
{
	Quaternion r;

  r.w = -q.w;
	r.v =  q.v;

	return r;
}


Quaternion QDeriv(const Quaternion &q, const LVector &w)
{
	Quaternion r;

  r.w   = 0.5 * (-q.v * w);
  r.v.x = 0.5 * ( q.w   * w.x - q.v.z * w.y + q.v.y * w.z);
  r.v.y = 0.5 * ( q.v.z * w.x + q.w   * w.y - q.v.x * w.z);
  r.v.z = 0.5 * (-q.v.y * w.x + q.v.x * w.y + q.w   * w.z);

	return r;
}


Quaternion QNorm(const Quaternion &q)
{
  Quaternion r;
	LReal       len;

  len = Sqrt(q.w*q.w + q.v*q.v); if (len==0.0) return q;

  r.w = q.w / len;
  r.v = q.v / len;

	return r;
}

Quaternion QSlerp(const Quaternion &q1, const Quaternion &q2, const LReal alfa)
{
	LReal       sum,teta,beta1,beta2;
	Quaternion q,qd=q2;

	sum = q1.w*q2.w + q1.v*q2.v;

	if (sum<0.0)
	{
		sum=-sum;
		qd.v=-qd.v;
		qd.w=-qd.w;
	}

	if (1.0-sum>EPSILON5)
	{
		teta = ACos(sum);
		LReal sn = 1.0/Sin(teta);
		beta1 = Sin((1.0-alfa)*teta)*sn;
		beta2 = Sin(alfa*teta)*sn;
	}
	else
	{
		beta1 = 1.0-alfa;
		beta2 = alfa;
	}

	q.w   = beta1*q1.w   + beta2*qd.w;
	q.v.x = beta1*q1.v.x + beta2*qd.v.x;
	q.v.y = beta1*q1.v.y + beta2*qd.v.y;
	q.v.z = beta1*q1.v.z + beta2*qd.v.z;

	return q;
}

// q0 = i-1, q1 = i, q2 = i+1, q3 = i+2
Quaternion QSquad(const Quaternion &q0, const Quaternion &q1, const Quaternion &q2, const Quaternion &q3, LReal t)
{
	LReal k = 2.0*(1.0-t)*t;
    return(QSlerp(QSlerp(q0,q3,t),QSlerp(q1,q2,t),k));
}

Quaternion QLogN(const Quaternion &q)
{
	LReal theta, scale;
	Quaternion qq;
	scale = Sqrt(q.v*q.v);
	theta = LReal(atan2(scale,q.w));
	if(scale>0.0) scale = theta/scale;
	qq.v *= scale;
	qq.w = 0.0;
	return qq;
}

// Exp: exponentiate quaternion (where q.w==0)  
Quaternion QExpQ(const Quaternion &q)
{
	LReal theta, scale;
	Quaternion qq;
	theta = Sqrt(q.v*q.v);
	scale = 1.0;
	if(theta>0.0) scale = Sin(theta)/theta;
	qq.v *= scale;
	qq.w = Cos(theta);
	return qq;
}

static Quaternion QTangent(const Quaternion &qn_m1, const Quaternion &qn, const Quaternion &qn_p1)
{
	Quaternion t1,t2;

	t1 = QMul(QInvert(qn),qn_p1);
	t2 = QMul(QInvert(qn),qn_m1);

	t1 = QLogN(t1);
	t2 = QLogN(t2);

	t1.v = (t1.v+t2.v)*(-0.25);
	t1.w = 0.0;


//	t1 = QAdd(t1,t2);
//	t1 = QMul(t1,-0.25);

	t1 = QExpQ(t1);
	t2 = QMul(qn,t1);
	return t2;
}

Quaternion QSpline(const Quaternion &qn_m1, const Quaternion &qn, const Quaternion &qn_p1, const Quaternion &qn_p2, LReal t)
{
	Quaternion an,an_p1;

	an    = QTangent(qn_m1,qn   ,qn_p1);
	an_p1 = QTangent(qn   ,qn_p1,qn_p2);

	return QSquad(qn,an,an_p1,qn_p1,t);
}

Quaternion QBlend(const Quaternion &q1, const Quaternion &q2, const LReal r)
{
	LReal w1 = 1.0-r;
	LReal w2 = r;
	LReal qq = q1.w*q2.w + q1.v*q2.v;
	LReal z = Sqrt((w1-w2)*(w1-w2)+4.0*w1*w2*qq*qq);
	LReal f1 = Sqrt(w1*(w1-w2+z)/(z*(w1+w2+z)));
	LReal f2 = Sqrt(w2*(w2-w1+z)/(z*(w1+w2+z)));

	Quaternion qm;
	if (qq<0.0)
	qm=QSub(QMul(q1,f1),QMul(q2,f2));
	else
	qm=QAdd(QMul(q1,f1),QMul(q2,f2));

	return QNorm(qm);			
}

static LVector LGetHPB(const LVector &v1, const LVector &v2, const LVector &v3)
{
  LVector rot;
  LReal l = Sqrt(v3.x*v3.x+v3.z*v3.z);

  if (l<0.00001)
	{
		rot.x = 0.0;
		rot.z = ACos(LVector(1.0,0.0,0.0) * !v1);

    if (v3.y>0.0)
		{
      rot.y = pi05;
			if (v1.z<0.0) rot.z = pi2-rot.z;
    }
		else
		{
      rot.y = -pi05;
			if (v1.z>0.0) rot.z = pi2-rot.z;
		}
  }
	else
	{
    if (v3.z>0.0)
      rot.x = -ASin(v3.x/l);
    else
      rot.x = pi+ASin(v3.x/l);

		if (rot.x<0.0) rot.x += pi2;
    rot.y = ATan(v3.y/l);
		rot.z = ACos(LVector(Cos(rot.x),0.0,Sin(rot.x)) * !v1);
		if (v1.y>0.0) rot.z = pi2-rot.z;
  }

	if (rot.z>pi) rot.z = rot.z-pi2; 
	return rot;
}

LVector LMatrixToHPB(const LMatrix &m)
{
	return LGetHPB(m.v1,m.v2,m.v3);
}

// ****************************************
inline void LSinCos(LReal w, LReal &sn, LReal &cn) { sn=Sin(w); cn=Cos(w); }

LMatrix LHPBToMatrix(const LVector &w)
{
	LReal cn,sn,ck,sk,cs,ss,cosksinn,sinksinn;

	LSinCos(w.x,ss,cs);
	LSinCos(w.y,sn,cn);
	LSinCos(w.z,sk,ck);

	cosksinn=ck*sn;
	sinksinn=sk*sn;

	return LMatrix(LVector(0.0),
								 LVector(ck*cs-sinksinn*ss,-sk*cn,ck*ss+sinksinn*cs),
								 LVector(sk*cs+cosksinn*ss,ck*cn,sk*ss-cosksinn*cs),
								 LVector(-cn*ss,sn,cn*cs));
}
