#ifndef Foundation_Math_Point_INCLUDED
#define Foundation_Math_Point_INCLUDED

#include <assert.h>
#include <utility>

#include <assert.h>

#include <foundation/TnTypes.h>

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


namespace Tn {

namespace math {

template <int I, typename T>
struct Point {

    static struct NoInit {} no_init;

    static const int dimension = I;
    typedef T component_type;

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

    void zero() {pe<I-1,T>::zero(p);}

    template <typename S>
    Point<I,S> cast() const {
        Point<I,S> result(Point<I,S>::no_init);
        pe<I-1,S>::cast(result.p,p);
        return result;
    }

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

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

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

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


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

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

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

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

    T p[I];
};


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



template <typename T>
struct Point2 : Point<2,T> {

    Point2<T>() : Point<2,T>() {}

    Point2<T>(Point<2,T> const & that) : Point<2,T>(that) {}

    Point2<T>(T a0, T a1) : Point<2,T>(Point<2,T>::no_init) {
        this->p[0] = a0;
        this->p[1] = a1;
    }
};


template <typename T>
struct Point3 : Point<3,T> {

    Point3<T>() : Point<3,T>() {}

    Point3<T>(Point<3,T> const & that) : Point<3,T>(that) {}

    Point3<T>(T a0, T a1, T a2) : Point<3,T>(Point<3,T>::no_init) {
        this->p[0] = a0;
        this->p[1] = a1;
        this->p[2] = a2;
    }
};


template <typename T>
struct Point4 : Point<4,T> {

    Point4<T>() : Point<4,T>() {}

    Point4<T>(Point<4,T> const & that) : Point<4,T>(that) {}

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


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


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


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


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


template <int I, typename T>
Point<I,T> toPoint(Vec<I,T> const & v) {
    Point<I,T> result;
    pe<I-1,T>::copy(result.p, v.v);
    return result;
}


template <int I, typename T>
Vec<I,T> toVec(Point<I,T> const & p) {
    Vec<I,T> result;
    pe<I-1,T>::copy(result.v, p.p);
    return result;
}


template <typename T>
Vec<2,T> noZ(Vec<3,T> const & v) {
    return Vec2<T>(v[0], v[1]);
}


template <typename T>
Point<2,T> noZ(Point<3,T> const & p) {
    return Point2<T>(p[0], p[1]);
}


template <int I, typename T>
Point<I,T> scale(Point<I,T> const & p, T s) {
    Point<I,T> result;
    pe<I-1,T>::scale(result.p,p.p,s);
    return result;
}

    
template <int I, typename T>
bool operator==(const Point<I,T>& a, const Point<I,T>& b) {
    return pe<I-1,T>::isEqual(a.p, b.p);
}
    

template <int I, typename T>
bool operator!=(const Point<I,T>& a, const Point<I,T>& b) {
    return !pe<I-1,T>::isEqual(a.p, b.p);
}
    
template <int I, typename T>
Point<I,T> midpoint(Point<I,T> const & p1, Point<I,T> const & p2)
{
    Point<I,T> retVal;
    for (int i = 0; i < I; ++i)
        retVal[i] = (p1[i]+p2[i]) / T(2.0);
    
    return retVal;
}


typedef Point2<float> Point2f;
typedef Point3<float> Point3f;
typedef Point4<float> Point4f;

typedef Point2<double> Point2d;
typedef Point3<double> Point3d;
typedef Point4<double> Point4d;

typedef Point2<int32_t> Point2i;
typedef Point3<int32_t> Point3i;
typedef Point4<int32_t> Point4i;

} // end namespace math

} // end namespace Tn

#endif

