#pragma once
#include "assert.hpp"
#include "DomainDiscretization_fwd.hpp"

namespace meshless {
	template<typename vec>
	const vec& DomainDiscretization<vec>::normal(int i) const {
		assert_msg(0 <= i && i <= size(), "Index %d out of range [0, %d)", i, size());
		assert_msg(types_[i] < 0, "Node %d must be a boundary node, got type %d.", i, types_[i]);
		assert_msg(boundaryMap_[i] != -1, "Node %d does not have a normal. Maybe you manually set"
			" supports and positions instead of using addInternalNode* methods?",
			i);
		return normals_[boundaryMap_[i]];

	}
	template<typename vec>
	vec& DomainDiscretization<vec>::normal(int i) {
		assert_msg(0 <= i && i <= size(), "Index %d out of range [0, %d)", i, size());
		assert_msg(types_[i] < 0, "Node %d must be a boundary node, got type %d.", i, types_[i]);
		assert_msg(boundaryMap_[i] != -1, "Node %d does not have a normal. Maybe you manually set"
			" supports and positions instead of using addInternalNode* methods?",
			i);
		return normals_[boundaryMap_[i]];

	}

	template<typename vec>
	int DomainDiscretization<vec>::addInternalNode(const vec& point, int type) {

		assert_msg(type > 0, "This function is for adding internal points, but got type %d, which is "
			"not positive. Use addBoundaryNode to add boundary nodes.",
			type);
		return addNode(point, type);
	}

	template<typename vec>
	int DomainDiscretization<vec>::addInternalNodeWithT(const vec& point, double t, int type) {

		assert_msg(type > 0, "This function is for adding internal points, but got type %d, which is "
			"not positive. Use addBoundaryNode to add boundary nodes.",
			type);
		return addNodeWithT(point, t, type);
	}

	/**
	 * Adds a boundary node with given type and normal to the domain.
	 * @param point Coordinates of the node to add.
	 * @param type Type of the point, must be negative.
	 * @param normal Outside unit normal to the boundary at point `point`.
	 * @return The index of the new node.
	 * @sa addInternalNode
	 */
	template<typename vec>
	int DomainDiscretization<vec>::addBoundaryNode(const vec& point, int type, const vec& normal) {
		assert_msg(type < 0, "Type of boundary points must be negative, got %d.", type);
		int idx = addNode(point, type);
		boundaryMap_[idx] = normals_.size();
		normals_.push_back(normal);
		return idx;

	}

	template<typename vec>
	int DomainDiscretization<vec>::addBoundaryNodeWithT(const vec& point, double t, int type, const vec& normal) {
		assert_msg(type < 0, "Type of boundary points must be negative, got %d.", type);
		int idx = addNodeWithT(point, t, type);
		boundaryMap_[idx] = normals_.size();
		normals_.push_back(normal);
		return idx;

	}

	template<typename vec>
	int DomainDiscretization<vec>::addNode(const vec& point, int type) {
		positions_.push_back(point);
		types_.push_back(type);
		support_.emplace_back();
		boundaryMap_.push_back(-1);
		return positions_.size() - 1;
	}

	template<typename vec>
	int DomainDiscretization<vec>::addNodeWithT(const vec& point, double t, int type) {
		positions_.push_back(point);
		inCurve_.push_back(t);
		types_.push_back(type);
		support_.emplace_back();
		boundaryMap_.push_back(-1);
		return positions_.size() - 1;
	}

	template<class vec>
	template<typename search_struc>
	void DomainDiscretization<vec>::makeDiscreteContainsStructure(search_struc& search) const {
		int size = boundary().size();
		std::vector<vec> boundary_points(size);
		for(int i = 0; i < boundaryMap_.size(); i++) {
			if(boundaryMap_[i] == -1)
				continue;
			boundary_points[boundaryMap_[i]] = positions_[i];
		}
		search.reset(boundary_points);
	}

	//bool DomainDiscretization::contains(const vec& point)const;
	template<typename vec>
	template<typename search_struc>
	bool DomainDiscretization<vec>::discreteContains(const Eigen::Vector3d& point, search_struc& search, int num)const {
		assert_msg((search.size() != 0), "Search structure should not be empty.");
		auto closet = search.query(point, num);
		auto closet_indexs = closet.first;
		for(int i = 0; i < closet_indexs.size(); i++) {
			auto idx = closet_indexs[i];
			auto n = normals_[idx];
			auto closet_point = search.get(idx);
			if(n.dot(point - closet_point) >= 0) {
				return false;
			}
		}
		// for (auto idx : closet_indexs)
		// {
		//     auto n = normals_[idx];
		//     auto closet_point = search.get(idx);
		//     if (n.dot(point - closet_point) >= 0)
		//     {
		//         return false;
		//     }
		// }
		return true;
		// int closest_index = search.query(point, 1).first[0];
		// auto closest_point = search.get(closest_index);
		// auto n = normals_[closest_index];
		// return n.dot(point - closest_point) < 0;
	}
};