// Copyright (C) 2013 mocchi

#include "IReferenceCounted.h"
#include "CGridSelector.h"
#include "CONObjSceneNode.h"

#include "opennurbs.h"
#include <cstdlib>
#include <cmath>
#include <set>

using namespace irr;
using namespace irr::core;
using namespace irr::scene;
using namespace irr::video;

typedef std::set<CONObjSceneNode *> sns_type;

struct CGridSelector::Impl{
	f32 unitSize; // 固定

	s32 gridSize[3];
	ON_3dPoint baseCenter;
	array<sns_type *> grid;

	Impl(f32 unitSize_, ON_BoundingBox &bb){
		gridSize[0] = gridSize[1] = gridSize[2] = 0;

		baseCenter = ON_3dPoint(0,0,0);
		unitSize = unitSize_;

		s32 minusDir[3], plusDir[3];

		howMuchToGrowFromAABB(bb, minusDir, plusDir);
		growGrid(minusDir, plusDir);
	}

	ON_3dPoint centerPointFromIndex(u32 index){
		u32 xIndex, yIndex, zIndex;
		div_t div_x = std::div(index, gridSize[0]);
		xIndex = div_x.rem;
		div_t div_y = std::div(div_x.quot, gridSize[1]);
		yIndex = div_y.rem;
		zIndex = div_y.quot;

		f64 unitSized = unitSize;

		return baseCenter
			+ ON_3dPoint(xIndex * unitSized, yIndex * unitSized, zIndex * unitSized);
	}

	bool gridIndex3DFromPoint(vector3df &pos, s32 xyzIndex[]){
		f64 unitSized = unitSize;
		f64 unitSized_2 = unitSized * 0.5;
		f64 pt[] = {pos.X, pos.Y, pos.Z};

		for (int i = 0; i < 3; ++i){
			double offset = pt[i] - (baseCenter[i] - unitSized_2);
			if (offset < 0) return false;
			s32 indexU = static_cast<u32>(std::floor(offset / unitSized));
			if (indexU >= gridSize[i]) return false;
			xyzIndex[i] = indexU;
		}
		return true;
	}

	s32 gridIndexFromPoint(vector3df &pos){
		s32 stride[] = {1, gridSize[0], gridSize[0] * gridSize[1]};

		s32 xyzIndex[3];
		if (!gridIndex3DFromPoint(pos, xyzIndex)) return -1;

		s32 index = 0;
		for (int i = 0; i < 3; ++i){
			index += xyzIndex[i] * stride[i];
		}
		return index;
	}

	void growGrid(s32 minusDir[3], s32 plusDir[3]){
		f64 unitSized = unitSize;

		// 新しいグリッドの変数を用意
		s32 newGridSize[3];
		for (int i = 0; i < 3; ++i) newGridSize[i] = gridSize[i] + plusDir[i] + minusDir[i];

		ON_3dPoint newBaseCenter = baseCenter;
		for (int i = 0; i < 3; ++i) newBaseCenter[i] -= minusDir[i] * unitSized;

		array<sns_type *> newGrid;
		newGrid.reallocate(newGridSize[0] * newGridSize[1] * newGridSize[2]);
		for (u32 i = 0; i < newGrid.allocated_size(); ++i) newGrid.push_back(0);

		// 今までのグリッドから新しいグリッドへ転送
		s32 stride[] = {1, gridSize[0], gridSize[0] * gridSize[1]};
		s32 newStride[] = {1, newGridSize[0], newGridSize[0] * newGridSize[1]};
		for (int k = 0, kji = 0; k < gridSize[2]; ++k, kji += stride[2]){
			int nkji = (k + minusDir[2]) * newStride[2];
			for (int j = 0, ji = 0; j < gridSize[1]; ++j, ji += stride[1]){
				int nji = (j + minusDir[1]) * newStride[1];
				for (int i = 0; i < gridSize[0]; ++i){
					int ni = (i + minusDir[0]) * newStride[0];
					// gridはすぐに消すので、grab dropは省略
					newGrid[ni + nji + nkji] = grid[i + ji + kji];
				}
			}
		}

		// 今までのグリッドと新しいグリッドを差し替え
		for (int i = 0; i < 3; ++i) gridSize[i] = newGridSize[i];
		baseCenter = newBaseCenter;
		for (u32 i = 0; i < grid.size(); ++i) delete grid[i];
		grid.swap(newGrid);
	}

