#pragma once
#ifndef O3D_MATRIX_H
#define O3D_MATRIX_H
#include "array.h"

#include <assert.h>
#include <stdint.h>
#include <cmath>
#include <iostream>
namespace o3d {

template <typename T, uint64_t irows, uint64_t iclos>
class Matrix {
private:
	template<typename uint64_t iirows, uint64_t iiclos>
	class Black {
	public:
		Black() = delete;
		Black(Matrix& m, const uint64_t& iiirows, const uint64_t& iiiclos) :data_(&m),
			rows_(iiirows),
			clos_(iiiclos) {
		}

		template<typename Ty>
		Matrix& operator=(Matrix<Ty, iirows, iiclos>& imtx) {
			Matrix& mt = *data_;
			for (size_t i = rows_; i < iirows; i++) {
				T* t1 = static_cast<T*>(mt) + i * mt.clos() + clos_;
				Ty* t2 = static_cast<Ty*>(imtx) + (i - rows_) * imtx.clos() + 0;
				for (size_t j = clos_; j < iiclos; j++) {
					(*t1) = (*t2);
					++t1;
					++t2;
				}
			}
			return *data_;
		}

		operator Matrix<T, iirows, iiclos>() {
			Matrix<T, iirows, iiclos> mat;
			Matrix& mt = *data_;
			for (size_t i = rows_; i < iirows; i++) {
				T* t1 = static_cast<T*>(mt) + i * mt.clos() + clos_;
				T* t2 = static_cast<T*>(mat) + (i - rows_) * mat.clos() + 0;
				for (size_t j = clos_; j < iiclos; j++) {
					(*t2) = (*t1);
					++t1;
					++t2;
				}
			}
			return mat;
		}


	public:
		Matrix* data_ = nullptr;
		uint64_t rows_ = 0;
		uint64_t clos_ = 0;
	};
public:

	template<uint64_t iitrows, uint64_t iitclos >
	Black<iitrows, iitclos> block(const uint64_t& iiiclos, const uint64_t& iiirows) {
		assert((iitrows + iiiclos) <= irows);
		assert((iitclos + iiirows) <= iclos);
		return Black<iitrows, iitclos>(*this, iiirows, iiiclos);
	}

	template <uint64_t nrows, uint64_t nclos>
	Matrix<T, nrows, nclos> blockt(const uint64_t& iirows,
		const uint64_t& iiclos) {
		assert((nrows + iirows) <= this->rows());
		assert((nclos + iiclos) <= this->clos());
		Matrix<T, nrows, nclos> tmx;
		for (size_t i = 0; i < tmx.rows(); i++) {
			for (size_t j = 0; j < tmx.clos(); j++) {
				tmx[i * tmx.clos() + j] =
					(*this)[(i + iirows) * this->clos() + j + iiclos];
			}
		}
		return tmx;
	}

	T& operator[](const uint64_t& idx) const { return *((T*)(&data_) + idx); }

	operator T* () const { return (T*)(&data_); }
	
	operator const T* () const { return (const T*)(&data_); }
	
	T& x() const { return (*this)[0]; }
	T& y() const { return (*this)[1]; }
	T& z() const { return (*this)[2]; }

	Matrix& operator+=(const Matrix& imx);

	template<uint64_t iiclos>
	Matrix<T, irows, iiclos> operator*(const Matrix<T, iclos, iiclos>& imxt) {
		// 可以优化.
		Matrix<T, irows, iiclos> out = Matrix<T, irows, iiclos>::Zero();
		Matrix<T, iiclos, iclos> imx = imxt.transposition();
		T* it0 = static_cast<T*>(out);
		T* it1 = static_cast<T*>(*this);
		T* it2 = static_cast<T*>(imx);
		for (size_t i = 0; i < out.rows(); i++) {
			for (size_t j = 0; j < out.clos(); j++) {
				it1 = static_cast<T*>(*this);
				it1 += (i * this->clos());
				it2 = static_cast<T*>(imx);
				it2 += (j * imx.clos());
				for (size_t k = 0; k < this->clos(); k++) {
					(*it0) += ((*it1) * (*it2));
					++it1;
					++it2;
				}
				++it0;
			}
		}
		return out;
	}

	static Matrix<T, irows, iclos> Zero();

	static Matrix<T, irows, iclos> Identity();

	double norm();

	Matrix<T, irows, iclos> dot(const Matrix<T, irows, iclos>& imtx) const;

