#include <amsculib3/amsculib3.hpp>

namespace amscuda
{

	__host__ __device__ cuvec2f::cuvec2f() 
	{
		x = 0; y = 0; 
		return;
	}

	__host__ __device__ cuvec2f::~cuvec2f() 
	{
		x = 0; y = 0; 
		return;
	}

	__host__ __device__ cuvec2f::cuvec2f(const float &_x, const float &_y)
	{
		x = _x; y = _y; 
		return;
	}

	__host__ __device__ float& cuvec2f::operator[](const int &I)
	{
		switch(I)
		{
			case 0:
				return x;
			case 1:
				return y;
		}

		return x;
	}

	__host__ __device__ const float& cuvec2f::operator[](const int &I) const
	{
		switch(I)
		{
			case 0:
				return x;
			case 1:
				return y;
		}

		return x;
	}

	__host__ __device__ cuvec2f cuvec2f::operator+(const cuvec2f& rhs) const
	{
		cuvec2f ret;
		ret.x = x + rhs.x;
		ret.y = y + rhs.y;
		return ret;
	}

	__host__ __device__ cuvec2f cuvec2f::operator-(const cuvec2f& rhs) const
	{
		cuvec2f ret;
		ret.x = x - rhs.x;
		ret.y = y - rhs.y;
		return ret;
	}

	__host__ __device__ cuvec2f cuvec2f::operator*(const cuvec2f& rhs) const
	{
		//Elementwise product
		cuvec2f ret;
		ret.x = x * rhs.x;
		ret.y = y * rhs.y;
		return ret;
	}

	__host__ __device__ cuvec2f cuvec2f::operator/(const cuvec2f& rhs) const
	{
		//Elementwise division
		cuvec2f ret;
		ret.x = x / rhs.x;
		ret.y = y / rhs.y;
		return ret;
	}

	__host__ __device__ cuvec2f operator*(const cuvec2f& lhs, const float& rhs)
	{
		cuvec2f ret;
		ret.x = lhs.x*rhs;
		ret.y = lhs.y*rhs;
		return ret;
	}

	__host__ __device__ cuvec2f operator*(const float& lhs, const cuvec2f& rhs)
	{
		cuvec2f ret;
		ret.x = lhs*rhs.x;
		ret.y = lhs*rhs.y;
		return ret;
	}

	__host__ __device__ cuvec2f operator/(const cuvec2f& lhs, const float& rhs)
	{
		cuvec2f ret;
		ret.x = lhs.x/rhs;
		ret.y = lhs.y/rhs;
		return ret;
	}

	__host__ __device__ cuvec2f operator/(const float& lhs, const cuvec2f& rhs)
	{
		cuvec2f ret;
		ret.x = lhs/rhs.x;
		ret.y = lhs/rhs.y;
		return ret;
	}

	__host__ __device__ cuvec2f operator-(const cuvec2f& other)
	{
		cuvec2f ret;
		ret.x = -other.x;
		ret.y = -other.y;
		return ret;
	}

	__host__ __device__ cuvec2f& cuvec2f::operator+=(const cuvec2f& rhs)
	{
		x += rhs.x;
		y += rhs.y;
		return *this;
	}

	__host__ __device__ cuvec2f& cuvec2f::operator-=(const cuvec2f& rhs)
	{
		x -= rhs.x;
		y -= rhs.y;
		return *this;
	}

	__host__ __device__ cuvec2f& cuvec2f::operator*=(const float& rhs)
	{
		x *= rhs;
		y *= rhs;
		return *this;
	}

	__host__ __device__ cuvec2f& cuvec2f::operator/=(const float& rhs)
	{
		x /= rhs;
		y /= rhs;
		return *this;
	}

    ////////////////
    //Matrix Class//
    ////////////////

    __host__ __device__ cumat2f::cumat2f() 
	{
		m00 = 0;
		m01 = 0;

		m10 = 0;
		m11 = 0;

		return;
	}

	__host__ __device__ cumat2f::~cumat2f() 
	{
		//m00 = 0;
		//m01 = 0;

		//m10 = 0;
		//m11 = 0;

		return;
	}

	__host__ __device__ cumat2f::cumat2f(
		const float& _m00, const float& _m01, 
		const float& _m10, const float& _m11
	)
	{
		m00 = _m00; 
		m10 = _m10; 

		m01 = _m01; 
		m11 = _m11; 


		return;
	}

