/*
-----------------------------------------------------------------------------
This source file is part of OpenSpace3D
For the latest info, see http://www.openspace3d.com

Copyright (c) 2010 I-maginer

This program 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; 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 Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public License along with
this program; if not, write to the Free Software Foundation, Inc., 59 Temple
Place - Suite 330, Boston, MA 02111-1307, USA, or go to
http://www.gnu.org/copyleft/lesser.txt

You may alternatively use this source under the terms of a specific version of
the OpenSpace3D Unrestricted License provided you have obtained such a license from
I-maginer.
-----------------------------------------------------------------------------
*/

#ifndef __BTK_PREREQUISITES_H__
#define __BTK_PREREQUISITES_H__

#include <tchar.h>
#include <scol.h>
#include <math.h>

int LoadBitmapToolKit(mmachine m);
int	LoadCaptureToolkit(mmachine m);
int LoadArToolkit(mmachine m);
int UnloadArToolkit();

// disable warning for aruco
#pragma warning(disable : 4290)


class Matrix3
{
  public:

  Matrix3 (float fEntry00, float fEntry01, float fEntry02,
                  float fEntry10, float fEntry11, float fEntry12,
                  float fEntry20, float fEntry21, float fEntry22)
  {
	  m[0][0] = fEntry00;
	  m[0][1] = fEntry01;
	  m[0][2] = fEntry02;
	  m[1][0] = fEntry10;
	  m[1][1] = fEntry11;
	  m[1][2] = fEntry12;
	  m[2][0] = fEntry20;
	  m[2][1] = fEntry21;
	  m[2][2] = fEntry22;
  }


  // member access, allows use of construct mat[r][c]
  inline float* operator[] (size_t iRow) const
  {
    return (float*)m[iRow];
  }

  bool operator== (const Matrix3& rkMatrix) const;
  inline bool operator!= (const Matrix3& rkMatrix) const
	{
		return !operator==(rkMatrix);
	}

  // arithmetic operations
  Matrix3 operator+ (const Matrix3& rkMatrix) const;
  Matrix3 operator- (const Matrix3& rkMatrix) const;
  Matrix3 operator* (const Matrix3& rkMatrix) const;
  Matrix3 operator- () const;

  // matrix * vector [3x3 * 3x1 = 3x1]
  //Vector3 operator* (const Vector3& rkVector) const;

  protected:
    float m[3][3];
};

class Vector3
{
  public:
    Vector3()
    {
      x = 0.0f;
      y = 0.0f;
      z = 0.0f;
    }

    Vector3(float xx, float yy, float zz)
    {
      x = xx;
      y = yy;
      z = zz;
    }
  protected:
  private:

  public:
	  float x;
    float y;
    float z;
  protected:
  private:
};

class Quaternion
{
  public:
    Quaternion()
    {
      w = 1.0f;
      x = 0.0f;
      y = 0.0f;
      z = 0.0f;
    }
  
    Quaternion(float ww, float xx, float yy, float zz)
    {
			w = ww;
      x = xx;
      y = yy;
      z = zz;
    }

    //-----------------------------------------------------------------------
    Quaternion Quaternion::operator+ (const Quaternion& rkQ) const
    {
        return Quaternion(w+rkQ.w,x+rkQ.x,y+rkQ.y,z+rkQ.z);
    }
    //-----------------------------------------------------------------------
    Quaternion Quaternion::operator- (const Quaternion& rkQ) const
    {
        return Quaternion(w-rkQ.w,x-rkQ.x,y-rkQ.y,z-rkQ.z);
    }
    //-----------------------------------------------------------------------
    Quaternion Quaternion::operator* (const Quaternion& rkQ) const
    {
        // NOTE:  Multiplication is not generally commutative, so in most
        // cases p*q != q*p.

        return Quaternion
        (
            w * rkQ.w - x * rkQ.x - y * rkQ.y - z * rkQ.z,
            w * rkQ.x + x * rkQ.w + y * rkQ.z - z * rkQ.y,
            w * rkQ.y + y * rkQ.w + z * rkQ.x - x * rkQ.z,
            w * rkQ.z + z * rkQ.w + x * rkQ.y - y * rkQ.x
        );
    }

    Quaternion Quaternion::Inverse () const
    {
        float fNorm = w*w+x*x+y*y+z*z;
        if ( fNorm > 0.0 )
        {
            float fInvNorm = 1.0f/fNorm;
            return Quaternion(w*fInvNorm,-x*fInvNorm,-y*fInvNorm,-z*fInvNorm);
        }
        else
        {
            // return an invalid result to flag the error
            return Quaternion();
        }
    }

  static Quaternion Quaternion::FromRotationMatrix(double rotMatrix[16], bool reverseX)
  {
    Quaternion mQuat;

    //revert Y axis
    Matrix3 kRot = Matrix3(rotMatrix[0],rotMatrix[4],rotMatrix[8],
                   -rotMatrix[1],-rotMatrix[5],-rotMatrix[9],
								   rotMatrix[2],rotMatrix[6],rotMatrix[10]);

    // Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
    // article "Quaternion Calculus and Fast Animation".

    float fTrace = kRot[0][0]+kRot[1][1]+kRot[2][2];
    float fRoot;

    if ( fTrace > 0.0 )
    { 
        // |w| > 1/2, may as well choose w > 1/2
        fRoot = sqrt(fTrace + 1.0f);  // 2w
        mQuat.w = 0.5f*fRoot;
        fRoot = 0.5f/fRoot;  // 1/(4w)
        mQuat.x = (kRot[2][1]-kRot[1][2])*fRoot;
        mQuat.y = (kRot[0][2]-kRot[2][0])*fRoot;
        mQuat.z = (kRot[1][0]-kRot[0][1])*fRoot;
    }
    else
    {
        // |w| <= 1/2
        static size_t s_iNext[3] = { 1, 2, 0 };
        size_t i = 0;
        if ( kRot[1][1] > kRot[0][0] )
            i = 1;
        if ( kRot[2][2] > kRot[i][i] )
            i = 2;
        size_t j = s_iNext[i];
        size_t k = s_iNext[j];

        fRoot = sqrt(kRot[i][i]-kRot[j][j]-kRot[k][k] + 1.0f);
        float* apkQuat[3] = { &mQuat.x, &mQuat.y, &mQuat.z };
        *apkQuat[i] = 0.5f*fRoot;
        fRoot = 0.5f/fRoot;
        mQuat.w = (kRot[k][j]-kRot[j][k])*fRoot;
        *apkQuat[j] = (kRot[j][i]+kRot[i][j])*fRoot;
        *apkQuat[k] = (kRot[k][i]+kRot[i][k])*fRoot;
    }

		if(reverseX)
		{
			mQuat.x = -mQuat.x;
			mQuat.w = -mQuat.w;
		}
    return mQuat;
  }
  protected:
  private:

  public:
	  float x;
    float y;
    float z;
    float w;
  protected:
  private:
};


#endif