#ifndef PIMathATH_H
#define PIMathATH_H

#include <cmath>
#include <complex>
#include <stdarg.h>
#include "piincludes.h"

using std::complex;

typedef complex<int> complexi;
typedef complex<float> complexf;
typedef complex<double> complexd;
typedef complex<long double> complexld;

const double deg2rad = atan(1) / 45.;
const double rad2deg = 45. / atan(1);

inline int pow2(int p) {return (int)1 << p;}
template<typename Type> inline int pimin(const Type & f, const Type & s) {return (f > s) ? s : f;}
template<typename Type> inline int pimax(const Type & f, const Type & s) {return (f < s) ? s : f;}

/// Vector

#define PIMV_FOR(v, s) for (uint v = s; v < Size; ++v)

template<uint Size, typename Type = double>
class PIMathVector {
	typedef PIMathVector<Size, Type> _CVector;
public:
	PIMathVector() {resize(Size);}
	PIMathVector(Type fval, ...) {resize(Size); c[0] = fval; va_list vl; va_start(vl, fval); PIMV_FOR(i, 1) c[i] = va_arg(vl, Type); va_end(vl);}
	
	inline uint size() const {return Size;}
	inline _CVector & fill(const Type & v) {PIMV_FOR(i, 0) c[i] = v; return *this;}
	inline _CVector & set(Type fval, ...) {c[0] = fval; va_list vl; va_start(vl, fval); PIMV_FOR(i, 1) c[i] = va_arg(vl, Type); va_end(vl); return *this;}
	inline _CVector & move(const Type & v) {PIMV_FOR(i, 0) c[i] += v; return *this;}
	inline Type lengthSqr() const {Type tv(0); PIMV_FOR(i, 0) tv += c[i] * c[i]; return tv;}
	inline Type length() const {return sqrt(lengthSqr());}
	inline Type manhattanLength() const {Type tv(0); PIMV_FOR(i, 0) tv += fabs(c[i]); return tv;}
	inline Type angleCos(const _CVector & v) const {Type tv = v.length() * length(); return (tv == Type(0) ? Type(0) : ((*this) ^ v) / tv);}
	inline Type angleSin(const _CVector & v) const {Type tv = angleCos(v); return sqrt(Type(1) - tv * tv);}
	inline Type angleRad(const _CVector & v) const {return acos(angleCos(v));}
	inline Type angleDeg(const _CVector & v) const {return acos(angleCos(v)) * rad2deg;}
	inline _CVector projection(const _CVector & v) {Type tv = v.length(); return (tv == Type(0) ? _CVector() : v * (((*this) ^ v) / tv));}
	inline void normalize() {Type tv = length(); if (tv == Type(1)) return; PIMV_FOR(i, 0) c[i] /= tv;}
	inline bool isNull() const {PIMV_FOR(i, 0) if (c[i] != Type(0)) return false; return true;}
	inline bool isOrtho(const _CVector & v) const {return ((*this) ^ v) == Type(0);}
	
	inline Type & at(uint index) {return c[index];}
	inline Type at(uint index) const {return c[index];}
	inline Type & operator [](uint index) {return c[index];}
	inline Type operator [](uint index) const {return c[index];}
	inline void operator =(const _CVector & v) {c = v.c;}
	inline bool operator ==(const _CVector & v) const {PIMV_FOR(i, 0) if (c[i] != v[i]) return false; return true;}
	inline bool operator !=(const _CVector & v) const {return !(*this == c);}
	inline void operator +=(const _CVector & v) {PIMV_FOR(i, 0) c[i] += v[i];}
	inline void operator -=(const _CVector & v) {PIMV_FOR(i, 0) c[i] -= v[i];}
	inline void operator *=(const Type & v) {PIMV_FOR(i, 0) c[i] *= v;}
	inline void operator /=(const Type & v) {PIMV_FOR(i, 0) c[i] /= v;}
	inline _CVector operator -() {_CVector tv; PIMV_FOR(i, 0) tv[i] = -c[i]; return tv;}
	inline _CVector operator +(const _CVector & v) {_CVector tv = _CVector(*this); PIMV_FOR(i, 0) tv[i] += v[i]; return tv;}
	inline _CVector operator -(const _CVector & v) {_CVector tv = _CVector(*this); PIMV_FOR(i, 0) tv[i] -= v[i]; return tv;}
	inline _CVector operator *(const Type & v) {_CVector tv = _CVector(*this); PIMV_FOR(i, 0) tv[i] *= v; return tv;}
	inline _CVector operator /(const Type & v) {_CVector tv = _CVector(*this); PIMV_FOR(i, 0) tv[i] /= v; return tv;}
	inline Type operator ^(const _CVector & v) const {Type tv(0); PIMV_FOR(i, 0) tv += c[i] * v[i]; return tv;}
	
private:
	inline void resize(uint size, const Type & new_value = Type()) {c.resize(size, new_value);}
	PIVector<Type> c;
	
};

