#pragma once
#include <vector>
#include <iostream>

#include "unitcells/UnitCellBuilder.h"
#include "unitcells/UnitCellBCC.h"

#include "BRepAlgoAPI_Fuse.hxx"
#include "BRepAlgoAPI_Common.hxx"
#include "BRepBuilderAPI_Transform.hxx"
#include "BRepPrimAPI_MakeBox.hxx"


template<class UnitCell>
class BuildLattice
{
protected:
	bool _buildSuccessful;
	double _cx, _cy, _cz;	// Size of each cell
	const std::vector<gp_Pnt>& _cellPositions;	// Position of node1 of unit cell (NOT CENTER)
	const std::vector<double>& _cellRadii;

	TopoDS_ListOfShape _unitCells;
	TopoDS_Shape _tmpUnitCell;
	TopoDS_Shape _lattice;
	TopoDS_Solid _boundingBox;

protected:
	bool buildCell(double w, double d, double h, double r);
	bool translateCell(double dx, double dy, double dz);
	bool mergeCells();
	bool trimLattice();

public:
	BuildLattice(const std::vector<gp_Pnt>& positions, const std::vector<double>& radii, double cx = 5.0, double cy = 5.0, double cz = 5.0)
		: _cellPositions(positions), _cellRadii(radii), _buildSuccessful(false),
		_cx(cx), _cy(cy), _cz(cz)
	{
		// Compute Basic Bounding Box
		double x0 = DBL_MAX, y0 = DBL_MAX, z0 = DBL_MAX;
		double x1 = -DBL_MAX, y1 = -DBL_MAX, z1 = -DBL_MAX;
		for (const gp_Pnt& point : _cellPositions)
		{
			x0 = point.X() < x0 ? point.X() : x0;
			y0 = point.Y() < y0 ? point.Y() : y0;
			z0 = point.Z() < z0 ? point.Z() : z0;
			x1 = point.X() > x1 ? point.X() : x1;
			y1 = point.Y() > y1 ? point.Y() : y1;
			z1 = point.Z() > z1 ? point.Z() : z1;
		}
		x1 += _cx;
		y1 += _cy;
		z1 += _cz;
		_boundingBox = BRepPrimAPI_MakeBox(gp_Pnt(x0, y0, z0), gp_Pnt(x1, y1, z1));
	}

	// Setters
	void setCellSize(double cellWidth, double cellDepth, double cellHeight) { _cx = cellWidth, _cy = cellDepth; _cz = cellHeight; }
	void setBoundingBox(const TopoDS_Solid& boundingBox) { _boundingBox = _boundingBox; }

	bool buildLattice();
	TopoDS_Shape getLattice() { return _lattice; }
};


// Builds a cell of type UnitCell
template<class UnitCell>
bool BuildLattice<UnitCell>::buildCell(double w, double d, double h, double r)
{
	UnitCell cellBuilder(w, d, h, r);
	cellBuilder.buildUnitCell();
	if (!cellBuilder.status())
	{
		std::cout << "[Error](buildCell) Error building unit cell" << std::endl;
		return false;
	}

	_tmpUnitCell = cellBuilder.getUnitCell();
	return true;
}

// Translates cell to position dx, dy, dz
template<class UnitCell>
bool BuildLattice<UnitCell>::translateCell(double dx, double dy, double dz)
{
	gp_Trsf translationVector;
	translationVector.SetTranslation(gp_Vec(dx, dy, dz));
	_unitCells.Append(BRepBuilderAPI_Transform(_tmpUnitCell, translationVector));
	return true;
}

// Merges all unit cells 
template<class UnitCell>
bool BuildLattice<UnitCell>::mergeCells()
{
	BRepAlgoAPI_Fuse fuser;
	TopoDS_ListOfShape arg;

	arg.Append(_unitCells.First());
	_unitCells.RemoveFirst();

	fuser.SetArguments(arg);
	fuser.SetTools(_unitCells);

	fuser.SetRunParallel(true);
	fuser.SetFuzzyValue(1e-4);

	fuser.Build();
	if (fuser.HasErrors())
	{
		fuser.DumpErrors(std::cout);
		std::cout << "[Error](mergeCells) Unknown error occured while merging cells" << std::endl;
		return false;
	}
	fuser.SimplifyResult();
	_lattice = fuser.Shape();
	return true;
}

// Trims outside of lattice
template<class UnitCell>
bool BuildLattice<UnitCell>::trimLattice()
{
	if (_lattice.IsNull() || _boundingBox.IsNull())
	{
		std::cout << "[Error](trimLattice) Lattice or bounding box is null" << std::endl;
		return false;
	}

	// Trim unitcell
	_lattice = BRepAlgoAPI_Common(_lattice, _boundingBox);
	return true;
}

template<class UnitCell>
inline bool BuildLattice<UnitCell>::buildLattice()
{
	size_t nCells = _cellRadii.size();
	std::cout << "[Info](buildLattice) Building " << nCells << " cells" << std::endl;
	for (size_t i = 0; i < nCells; i++)
	{
		if (!buildCell(_cx, _cy, _cz, _cellRadii[i]))
			return false;
		const gp_Pnt& pos = _cellPositions[i];
		if (!translateCell(pos.X(), pos.Y(), pos.Z()))
			return false;

		if (i % 10 == 0)
			std::cout << "[Info](bulidLattice) (" << i << "/" << nCells << ") built" << std::endl;
	}
	std::cout << "[Info](bulidLattice) (" << nCells << "/" << nCells << ") built" << std::endl;
	std::cout << "[Info](buildLattice) All cells built" << std::endl;

	std::cout << "[Info](buildLattice) Merging cells (this step takes some time)" << std::endl;
	if (!mergeCells())
		return false;
	std::cout << "[Info](buildLattice) Trimming lattice" << std::endl;
	if (!trimLattice())
		return false;

	return true;
}