	bool howMuchToGrowFromAABB(ON_BoundingBox &bb, s32 minusDir[3], s32 plusDir[3]){
		f64 unitSized = unitSize;
		f64 unitSized_2 = unitSized * 0.5;

		for (int i = 0; i < 3; ++i){
			double underflow = (baseCenter[i] - unitSized_2) - bb.m_min[i];
			if (underflow < 0) minusDir[i] = 0;
			else minusDir[i] = static_cast<int>(std::ceil(underflow / unitSized));
		}

		for (int i = 0; i < 3; ++i){
			double overflow = bb.m_max[i] - ((baseCenter[i] - unitSized_2) + unitSized * gridSize[i]);
			if (overflow < 0) plusDir[i] = 0;
			else plusDir[i] = static_cast<int>(std::ceil(overflow / unitSized));
		}
		for (int i = 0; i < 3; ++i) if (minusDir[i] != 0 || plusDir[i] != 0) return true;
		return false;
	}

	// 戻り値:Growする必要があるかどうか true:ある、false:なし
	bool howMuchToGrowFromPoint(ON_3dPoint &pt, s32 minusDir[3], s32 plusDir[3]){
		return howMuchToGrowFromAABB(ON_BoundingBox(pt, pt), minusDir, plusDir);
	}

	// F(s32 index, vector3df &v) : index番目のvを求めるファンクタ
	template <typename F> bool addVertices(CONObjSceneNode *sn, F &getVertex, s32 numCods){
		std::set<s32> indices;
		for (s32 i = 0; i < numCods; ++i){
			vector3df vertex;
			getVertex(i, vertex);
			ON_3dPoint pt(vertex.X, vertex.Y, vertex.Z);
			s32 minusDir[3], plusDir[3];
			if (howMuchToGrowFromPoint(pt, minusDir, plusDir)){
				growGrid(minusDir, plusDir);
			}
			s32 index = gridIndexFromPoint(vertex);
			if (index < 0) return false; // Grow処理があるため、indexは必ず正になるはず
			indices.insert(index);
		}
		// 上記処理で失敗した場合、中途半端に登録した状態にならないように、
		// 登録対象を一度arrayにいれて、↓で登録
		for (std::set<s32>::iterator iter = indices.begin(); iter != indices.end(); ++iter){
			s32 index = *iter;
			if (!grid[index]) grid[index] = new sns_type();
			grid[index]->insert(sn);
		}
		return true;
	}

	// F(s32 index, ON_BoundingBox &bb) : index番目のBoindingBoxを求めるファンクタ
	template <typename F> bool addBoundingBoxes(CONObjSceneNode *sn, F &getBBs, s32 numBBs){
		std::set<s32> indices;
		for (s32 l = 0; l < numBBs; ++l){
			ON_BoundingBox bb;
			getBBs(l, bb);
			vector3df bbMin(static_cast<f32>(bb.m_min[0]), static_cast<f32>(bb.m_min[1]), static_cast<f32>(bb.m_min[2]));
			vector3df bbMax(static_cast<f32>(bb.m_max[0]), static_cast<f32>(bb.m_max[1]), static_cast<f32>(bb.m_max[2]));
			s32 minusDir[3], plusDir[3];
			if (howMuchToGrowFromAABB(bb, minusDir, plusDir)){
				growGrid(minusDir, plusDir);
			}
			s32 indexXYZ[2][3];
			gridIndex3DFromPoint(bbMin, indexXYZ[0]);
			gridIndex3DFromPoint(bbMax, indexXYZ[1]);
			s32 stride[] = {1, gridSize[0], gridSize[0] * gridSize[1]};
			for (s32 k = indexXYZ[0][2]; k <= indexXYZ[1][2]; ++k){
				for (s32 j = indexXYZ[0][1]; j <= indexXYZ[1][1]; ++j){
					for (s32 i = indexXYZ[0][0]; i <= indexXYZ[1][0]; ++i){
						s32 index = k * stride[2] + j * stride[1] + i;
						indices.insert(index);
					}
				}
			}
		}
		for (std::set<s32>::iterator iter = indices.begin(); iter != indices.end(); ++iter){
			s32 index = *iter;
			if (!grid[index]) grid[index] = new sns_type();
			grid[index]->insert(sn);
		}
		return true;
	}
};

CGridSelector::CGridSelector() : pimpl(0){
}

void CGridSelector::Initialize(f32 unitSize, ON_BoundingBox &bb){
	if (pimpl) delete pimpl;
	pimpl = new Impl(unitSize, bb);
}