template<uint Size, typename Type>
inline std::ostream & operator <<(std::ostream & s, const PIMathVector<Size, Type> & v) {s << '{'; PIMV_FOR(i, 0) {s << v[i]; if (i < Size - 1) s << ", ";} s << '}'; return s;}

typedef PIMathVector<2u, int> PIMathVector2i;
typedef PIMathVector<3u, int> PIMathVector3i;
typedef PIMathVector<4u, int> PIMathVector4i;
typedef PIMathVector<2u, double> PIMathVector2d;
typedef PIMathVector<3u, double> PIMathVector3d;
typedef PIMathVector<4u, double> PIMathVector4d;

/// Matrix

#define PIMM_FOR(c, r) for (uint c = 0; c < Cols; ++c) { for (uint r = 0; r < Rows; ++r) {
#define PIMM_FOR_WB(c, r) for (uint c = 0; c < Cols; ++c) for (uint r = 0; r < Rows; ++r) // without brakes
#define PIMM_FOR_I(c, r) for (uint r = 0; r < Rows; ++r) { for (uint c = 0; c < Cols; ++c) {
#define PIMM_FOR_I_WB(c, r) for (uint r = 0; r < Rows; ++r) for (uint c = 0; c < Cols; ++c) // without brakes
#define PIMM_FOR_C(v) for (uint v = 0; v < Cols; ++v)
#define PIMM_FOR_R(v) for (uint v = 0; v < Rows; ++v)

template<uint Cols, uint Rows = Cols, typename Type = double>
class PIMathMatrix {
	typedef PIMathMatrix<Cols, Rows, Type> _CMatrix;
	typedef PIMathMatrix<Rows, Cols, Type> _CMatrixI;
	typedef PIMathVector<Rows, Type> _CMCol;
	typedef PIMathVector<Cols, Type> _CMRow;
public:
	PIMathMatrix() {resize(Cols, Rows);}
	PIMathMatrix(Type fval, ...) {resize(Cols, Rows); va_list vl; va_start(vl, fval); PIMM_FOR_I_WB(c, r) m[c][r] = (r + c == 0 ? fval : va_arg(vl, Type)); va_end(vl);}
	
	inline uint cols() const {return Cols;}
	inline uint rows() const {return Rows;}
	inline _CMCol col(uint index) {_CMCol tv; PIMM_FOR_R(i) tv[i] = m[index][i]; return tv;}
	inline _CMRow row(uint index) {_CMRow tv; PIMM_FOR_C(i) tv[i] = m[i][index]; return tv;}
	inline _CMatrix & setCol(uint index, const _CMCol & v) {PIMM_FOR_R(i) m[index][i] = v[i]; return *this;}
	inline _CMatrix & setRow(uint index, const _CMRow & v) {PIMM_FOR_C(i) m[i][index] = v[i]; return *this;}
	inline _CMatrix & fill(const Type & v) {PIMM_FOR_WB(c, r) m[c][r] = v; return *this;}
	//inline _CMatrix & set(Type fval, ...) {m[0] = fval; va_list vl; va_start(vl, fval); PIMV_FOR(i, 1) m[i] = va_arg(vl, Type); va_end(vl); return *this;}
	//inline void normalize() {Type tv = length(); if (tv == Type(1)) return; PIMV_FOR(i, 0) m[i] /= tv;}
	inline bool isSquare() const {return cols() == rows();}
	inline bool isIdentity() const {PIMM_FOR_WB(c, r) if ((c == r) ? m[c][r] != Type(1) : m[c][r] != Type(0)) return false; return true;}
	inline bool isNull() const {PIMM_FOR_WB(c, r) if (m[c][r] != Type(0)) return false; return true;}
	
