///////////////////////////////////////////////////////////////////////////////
//  SQuaternion.cpp

#include <math.h>
#include <algorithm>
#include "3DDefine.h"
#include "S3D.h"
#include "SMatrix.h"
#include "SQuaternion.h"

SQuaternion::SQuaternion(void) :
	fX(0.0f),
	fY(0.0f),
	fZ(0.0f),
	fW(1.0f)
{
}

SQuaternion::SQuaternion(float fX_, float fY_, float fZ_, float fW_) :
	fX(fX_),
	fY(fY_),
	fZ(fZ_),
	fW(fW_)
{
}

// クリエイションメソッド
SQuaternion	SQuaternion::CreateIdentity()
{
	SQuaternion Q;
	Q.Identity();

	return Q;
}

SQuaternion SQuaternion::CreateAxisRotation(const S3D& Axis, float RotateRadian)
{
	// Axisは正規化されているものとする

	const float ccc = cosf(0.5f * RotateRadian);
	const float sss = sinf(0.5f * RotateRadian);

	const S3D Vec = Axis * sss;

	return SQuaternion(Vec.fX, Vec.fY, Vec.fZ, ccc);

/*	const float Norm = S3D::DotProduct(Axis, Axis);
	if (Norm <= 0.0f) return SQuaternion(0.0f, 0.0f, 0.0f, 0.0f);;

	const float InvNorm = 1.0f / sqrt(Norm);

	const float ccc = cos(0.5f * RotateRadian);
	const float sss = sin(0.5f * RotateRadian);

	const S3D Vec = Axis * InvNorm * sss;

	return SQuaternion(Vec.fX, Vec.fY, Vec.fZ, ccc);
*/
}

SQuaternion SQuaternion::CreateXRotation(float RotateRadian)
{
	const float ccc = cosf(0.5f * RotateRadian);
	const float sss = sinf(0.5f * RotateRadian);

	return SQuaternion(sss, 0.0f, 0.0f, ccc);
}

SQuaternion SQuaternion::CreateYRotation(float RotateRadian)
{
	const float ccc = cosf(0.5f * RotateRadian);
	const float sss = sinf(0.5f * RotateRadian);

	return SQuaternion(0.0f, sss, 0.0f, ccc);
}

SQuaternion SQuaternion::CreateZRotation(float RotateRadian)
{
	const float ccc = cosf(0.5f * RotateRadian);
	const float sss = sinf(0.5f * RotateRadian);

	return SQuaternion(0.0f, 0.0f, sss, ccc);
}

SQuaternion SQuaternion::CreateFromToRotation(const S3D& u, const S3D& v)
{
	// 「GAME PROGRAMMING Gems1 2.10 最小弧クォータニオン」を参照すること
	// FromToベクトルは正規化されているものとする

	const float m = sqrtf(2.0f + 2.0f * S3D::DotProduct(u, v));
	if (m == 0.0f) return SQuaternion();

	const S3D w = S3D::CrossProduct(u, v) * (1.0f / m);

	return SQuaternion(w.fX, w.fY, w.fZ, 0.5f * m);
}

// operator
SQuaternion SQuaternion::operator+(const SQuaternion& Q) const
{
	return SQuaternion(fX + Q.fX, fY + Q.fY, fZ + Q.fZ, fW + Q.fW);
}

SQuaternion SQuaternion::operator-(const SQuaternion& Q) const
{
	return SQuaternion(fX - Q.fX, fY - Q.fY, fZ - Q.fZ, fW - Q.fW);
}

SQuaternion SQuaternion::operator*(float fT) const
{
	return SQuaternion(fX * fT, fY * fT, fZ * fT, fW * fT);
}

SQuaternion SQuaternion::operator/(float fT) const
{
	const float fInvT = 1.0f / fT;
	return SQuaternion(fX * fInvT, fY * fInvT, fZ * fInvT, fW * fInvT);
}

// operator
SQuaternion SQuaternion::operator*(const SQuaternion& Q) const
{
	SQuaternion   Result;      
	float   d1, d2, d3, d4;      

	d1   =  fW * Q.fW;      
	d2   = -fX * Q.fX;      
	d3   = -fY * Q.fY;      
	d4   = -fZ * Q.fZ;      
	Result.fW = d1+ d2+ d3+ d4; 

	d1   =  fW * Q.fX;      
	d2   =  Q.fW * fX;      
	d3   =  fY * Q.fZ;      
	d4   = -fZ * Q.fY;      
	Result.fX =  d1+ d2+ d3+ d4; 

	d1   =  fW * Q.fY;      
	d2   =  Q.fW * fY;      
	d3   =  fZ * Q.fX;      
	d4   = -fX * Q.fZ;      
	Result.fY =  d1+ d2+ d3+ d4; 

	d1   =  fW * Q.fZ;      
	d2   =  Q.fW * fZ;      
	d3   =  fX * Q.fY;      
	d4   = -fY * Q.fX;      
	Result.fZ =  d1+ d2+ d3+ d4; 

	return   Result;      
}