void CGridSelector::Initialize(f32 unitSize, aabbox3d<f32> &bb){
	Initialize(unitSize, ON_BoundingBox(
		ON_3dPoint(bb.MinEdge.X, bb.MinEdge.Y, bb.MinEdge.Z),
		ON_3dPoint(bb.MaxEdge.X, bb.MaxEdge.Y, bb.MaxEdge.Z)
	));
}

CGridSelector::~CGridSelector(){
	delete pimpl;
}

void CGridSelector::growGrid(ON_BoundingBox &bb){
	if (!pimpl) return; // Initializeが実施されていない
	s32 minusDir[3], plusDir[3];
	if (pimpl->howMuchToGrowFromAABB(bb, minusDir, plusDir)){
		pimpl->growGrid(minusDir, plusDir);
	}
}

void CGridSelector::getBoundingBox(ON_BoundingBox &bb){
	f64 unitSized = static_cast<f64>(pimpl->unitSize);
	f64 unitSized_2 = unitSized * 0.5;
	s32 *gs = pimpl->gridSize;

	bb.m_min = pimpl->baseCenter - ON_3dVector(unitSized_2, unitSized_2, unitSized_2);
	bb.m_max = pimpl->baseCenter +
		ON_3dVector(unitSized * gs[0] - unitSized_2, unitSized * gs[1] - unitSized_2, unitSized * gs[2] - unitSized_2);
}


bool CGridSelector::addVertices(CONObjSceneNode *sn, vector3df *vertices, u32 num){
	struct Functor{
		vector3df *vertices; s32 num;
		Functor(vector3df *v_, s32 n_) : vertices(v_), num(n_){}
		void operator () (s32 index, vector3df &v){
			v = vertices[index];
		}
	}f(vertices, num);
	return pimpl->addVertices(sn, f, num);
}
bool CGridSelector::addVertices(CONObjSceneNode *sn, S3DVertex *vertices, u32 num){
	struct Functor{
		S3DVertex *vertices; s32 num;
		Functor(S3DVertex *v_, s32 n_) : vertices(v_), num(n_){}
		void operator () (s32 index, vector3df &v){
			v = vertices[index].Pos;
		}
	}f(vertices, num);
	return pimpl->addVertices(sn, f, num);
}
bool CGridSelector::addTriangles(CONObjSceneNode *sn, u32 *indices, S3DVertex *vertices, u32 numIndices){
	struct Functor{
		u32 *indices;
		S3DVertex *vertices; s32 num;
		Functor(u32 *i_, S3DVertex *v_, s32 n_) : indices(i_), vertices(v_), num(n_){}
		void operator () (s32 index, ON_BoundingBox &bb){
			bb.Destroy();
			for (s32 i = 0; i < 3; ++i){
				vector3df &v = vertices[indices[index*3+i]].Pos;
				bb.Set(ON_3dPoint(v.X, v.Y, v.Z), true);
			}
		}
	}f(indices, vertices, numIndices);
	return pimpl->addBoundingBoxes(sn, f, numIndices/3);
}

void CGridSelector::queryBoxFromThickRay(
		const line3df &ray,
		f32 thickness,
		bool rough,
		array<QueryResult> &results){

	if (!pimpl) return; // Initializeが実施されていない

	double unitSized_2 = static_cast<double>(pimpl->unitSize) * 0.5;
	double thickness2 = thickness;
	double radius = 2.0 * unitSized_2 * unitSized_2; // 外接球の半径
	double thicked_radius2 = std::sqrt(2.0 * unitSized_2 * unitSized_2) + thickness;
	thicked_radius2 *= thicked_radius2;

	ON_3dVector harf3d(unitSized_2, unitSized_2, unitSized_2);
	ON_Line lin(ON_3dPoint(ray.start.X, ray.start.Y, ray.start.Z), ON_3dPoint(ray.end.X, ray.end.Y, ray.end.Z));

#if 0
	// dbg
	ONX_Model model_dbg;
	{
		ONX_Model_Object &obj = model_dbg.m_object_table.AppendNew();
		obj.m_object = new ON_LineCurve(lin);
		obj.m_attributes.SetColorSource(ON::color_from_object);
		obj.m_attributes.m_color.SetRGB(255,255,255);
		obj.m_bDeleteObject = false;
	}
	// dbg
#endif
	for (u32 i = 0; i < pimpl->grid.size(); ++i){
		ON_3dPoint pt = pimpl->centerPointFromIndex(i);
		ON_BoundingBox bb(pt - harf3d, pt + harf3d);
		bool inside;
		if (rough){
			inside = ((lin.ClosestPointTo(pt) - pt).LengthSquared() < thicked_radius2);
		}else{
			double dist = bb.MinimumDistanceTo(lin);
			inside = (dist < static_cast<double>(thickness));
		}
		if (inside){
			sns_type * sns = pimpl->grid[i];
			if (!sns) continue;

			results.push_back(QueryResult());
			QueryResult &qr = results.getLast();
			qr.box = bb;
			for (sns_type::iterator iter = sns->begin(); iter != sns->end(); ++iter){
				(*iter)->grab();
				qr.sceneNodes.push_back(*iter);
			}
#if 0
			// dbg
			ON_LineCurve *lc[2] = {
				new ON_LineCurve(bb.m_min, bb.m_max),
				new ON_LineCurve(
					ON_3dPoint(bb.m_min.x, bb.m_max.y, bb.m_min.z),
					ON_3dPoint(bb.m_max.x, bb.m_min.y, bb.m_max.z)
				)
			};

			for (int h = 0; h < 2; ++h){
				ONX_Model_Object &obj = model_dbg.m_object_table.AppendNew();
				obj.m_object = lc[h];
				obj.m_attributes.SetColorSource(ON::color_from_object);
				obj.m_attributes.m_color.SetRGB(255,255,255);
				obj.m_bDeleteObject = false;
			}
			// dbg
#endif

		}
	}

#if 0
	// dbg
	model_dbg.Write("d:/penguin_gridmesh_ray.3dm", 4);
	// dbg
#endif
}