	Matrix<T, irows, iclos>& doted(const Matrix<T, irows, iclos>& imtx) { return Dot(*this, imtx); }

	template <uint64_t tclos>
	Matrix<T, iclos, tclos> cross(const Matrix<T, iclos, tclos>& imtx) const;

	Matrix<T, irows, iclos>& crossed(const Matrix<T, irows, iclos>& imtx);

	Matrix<T, irows, iclos> add(const Matrix<T, irows, iclos>& imx);

	Matrix<T, irows, iclos>& added(const Matrix<T, irows, iclos>& imx) { return Add(*this, imx); }

	Matrix<T, iclos, irows> transposition()const {
		Matrix<T, irows, iclos> outMx = *this;
		return Transposition(outMx);
	}

	Matrix<T, irows, iclos>& transpositioned() {
		static_assert(this->clos() == this->rows());
		return Transposition(*this);
	}

	T& operator()(const uint64_t& iirows, const uint64_t& iiclos) {
		assert(iirows < this->rows());
		assert(iiclos < this->clos());
		return (*(this))[iirows * this->clos() + iiclos];
	}

	bool hasNAN() {
		const T* pt = static_cast<const T*>(*this);
		for (size_t i = 0; i < this->size(); i++) {
			if (std::isnan<T>(*pt)) {
				return true;
			}
		}
		return false;
	}

	bool hasINF() {
		const T* pt = static_cast<const T*>(*this);
		for (size_t i = 0; i < this->size(); i++) {
			if (std::isinf<T>(*pt)) {
				return true;
			}
		}
		return false;
	}
public:
	template<uint64_t itrows, uint64_t itclos>
	Matrix<T, irows, iclos>& insert(uint64_t iirows, uint64_t iiclos, const Matrix<T, itrows, itclos>& iimtx) {
		assert((iirows + iimtx.rows()) < this->rows());
		assert((iiclos + iimtx.clos()) < this->clos());
		for (size_t i = 0; i < iimtx.rows(); i++) {
			for (size_t j = 0; j < iimtx.clos(); j++) {
				(*this)[iirows * this->clos()]
			}
		}
		return *this;
	}
	constexpr  const uint64_t size() const { return irows * iclos; }
	constexpr  const uint64_t clos() const { return iclos; }
	constexpr  const uint64_t rows() const { return irows; }

	inline static Matrix<T, iclos, irows>& Transposition(
		Matrix<T, irows, iclos>& imx) {
		Matrix<T, irows, iclos> tmx = imx;
		for (size_t i = 0; i < tmx.clos(); i++) {
			for (size_t j = 0; j < tmx.rows(); j++) {
				imx[i * tmx.rows() + j] = tmx[j * tmx.clos() + i];
			}
		}
		return *(reinterpret_cast<Matrix<T, iclos, irows>*>(&imx));
	}