const SQuaternion& SQuaternion::Identity()
{
	fX = fY = fZ = 0.0f;
	fW = 1.0f;
	
	return *this;
}

const SQuaternion& SQuaternion::Normalize()
{
	const float fMag = sqrtf(fX * fX + fY * fY + fZ * fZ + fW * fW);

	if (fMag > 1.0e-5f) {
		fX /= fMag;
		fY /= fMag;
		fZ /= fMag;
		fW /= fMag;
	}

	return *this;
}

SQuaternion SQuaternion::GetInverse() const
{
	return SQuaternion(-fX, -fY, -fZ, fW);
}

SQuaternion SQuaternion::GetNormalize() const
{
	SQuaternion Obj = *this;
	Obj.Normalize();
	return Obj;
}

//
SMatrix SQuaternion::GetMatrix() const
{
	const float fXX = fX * fX;
	const float fYY = fY * fY;
	const float fZZ = fZ * fZ;

	const float fXY = fX * fY;
	const float fXZ = fX * fZ;
	const float fXW = fX * fW;
	const float fYZ = fY * fZ;
	const float fYW = fY * fW;
	const float fZW = fZ * fW;

	return SMatrix(
		1.0f - 2.0f * (fYY + fZZ),
		       2.0f * (fXY + fZW),
		       2.0f * (fXZ - fYW),
		0.0f,

			   2.0f * (fXY - fZW),
		1.0f - 2.0f * (fZZ + fXX),
		       2.0f * (fYZ + fXW),
		0.0f,

		       2.0f * (fXZ + fYW),
		       2.0f * (fYZ - fXW),
		1.0f - 2.0f * (fYY + fXX),
		0.0f,

		0.0f,
		0.0f,
		0.0f,
		1.0f);
}

void SQuaternion::SetEulerRadXYZ(const S3D& Euler)
{
	// オイラー角の範囲は、B(-180〜180),P(-90〜90),H(-180〜180),である必要がある

	// CreateX〜ZRotationを使用すると正確な値にならないので使用しないこと

	*this = (
		SMatrix::CreateRotPRad(Euler.fX) *
		SMatrix::CreateRotHRad(Euler.fY) *
		SMatrix::CreateRotBRad(Euler.fZ)).GetQuaternion();
}

void SQuaternion::SetEulerRadYXZ(const S3D& Euler)
{
	*this = (
		SMatrix::CreateRotHRad(Euler.fY) *
		SMatrix::CreateRotPRad(Euler.fX) *
		SMatrix::CreateRotBRad(Euler.fZ)).GetQuaternion();
}

void SQuaternion::SetEulerRadZXY(const S3D& Euler)
{
	*this = (
		SMatrix::CreateRotBRad(Euler.fZ) *
		SMatrix::CreateRotPRad(Euler.fX) *
		SMatrix::CreateRotHRad(Euler.fY)).GetQuaternion();
}

void SQuaternion::SetEulerRadZYX(const S3D& Euler)
{
	*this = (
		SMatrix::CreateRotBRad(Euler.fZ) *
		SMatrix::CreateRotHRad(Euler.fY) *
		SMatrix::CreateRotPRad(Euler.fX)).GetQuaternion();
}

void SQuaternion::SetEulerXYZ(const S3D& Euler)
{
	// オイラー角の範囲は、B(-180〜180),P(-90〜90),H(-180〜180),である必要がある

	*this = (
		SMatrix::CreateRotP(Euler.fX) *
		SMatrix::CreateRotH(Euler.fY) *
		SMatrix::CreateRotB(Euler.fZ)).GetQuaternion();
}

void SQuaternion::SetEulerYXZ(const S3D& Euler)
{
	*this = (
		SMatrix::CreateRotH(Euler.fY) *
		SMatrix::CreateRotP(Euler.fX) *
		SMatrix::CreateRotB(Euler.fZ)).GetQuaternion();
}