	inline Type & at(uint col, uint row) {return m[col][row];}
	inline Type at(uint col, uint row) const {return m[col][row];}
	inline PIVector<Type> & operator [](uint col) {return m[col];}
	inline PIVector<Type> operator [](uint col) const {return m[col];}
	inline void operator =(const _CMatrix & sm) {m = sm.m;}
	inline bool operator ==(const _CMatrix & sm) const {PIMM_FOR_WB(c, r) if (m[c][r] != sm.m[c][r]) return false; return true;}
	inline bool operator !=(const _CMatrix & sm) const {return !(*this == sm);}
	inline void operator +=(const _CMatrix & sm) {PIMM_FOR_WB(c, r) m[c][r] += sm.m[c][r];}
	inline void operator -=(const _CMatrix & sm) {PIMM_FOR_WB(c, r) m[c][r] -= sm.m[c][r];}
	inline void operator *=(const Type & v) {PIMM_FOR_WB(c, r) m[c][r] *= v;}
	inline void operator /=(const Type & v) {PIMM_FOR_WB(c, r) m[c][r] /= v;}
	inline _CMatrix operator -() {_CMatrix tm; PIMM_FOR_WB(c, r) tm.m[c][r] = -m[c][r]; return tm;}
	inline _CMatrix operator +(const _CMatrix & sm) {_CMatrix tm = _CMatrix(*this); PIMM_FOR_WB(c, r) tm.m[c][r] += sm.m[c][r]; return tm;}
	inline _CMatrix operator -(const _CMatrix & sm) {_CMatrix tm = _CMatrix(*this); PIMM_FOR_WB(c, r) tm.m[c][r] -= sm.m[c][r]; return tm;}
	inline _CMatrix operator *(const Type & v) {_CMatrix tm = _CMatrix(*this); PIMM_FOR_WB(c, r) tm.m[c][r] *= v; return tm;}
	inline _CMatrix operator /(const Type & v) {_CMatrix tm = _CMatrix(*this); PIMM_FOR_WB(c, r) tm.m[c][r] /= v; return tm;}
	
private:
	inline void resize(uint cols, uint rows, const Type & new_value = Type()) {m.resize(cols); PIMM_FOR_C(i) m[i].resize(rows, new_value);}
	PIVector<PIVector<Type> > m;
	
};

template<uint Cols, uint Rows, typename Type>
inline std::ostream & operator <<(std::ostream & s, const PIMathMatrix<Cols, Rows, Type> & m) {s << '{'; PIMM_FOR_I(c, r) s << m[c][r]; if (c < Cols - 1 || r < Rows - 1) s << ", ";} if (r < Rows - 1) s << endl;} s << '}'; return s;}

typedef PIMathMatrix<2u, 2u, int> PIMathMatrix22i;
typedef PIMathMatrix<3u, 3u, int> PIMathMatrix33i;
typedef PIMathMatrix<4u, 4u, int> PIMathMatrix44i;
typedef PIMathMatrix<2u, 2u, double> PIMathMatrix22d;
typedef PIMathMatrix<3u, 3u, double> PIMathMatrix33d;
typedef PIMathMatrix<4u, 4u, double> PIMathMatrix44d;

#undef PIMV_FOR
#undef PIMM_FOR
#undef PIMM_FOR_WB

#endif // PIMathATH_H