	__host__ __device__ cumat2f::cumat2f(const float* data4)
	{
		m00 = data4[0]; 
		m10 = data4[1]; 

		m01 = data4[2]; 
		m11 = data4[3]; 


		return;
	}

	__host__ __device__ float& cumat2f::operator[](const int &I)
	{
		switch(I)
		{
			case 0:
				return m00;
			case 1:
				return m10;
			case 2:
				return m01;
			case 3:
				return m11;
		}

		return m00;
	}

	__host__ __device__ const float& cumat2f::operator[](const int &I) const
	{
		switch(I)
		{
			case 0:
				return m00;
			case 1:
				return m10;
			case 2:
				return m01;
			case 3:
				return m11;
		}

		return m00;
	}

	__host__ __device__ float& cumat2f::operator()(const int &I, const int &J)
	{
		return (*this)[I+2*J];
	}

	__host__ __device__ const float& cumat2f::operator()(const int &I, const int &J) const
	{
		return (*this)[I+2*J];
	}

	__host__ __device__ float& cumat2f::at(const int &I, const int &J)
	{
		return (*this)[I+2*J];
	}

	__host__ __device__ const float& cumat2f::at(const int &I, const int &J) const
	{
		return (*this)[I+2*J];
	}

    __host__ __device__ float* cumat2f::data()
    {
        return (float*)this;
    }

    __host__ __device__ const float* cumat2f::data() const
    {
        return (float*)this;
    }

    __host__ __device__ cumat2f cumat2f::operator+(const cumat2f& rhs) const
    {
        cumat2f ret;
        ret.m00 = m00 + rhs.m00;
        ret.m10 = m10 + rhs.m10;

        ret.m01 = m01 + rhs.m01;
        ret.m11 = m11 + rhs.m11;

        return ret;
    }

    __host__ __device__ cumat2f cumat2f::operator-(const cumat2f& rhs) const
    {
        cumat2f ret;
        ret.m00 = m00 - rhs.m00;
        ret.m10 = m10 - rhs.m10;

        ret.m01 = m01 - rhs.m01;
        ret.m11 = m11 - rhs.m11;

        return ret;
    }

    __host__ __device__ cumat2f cumat2f::operator*(const cumat2f& rhs) const
    {
        cumat2f ret; //should be zeroed in constructor
        ret.m00 = m00*rhs.m00 + m01*rhs.m10;
        ret.m01 = m00*rhs.m01 + m01*rhs.m11;

        ret.m10 = m10*rhs.m00 + m11*rhs.m10;
        ret.m11 = m10*rhs.m01 + m11*rhs.m11;

        return ret;
    }

    __host__ __device__ cumat2f operator*(const cumat2f& lhs, const float& rhs)
    {
        cumat2f ret;
        ret.m00=lhs.m00*rhs;
        ret.m10=lhs.m10*rhs;
        ret.m01=lhs.m01*rhs;
        ret.m11=lhs.m11*rhs;
        return ret;
    }

    __host__ __device__ cumat2f operator/(const cumat2f& lhs, const float& rhs)
    {
        cumat2f ret;
        ret.m00=lhs.m00/rhs;
        ret.m10=lhs.m10/rhs;
        ret.m01=lhs.m01/rhs;
        ret.m11=lhs.m11/rhs;
        return ret;
    }

    __host__ __device__ cumat2f operator*(const float& lhs, const cumat2f& rhs)
    {
        cumat2f ret;
        ret.m00=lhs*rhs.m00;
        ret.m10=lhs*rhs.m10;
        ret.m01=lhs*rhs.m01;
        ret.m11=lhs*rhs.m11;
        return ret;
    }

    __host__ __device__ cuvec2f operator*(const cumat2f& lhs, const cuvec2f& rhs)
    {
        cuvec2f ret;
        ret.x = lhs.m00*rhs.x + lhs.m01*rhs.y;
        ret.y = lhs.m10*rhs.x + lhs.m11*rhs.y;
        return ret;
    }

    __host__ __device__ cuvec2f operator*(const cuvec2f& lhs, const cumat2f& rhs)
    {
        cuvec2f ret;
        ret.x = lhs.x*rhs.m00 + lhs.y*rhs.m10;
        ret.y = lhs.x*rhs.m01 + lhs.y*rhs.m11;
        return ret;
    }