S3D SQuaternion::GetEulerRadXYZ() const
{
	// X-Y-Z順で回転しているとする

	return GetMatrix().GetEulerRadXYZ();

/*	const SMatrix RotMatrix = GetMatrix();

	const float xx = RotMatrix.m[0][0];
	const float xy = RotMatrix.m[0][1];
	const float xz = RotMatrix.m[0][2];
	const float yx = RotMatrix.m[1][0];
	const float yy = RotMatrix.m[1][1];
	const float yz = RotMatrix.m[1][2];
	const float zx = RotMatrix.m[2][0];
	const float zy = RotMatrix.m[2][1];
	const float zz = RotMatrix.m[2][2];

	float fB = RADtoDEG(static_cast<float>(-atan2(yx, yy)));
	float fP = RADtoDEG(static_cast<float>(asin(yz)));

	float fH = RADtoDEG(static_cast<float>(-asin(xz / cos(DEGtoRAD(fP)))));
	if (zz < 0) fH = 180 - fH;

	return S3D(DEGtoRAD(fP), DEGtoRAD(fH), DEGtoRAD(fB));
*/
/*	S3D Angle;

	// Y回転を求める
	float x2 = fX + fX;
	float y2 = fY + fY;
	float z2 = fZ + fZ;
	float xz2 = fX * z2;
	float wy2 = fW * y2;
	float temp = -(xz2 - wy2);
	// 誤差対策
	if(temp >= 1.f){ temp = 1.f; }
	else if(temp <= -1.f){ temp = -1.f; }
	float yRadian = static_cast<float>(asin(temp));
	Angle.fY = yRadian;
	// 他の回転を求める
	float xx2 = fX * x2;
	float xy2 = fX * y2;
	float zz2 = fZ * z2;
	float wz2 = fW * z2;
	if(yRadian < PI / 2){
		if(yRadian > -PI / 2){
			float yz2 = fY * z2;
			float wx2 = fW * x2;
			float yy2 = fY * y2;
			Angle.fX = static_cast<float>(atan2((yz2 + wx2), (1.0f - (xx2 + yy2))));
			Angle.fZ = static_cast<float>(atan2((xy2 + wz2), (1.0f - (yy2 + zz2))));
		}
		else{
			Angle.fX = -static_cast<float>(atan2((xy2 - wz2), (1.0f - (xx2 + zz2))));
			Angle.fZ = 0.0f;
		}
	}
	else {
		Angle.fX = static_cast<float>(atan2((xy2 - wz2), (1.0f - (xx2 + zz2))));
		Angle.fZ = 0.0f;
	}

//	Angle.fX = RADtoDEG(Angle.fX);
//	Angle.fY = RADtoDEG(Angle.fY);
//	Angle.fZ = RADtoDEG(Angle.fZ);

	return Angle;
*/
/*	S3D Angle;

	float		cx, sx;
	float		cy, sy, yr;
	float		cz, sz;

	// クオータニオンを行列に変換（必要のない部分は無視）
	float _11 = 1.0f - (2.0f * fY * fY) - (2.0f * fZ * fZ);
	float _21 = (2.0f * fX * fY) + (2.0f * fW * fZ);
	float _31 = (2.0f * fX * fZ) - (2.0f * fW * fY);
	float _32 = (2.0f * fY * fZ) + (2.0f * fW * fX);
	float _33 = 1.0f - (2.0f * fX * fX) - (2.0f * fY * fY);

	sy = -_31;
	cy = sqrtf(1 - (sy * sy));
	yr = atan2f(sy, cy);
	Angle.fY= (yr * 180.0f) / PI;

	// Yの角度が90°か270°ではない場合
	if((1.0f != sy) && (-1.0f != sy)){
		cx = _33 / cy;
		sx = _32 / cy;
		Angle.fX = (atan2f(sx, cx) * 180.0f) / PI;

		cz = _11 / cy;
		sz = _21 / cy;
		Angle.fZ = (atan2f(sz, cz) * 180.0f) / PI;
	}
	else{
		float _22 = 1.0f - (2.0f * fX * fX) - (2.0f * fZ * fZ);
		float _23 = (2.0f * fY * fZ) - (2.0f * fW * fX);
		cx = _22;
		sx = -_23;
		Angle.fX = (atan2f(sx, cx) * 180.0f) / PI;

		cz = 1.0f;
		sz = 0.0f;
		Angle.fZ = (atan2f(sz, cz) * 180.0f) / PI;
	}

	return S3D(DEGtoRAD(Angle.fX), DEGtoRAD(Angle.fY), DEGtoRAD(Angle.fZ));;
*/
}

S3D SQuaternion::GetEulerRadYXZ() const
{
	return GetMatrix().GetEulerRadYXZ();
}

S3D SQuaternion::GetEulerRadZXY() const
{
	return GetMatrix().GetEulerRadZXY();
}

S3D SQuaternion::GetEulerRadZYX() const
{
	return GetMatrix().GetEulerRadZYX();
}

S3D SQuaternion::GetEulerXYZ(void) const
{
	return GetEulerRadXYZ() * (180.0f / PI);
}

