#ifndef Foundation_Math_Vec_INCLUDED
#define Foundation_Math_Vec_INCLUDED


#include <assert.h>
#include <math.h>

#include <boost/static_assert.hpp>

#include <foundation/TnTypes.h>

#include <foundation/Math/misc.hpp>
#include <foundation/Math/pe.hpp>


namespace Tn {

namespace math {

template <int I, typename T>
struct Vec {

    static struct NoInit {} no_init;

    static const int dimension = I;
    typedef T component_type;

    Vec() {pe<I-1,T>::zero(v);}
    Vec(NoInit const &) {}
    Vec(Vec<I,T> const & that) {pe<I-1,T>::copy(v,that.v);}

    void zero() {pe<I-1,T>::zero(v);}
 
    template <typename S>
    Vec<I,S> cast() const {
        Vec<I,S> result(Vec<I,S>::no_init);
        pe<I-1,S>::cast(result.v,v);
        return result;
    }

    T& operator[](int i) {
        assert((i<I) && (i>=0));
        return v[i];
    }

    T const & operator[](int i) const {
        assert((i<I) && (i>=0));
        return v[i];
    }

    template <int J>
    T const & get() {
        BOOST_STATIC_ASSERT((J<I) && (J>=0));
        return v[J];
    }

    template <int J>
    T const & set(T const & t) {
        BOOST_STATIC_ASSERT((J<I) && (J>=0));
        v[J] = t;
    }

    T const & getX() {return get<0>();}
    T const & getY() {return get<1>();}
    T const & getZ() {return get<2>();}
    T const & getW() {return get<3>();}

    void setX(T const & t) {set<0>(t);}
    void setY(T const & t) {set<1>(t);}
    void setZ(T const & t) {set<2>(t);}
    void setW(T const & t) {set<3>(t);}


    Vec<I,T>& operator+=(Vec<I,T> const & that) {
        pe<I-1,T>::add(v,v,that.v);
        return *this;
    }

    Vec<I,T>& operator-=(Vec<I,T> const & that) {
        pe<I-1,T>::sub(v,v,that.v);
        return *this;
    }

    Vec<I,T>& operator*=(T s) {
        pe<I-1,T>::scale(v,v,s);
        return *this;
    }

    Vec<I,T>& operator/=(T s) {
        T inv_s = invert(s);
        pe<I-1,T>::scale(v,v,inv_s);
        return *this;
    }

    T v[I];
};

template <int I, typename T>
typename Vec<I, T>::NoInit Vec<I,T>::no_init;


template <typename T>
struct Vec2 : Vec<2,T> {

    Vec2<T>() : Vec<2,T>() {}

    Vec2<T>(Vec<2,T> const & that) : Vec<2,T>(that) {}

    Vec2<T>(T a0, T a1) : Vec<2,T>(Vec<2,T>::no_init) {
        this->v[0] = a0;
        this->v[1] = a1;
    }
};


template <typename T>
struct Vec3 : Vec<3,T> {

    Vec3<T>() : Vec<3,T>() {}

    Vec3<T>(Vec<3,T> const & that) : Vec<3,T>(that) {}

    Vec3<T>(T a0, T a1, T a2) : Vec<3,T>(Vec<3,T>::no_init) {
        this->v[0] = a0;
        this->v[1] = a1;
        this->v[2] = a2;
    }
};


template <typename T>
struct Vec4 : Vec<4,T> {

    Vec4<T>() : Vec<4,T>() {}

    Vec4<T>(Vec<4,T> const & that) : Vec<4,T>(that) {}

    Vec4<T>(T a0, T a1, T a2, T a3) : Vec<4,T>(Vec<4,T>::no_init) {
        this->v[0] = a0;
        this->v[1] = a1;
        this->v[2] = a2;
        this->v[3] = a3;
    }
};


template <int I, typename T>
T dot(Vec<I,T> const & a, Vec<I,T> const & b) {

    return pe<I-1,T>::dot(a.v, b.v);
}


template <int I, typename T>
T dot(Vec<I,T> const & a) {

    return pe<I-1,T>::dot(a.v, a.v);
}


template <int I, typename T>
Vec<I,T> operator+(Vec<I,T> const & a, Vec<I,T> const & b) {
    Vec<I,T> result;
    pe<I-1,T>::add(result.v, a.v, b.v);
    return result;
}


template <int I, typename T>
Vec<I,T> operator-(Vec<I,T> const & a, Vec<I,T> const & b) {
    Vec<I,T> result;
    pe<I-1,T>::sub(result.v, a.v, b.v);
    return result;
}


template <int I, typename T>
Vec<I,T> operator-(Vec<I,T> const & a) {
    Vec<I,T> result;
    pe<I-1,T>::neg(result.v, a.v);
    return result;
}


template <int I, typename T>
Vec<I,T> operator*(Vec<I,T> const & a, T s) {
    Vec<I,T> result;
    pe<I-1,T>::scale(result.v, a.v, s);
    return result;
}

template <int I, typename T>
Vec<I,T> operator*(T s, Vec<I,T> const & a) {
    Vec<I,T> result;
    pe<I-1,T>::scale(result.v, s, a.v);
    return result;
}


template <int I, typename T>
Vec<I,T> operator/(Vec<I,T> const & a, T s) {
    T inv_s = invert(s);
    Vec<I,T> result;
    pe<I-1,T>::scale(result.v, a.v, inv_s);
    return result;
}


template <int I, typename T>
T magnitude(Vec<I,T> const & v) {return (T)sqrt(dot(v,v));}


template <int I, typename T>
Vec<I,T> normalize(Vec<I,T> const & v) {
    T inv_sqrt = invSqrt(dot(v,v));
    return v * inv_sqrt;
}


template <typename T>
Vec<3,T> cross(Vec<3,T> const & a, Vec<3,T> const & b) {
    return Vec3<T>(a[1]*b[2] - a[2]*b[1],
                   a[2]*b[0] - a[0]*b[2],
                   a[0]*b[1] - a[1]*b[0]);
}


template <typename T>
T cross(Vec<2,T>  const & a, Vec<2,T> const & b) {
    return a[0] * b[1] - b[0] * a[1];
}


template <int I, typename T>
T angle(Vec<I,T>  const & a, Vec<I,T> const & b) {
    return acos(clamp(dot(normalize(a), normalize(b)),-1.0,1.0));
}


typedef Vec2<float> Vec2f;
typedef Vec3<float> Vec3f;
typedef Vec4<float> Vec4f;

typedef Vec2<double> Vec2d;
typedef Vec3<double> Vec3d;
typedef Vec4<double> Vec4d;

typedef Vec2<int32_t> Vec2i;
typedef Vec3<int32_t> Vec3i;
typedef Vec4<int32_t> Vec4i;


} // namespace math

} // namespace Tn


template <int I, typename T>
inline Tn::math::Vec<I,T> invert(Tn::math::Vec<I,T> const & v) {
    return -v;
}


template <int I, typename T>
inline bool isIdentity(Tn::math::Vec<I,T> const & v) {

    return Tn::math::pe<I-1,T>::isZero(v);
}


#endif