bool CGridSelector::getGridMesh(ON_Mesh &mesh){
	mesh.Destroy();
	double unitSized = pimpl->unitSize;
	double unitSized_2 = unitSized * 0.5;
	ON_3dVector offsets[] = {
		ON_3dVector(-unitSized_2, -unitSized_2, -unitSized_2),
		ON_3dVector( unitSized_2, -unitSized_2, -unitSized_2),
		ON_3dVector(-unitSized_2,  unitSized_2, -unitSized_2),
		ON_3dVector( unitSized_2,  unitSized_2, -unitSized_2),
		ON_3dVector(-unitSized_2, -unitSized_2,  unitSized_2),
		ON_3dVector( unitSized_2, -unitSized_2,  unitSized_2),
		ON_3dVector(-unitSized_2,  unitSized_2,  unitSized_2),
		ON_3dVector( unitSized_2,  unitSized_2,  unitSized_2)
	};
	//   0    1
	//    +---+
	//  2/  3/|
	//  +---+ |
	//  |4+-|-+5
	//  |/  |/
	// 6+---+7
	int indices[] = {
		0, 2, 3, 1,
		0, 1, 5, 4,
		4, 5, 7, 6,
		2, 6, 7, 3,
		0, 4, 6, 2,
		1, 3, 7, 5
	};
	for (u32 j = 0; j < pimpl->grid.size(); ++j){
		if (!pimpl->grid[j] || pimpl->grid[j]->size() == 0) continue;
		double rgba[4] = {0};
		for (sns_type::iterator iter = pimpl->grid[j]->begin(); iter != pimpl->grid[j]->end(); ++iter){
			SMaterial &m = (*iter)->Material;
			rgba[0] += m.AmbientColor.getRed() + m.DiffuseColor.getRed() + m.EmissiveColor.getRed();
			rgba[1] += m.AmbientColor.getGreen() + m.DiffuseColor.getGreen() + m.EmissiveColor.getGreen();
			rgba[2] += m.AmbientColor.getBlue() + m.DiffuseColor.getBlue() + m.EmissiveColor.getBlue();
			rgba[3] += m.AmbientColor.getAlpha() + m.DiffuseColor.getAlpha() + m.EmissiveColor.getAlpha();
		}
		for (int i = 0; i < 4; ++i) rgba[i] /= (pimpl->grid[j]->size() * 4.0);
		ON_3dPoint center = pimpl->centerPointFromIndex(j);
		for (u32 i = 0; i < 6; ++i){
			ON_MeshFace &mf = mesh.m_F.AppendNew();
			for (int h = 0; h < 4; ++h) mf.vi[h] = indices[i*4+h] + mesh.m_V.Count();
		}
		ON_Color col(
			static_cast<int>(rgba[0]), static_cast<int>(rgba[1]),
			static_cast<int>(rgba[2]), 255 - static_cast<int>(rgba[3]));
		for (u32 i = 0; i < 8; ++i) mesh.m_V.Append(center + offsets[i]), mesh.m_C.Append(col);
	}
	return true;
}