// 回転量制限
bool SQuaternion::LimitRotate(double LimitRad)
{
	double RadX = asin(static_cast<double>(fX)) * 2.0;
	double RadY = asin(static_cast<double>(fY)) * 2.0;
	double RadZ = asin(static_cast<double>(fZ)) * 2.0;
	double RadW = acos(static_cast<double>(fW)) * 2.0;

	const double MaxRad = (std::max)((std::max)(abs(RadX), abs(RadY)), (std::max)(abs(RadZ), abs(RadW)));

	if (MaxRad <= LimitRad) return false;

	const double MagRad = LimitRad / MaxRad;

	RadX *= MagRad;
	RadY *= MagRad;
	RadZ *= MagRad;
	RadW *= MagRad;

	double NewX = sin(RadX * 0.5);
	double NewY = sin(RadY * 0.5);
	double NewZ = sin(RadZ * 0.5);
	double NewW = cos(RadW * 0.5);

	// 正規化
	const double fMag = sqrt(NewX * NewX + NewY * NewY + NewZ * NewZ + NewW * NewW);

	if (fMag > 1.0e-5) {
		NewX /= fMag;
		NewY /= fMag;
		NewZ /= fMag;
		NewW /= fMag;
	}
	else {
		return false;
	}

	fX = static_cast<float>(NewX);
	fY = static_cast<float>(NewY);
	fZ = static_cast<float>(NewZ);
	fW = static_cast<float>(NewW);

	return true;
}

//
SQuaternion SQuaternion::Lerp(const SQuaternion& Q, float Rate) const
{
	return *this + (Q - *this) * Rate;
}

SQuaternion SQuaternion::Slerp(const SQuaternion& Q, float Rate) const
{
/*	SQuaternion Q3;
	float fDot = fX * Q.fX + fY * Q.fY + fZ * Q.fZ + fW * Q.fW;

	if (fDot < 0.0f) {
		fDot = -fDot;
		Q3 = Q * -1.0f;
	}
	else {
		Q3 = Q;
	}

	if (fDot < 0.95f) {
		const float fAngle = acos(fDot);
		const float fSina = sin(fAngle);
		const float fSinat = sin(fAngle * Rate);
		const float fSinaomt = sin(fAngle * (1.0f - Rate));

		return (*this * fSinaomt + Q3 * fSinat) / fSina;
	}
	else {
		return Lerp(Q3, Rate);
	}
*/
/*	float qr = fX * Q.fX + fY * Q.fY + fZ * Q.fZ + fW * Q.fW;
	float ss = 1.0f - qr * qr;

	if (ss <= 0.0f) return *this;

	float sp = sqrt(ss);
	if (sp == 0.0f) return *this;

	float ph = acos(qr);
	float pt = ph * Rate;
	float t1 = sin(pt) / sp;
	float t0 = sin(ph - pt) / sp;

	return SQuaternion(
		fX * t0 + Q.fX * t1,
		fY * t0 + Q.fY * t1,
		fZ * t0 + Q.fZ * t1,
		fW * t0 + Q.fW * t1);
*/
	float to1[4];
	float scale0, scale1;

	// calc cosine
	float cosom = fX * Q.fX + fY * Q.fY + fZ * Q.fZ + fW * Q.fW;

	// adjust signs (if necessary)
	if (cosom < 0.0f) {
		cosom = -cosom;
		
		to1[0] = - Q.fX;
		to1[1] = - Q.fY;
		to1[2] = - Q.fZ;
		to1[3] = - Q.fW;
	}
	else {
		to1[0] = Q.fX;
		to1[1] = Q.fY;
		to1[2] = Q.fZ;
		to1[3] = Q.fW;
	}

	// calculate coefficients
	const float DELTA = 0.01f;

	if ((1.0f - cosom) > DELTA) {
		// standard case (slerp)
		float omega = acosf(cosom);
		float sinom = sinf(omega);
		scale0 = sinf((1.0f - Rate) * omega) / sinom;
		scale1 = sinf(Rate * omega) / sinom;
	}
	else {        
		// "from" and "to" quaternions are very close 
		//  ... so we can do a linear interpolation
		scale0 = 1.0f - Rate;
		scale1 = Rate;
	}

	// calculate final values
	return SQuaternion(
		scale0 * fX + scale1 * to1[0],
		scale0 * fY + scale1 * to1[1],
		scale0 * fZ + scale1 * to1[2],
		scale0 * fW + scale1 * to1[3]);
}

// 座標系変換
SQuaternion SQuaternion::GetOpenGL() const
{
	return SQuaternion(fX, fY, fZ, -fW);
}