    __host__ __device__ cumat2f operator-(const cumat2f& rhs)
    {
        cumat2f ret;
        ret.m00 = -rhs.m00;
        ret.m10 = -rhs.m10;
        ret.m01 = -rhs.m01;
        ret.m11 = -rhs.m11;
        return ret;
    }

    __host__ __device__ cumat2f& cumat2f::operator+=(const cumat2f& rhs)
    {
        m00 += rhs.m00;
        m10 += rhs.m10;
        m01 += rhs.m01;
        m11 += rhs.m11;
        return *this;
    }

    __host__ __device__ cumat2f& cumat2f::operator-=(const cumat2f& rhs)
    {
        m00 -= rhs.m00;
        m10 -= rhs.m10;
        m01 -= rhs.m01;
        m11 -= rhs.m11;
        return *this;
    }

    __host__ __device__ cumat2f& cumat2f::operator*=(const float& rhs)
    {
        m00 *= rhs;
        m10 *= rhs;
        m01 *= rhs;
        m11 *= rhs;
        return *this;
    }

    __host__ __device__ cumat2f& cumat2f::operator/=(const float& rhs)
    {
        m00 /= rhs;
        m10 /= rhs;
        m01 /= rhs;
        m11 /= rhs;
        return *this;
    }

    __host__ __device__ cumat2f& cumat2f::operator*=(const cumat2f& rhs)
    {
        cumat2f tmp = *this;
        m00 = tmp.m00*rhs.m00 + tmp.m01*rhs.m10;
        m01 = tmp.m00*rhs.m01 + tmp.m01*rhs.m11;
        m10 = tmp.m10*rhs.m00 + tmp.m11*rhs.m10;
        m11 = tmp.m10*rhs.m01 + tmp.m11*rhs.m11;
        return *this;
    }

    __host__ __device__ cumat2f cumat2f::transpose() const
    {
        cumat2f ret;
        ret.m00 = m00;
        ret.m10 = m01;
        ret.m01 = m10;
        ret.m11 = m11;
        return ret;
    }

    ///////////////////
    //Det and Inverse//
    ///////////////////

    __host__ __device__ float cumat2f::det()
    {
        float ret = 0;
    
        ret += m00*m11;
        ret -= m01*m10;
    
        return ret;
    }
    
    __host__ __device__ cumat2f cumat2f::inverse()
    {
        cumat2f q;
        float dt = det();
        if(dt!=0)
        {
            q(0,0) = m11/dt;
            q(0,1) = -m01/dt;
            q(1,0) = -m10/dt;
            q(1,1) = m00/dt;

        }
        else
        {
            q(0,0) = inf;
            q(0,1) = inf;
            q(1,0) = inf;
            q(1,1) = inf;
        }
    
        return q;
    }

    ////////////////////////
    //Non member functions//
    ////////////////////////

    __host__ __device__ cumat2f cumat2f_rot_from_angle(const float &angle)
    {
        cumat2f R;
        R(0,0) = ::cosf(angle);
        R(1,0) = ::sinf(angle);
        R(0,1) = -::sinf(angle);
        R(1,1) = ::cosf(angle);
        return R;
    }

    __host__ __device__ float cuvec2f_dot(const cuvec2f &a, const cuvec2f &b)
    {
        float ret = a.x*b.x+a.y*b.y;
        return ret;
    }

    __host__ __device__ float cuvec2f_cross(const cuvec2f &a, const cuvec2f &b)
    {
        float ret = a.x*b.y-a.y*b.x;
        return ret;
    }

    __host__ __device__ float cuvec2f_norm(const cuvec2f &a)
    {
        float ret = ::sqrtf(a.x*a.x+a.y*a.y);
        return ret;
    }

    __host__ __device__ cuvec2f cuvec2f_normalize(const cuvec2f &a)
    {
        cuvec2f ret;
        float m = cuvec2f_norm(a);
        if(m>0)
        {
            ret.x = a.x/m; ret.y = a.y/m;
        }
        else
        {
            ret.x = 0; ret.y = 0;
        }
        return ret;
    }

    __host__ __device__ cuvec2f cuvec2f_proj(const cuvec2f &a, const cuvec2f &b)
    {
        cuvec2f ret;
        cuvec2f bn = cuvec2f_normalize(b);
        float m = cuvec2f_dot(a,bn);
        ret = bn*m;
        return ret;
    }


void test_cuvec2f_1()
{


    return;
}

};