#include <amsculib3/amsculib3.hpp>

namespace amscuda
{

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

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

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

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

		return x;
	}

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

		return x;
	}

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

		m10 = 0;
		m11 = 0;

		return;
	}

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

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

		return;
	}

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

		m01 = _m01; 
		m11 = _m11; 


		return;
	}

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

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


		return;
	}

	__host__ __device__ double& cumat2::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 double& cumat2::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__ double& cumat2::operator()(const int &I, const int &J)
	{
		return (*this)[I+2*J];
	}

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

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

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

    __host__ __device__ double* cumat2::data()
    {
        return (double*)this;
    }

    __host__ __device__ const double* cumat2::data() const
    {
        return (double*)this;
    }

    __host__ __device__ cumat2 cumat2::operator+(const cumat2& rhs) const
    {
        cumat2 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__ cumat2 cumat2::operator-(const cumat2& rhs) const
    {
        cumat2 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__ cumat2 cumat2::operator*(const cumat2& rhs) const
    {
        cumat2 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__ cumat2 operator*(const cumat2& lhs, const double& rhs)
    {
        cumat2 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__ cumat2 operator/(const cumat2& lhs, const double& rhs)
    {
        cumat2 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__ cumat2 operator*(const double& lhs, const cumat2& rhs)
    {
        cumat2 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__ cuvec2 operator*(const cumat2& lhs, const cuvec2& rhs)
    {
        cuvec2 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__ cuvec2 operator*(const cuvec2& lhs, const cumat2& rhs)
    {
        cuvec2 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__ cumat2 operator-(const cumat2& rhs)
    {
        cumat2 ret;
        ret.m00 = -rhs.m00;
        ret.m10 = -rhs.m10;
        ret.m01 = -rhs.m01;
        ret.m11 = -rhs.m11;
        return ret;
    }

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

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

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

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

    __host__ __device__ cumat2& cumat2::operator*=(const cumat2& rhs)
    {
        cumat2 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__ cumat2 cumat2::transpose() const
    {
        cumat2 ret;
        ret.m00 = m00;
        ret.m10 = m01;
        ret.m01 = m10;
        ret.m11 = m11;
        return ret;
    }

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

    __host__ __device__ double cumat2::det()
    {
        double ret = 0;
    
        ret += m00*m11;
        ret -= m01*m10;
    
        return ret;
    }
    
    __host__ __device__ cumat2 cumat2::inverse()
    {
        cumat2 q;
        double 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__ cumat2 cumat2_rot_from_angle(const double &angle)
    {
        cumat2 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__ double cuvec2_dot(const cuvec2 &a, const cuvec2 &b)
    {
        double ret = a.x*b.x+a.y*b.y;
        return ret;
    }

    __host__ __device__ double cuvec2_cross(const cuvec2 &a, const cuvec2 &b)
    {
        double ret = a.x*b.y-a.y*b.x;
        return ret;
    }

    __host__ __device__ double cuvec2_norm(const cuvec2 &a)
    {
        double ret = ::sqrtf(a.x*a.x+a.y*a.y);
        return ret;
    }

    __host__ __device__ cuvec2 cuvec2_normalize(const cuvec2 &a)
    {
        cuvec2 ret;
        double m = cuvec2_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__ cuvec2 cuvec2_proj(const cuvec2 &a, const cuvec2 &b)
    {
        cuvec2 ret;
        cuvec2 bn = cuvec2_normalize(b);
        double m = cuvec2_dot(a,bn);
        ret = bn*m;
        return ret;
    }


void test_cuvec2_1()
{


    return;
}

};