	friend std::ostream& operator<<(std::ostream& istrm,
		const Matrix<T, irows, iclos>& imatx) {
		// 可以尝试优化掉.
		const uint64_t trows = imatx.rows();
		const uint64_t tclos = imatx.clos();
		const T* it = static_cast<const T*>(imatx);
		if (trows == 1 || tclos == 1) {
			istrm << "{";
			for (size_t i = 0; i < tclos * trows - 1; ++i, ++it) {
				istrm << *it << ",";
			}
			istrm << *it << "}";
		} else {
			istrm << '\n';
			for (size_t i = 0, j = 1; i < tclos * trows; ++i, ++j, ++it) {
				if (j == tclos) {
					j = 0;
					istrm << *it << "\n";
				} else {
					istrm << *it << " ";
				}
			}
		}
		return istrm;
	}

public:
	o3d::array<o3d::array<T, iclos>, irows> data_;
};

template <typename T>
inline T& CrossSubFun(T& num,
	const T* arr1,
	const T*& arr2,
	const uint64_t& irows,
	const uint64_t& iclos) {
	for (size_t i = 0; i < iclos; i++) {
		num += ((*arr1) * (*arr2));
		++arr1;
		++arr2;
	}
	return num;
}
template <typename T, uint64_t irows, uint64_t iclos, uint64_t tclos>
inline Matrix<T, iclos, tclos>& Cross(const Matrix<T, irows, iclos>& im1,
	Matrix<T, iclos, tclos>& im2) {
	static_assert(im1.rows() == im1.clos());
	Matrix<T, tclos, iclos> tm1 = im2.transposition();
	im2 = Matrix<T, iclos, tclos>::Zero();
	const T* t1 = static_cast<const T*>(im1);
	const T* t2 = static_cast<const T*>(tm1);
	for (size_t i = 0; i < im2.rows(); i++) {
		t2 = static_cast<T*>(tm1);
		for (size_t j = 0; j < im2.clos(); j++) {
			CrossSubFun(im2[i * im2.clos() + j], t1, t2, im1.rows(),
				im1.clos());
		}
		t1 += im1.clos();
	}
	return im2;
}
template <typename T, uint64_t irows, uint64_t iclos>
inline static Matrix<T, irows, iclos>& Dot(Matrix<T, irows, iclos>& im1,
	const Matrix<T, irows, iclos>& im2) {
	T* m1p = static_cast<T*>(im1);
	const T* m2p = static_cast<const T*>(im2);
	for (size_t i = 0; i < m1.size(); i++) {
		(*m1p) = (*mp1) * (*mp2);
		m1p++;
		mp2++;
	}
	return im1;
}
template <typename T, uint64_t irows, uint64_t iclos>
inline static Matrix<T, irows, iclos>& Add(Matrix<T, irows, iclos>& im1,
	const Matrix<T, irows, iclos>& im2) {
	T* m1p = static_cast<T*>(im1);
	const T* m2p = static_cast<const T*>(im2);
	for (size_t i = 0; i < m1.size(); i++) {
		(*m1p) = (*mp1) + (*mp2);
		m1p++;
		mp2++;
	}
	return im1;
}

template<typename T, uint64_t irows, uint64_t iclos>
inline Matrix<T, irows, iclos>& Matrix<T, irows, iclos>::operator+=(const Matrix<T, irows, iclos>& imx) {
	T* it1 = static_cast<T*>(*this);
	const T* it2 = static_cast<const T*>(imx);
	for (size_t i = 0; i < this->size(); i++) {
		(*it1) += (*it2);
		++it1;
		++it2;
	}
	return *this;
}

template<typename T, uint64_t irows, uint64_t iclos>
inline Matrix<T, irows, iclos> Matrix<T, irows, iclos>::Zero() {
	return Matrix<T, irows, iclos>({ 0 });
}

template<typename T, uint64_t irows, uint64_t iclos>
inline Matrix<T, irows, iclos> Matrix<T, irows, iclos>::Identity() {
	Matrix<T, irows, iclos> mtx = Matrix<T, irows, iclos>::Zero();
	T* mp = static_cast<T*>(mtx);
	uint64_t lineNum = std::min(mtx.rows(), mtx.clos());
	for (size_t i = 0; i < lineNum; i++) {
		*mp = 1;
		mp += (mtx.clos() + 1);
	}
	return mtx;
}

template<typename T, uint64_t irows, uint64_t iclos>
inline double Matrix<T, irows, iclos>::norm() {
	static_assert(this->clos() == 1 || this->rows() == 1);
	const T* it = static_cast<const T*>(*this);
	double nomNum = 0;
	for (size_t i = 0; i < this->size(); i++) {
		nomNum += ((*it) * (*it));
		++it;
	}
	return std::sqrt(nomNum);
}

template<typename T, uint64_t irows, uint64_t iclos>
inline Matrix<T, irows, iclos> Matrix<T, irows, iclos>::dot(const Matrix<T, irows, iclos>& imtx) const {
	Matrix<T, irows, iclos> mtx1 = *this;
	Dot(mtx1, imtx);
	return mtx1;
}

template<typename T, uint64_t irows, uint64_t iclos>
inline Matrix<T, irows, iclos>& Matrix<T, irows, iclos>::crossed(const Matrix<T, irows, iclos>& imtx) {
	static_assert(irows == iclos);
	Matrix<T, irows, iclos> mtx1 = imtx;
	Cross(*this, mtx1);
	*this = mtx1;
	return *this;
}

template<typename T, uint64_t irows, uint64_t iclos>
inline Matrix<T, irows, iclos> Matrix<T, irows, iclos>::add(const Matrix<T, irows, iclos>& imx) {
	Matrix<T, irows, iclos> mt = *this;
	Add(mt, imx);
	return mt;
}

template<typename T, uint64_t irows, uint64_t iclos>
template<uint64_t tclos>
inline Matrix<T, iclos, tclos> Matrix<T, irows, iclos>::cross(const Matrix<T, iclos, tclos>& imtx) const {
	Matrix<T, iclos, tclos> mtx1 = imtx;
	Cross(*this, mtx1);
	return mtx1;
}
}  // namespace o3d
#endif