#pragma once
#include "../../stdafx.h"

#include "glLibMathVec.h"

namespace glLibMathLA {
	template <typename type_float> inline void project_point_plane(
		type_float point[3],
		type_float plane[4],
		type_float projected_point[3])
	{
		project_point_plane(
			point[0],point[1],point[2],
			plane[0],plane[1],plane[2],plane[3],
			projected_point,projected_point+1,projected_point+2);
	}
	template <typename type_float> inline void project_point_plane(
		type_float point_x, type_float point_y, type_float point_z,
		type_float plane[4],
		type_float* projected_point_x, type_float* projected_point_y, type_float* projected_point_z)
	{
		project_point_plane(
			point_x,point_y,point_z,
			plane[0],plane[1],plane[2],plane[3],
			projected_point_x,projected_point_y,projected_point_z);
	}
	template <typename type_float> inline void project_point_plane(
		type_float point[3],
		type_float a, type_float b, type_float c, type_float d,
		type_float* projected_point_x, type_float* projected_point_y, type_float* projected_point_z)
	{
		project_point_plane(
			point[0],point[1],point[2],
			a,b,c,d,
			projected_point_x,projected_point_y,projected_point_z);
	}
	template <typename type_float> void project_point_plane(
		type_float point_x, type_float point_y, type_float point_z,
		type_float a, type_float b, type_float c, type_float d,
		type_float* projected_point_x, type_float* projected_point_y, type_float* projected_point_z)
	{
		//http://www.9math.com/book/projection-point-plane
		#ifdef GLLIB_ERROR_MODE_REPORT_ERRORS
		if ((a==0)&&(b==0)&&(c==0)) throw glLibError("This plane has a zero normal vector!");
		#endif
		type_float factor = (a*point_x + b*point_y + c*point_z + d)  /  (a*a + b*b + c*c);
		*projected_point_x = point_x - a*factor;
		*projected_point_y = point_y - b*factor;
		*projected_point_z = point_z - c*factor;
	}

	template <typename type_float> inline type_float distance_point_plane(
		type_float point[3],
		type_float plane[4], bool plane_normalized=false)
	{
		return distance_point_plane(
			point[0],point[1],point[2],
			plane[0],plane[1],plane[2],plane[3],plane_normalized);
	}
	template <typename type_float> inline type_float distance_point_plane(
		type_float point_x, type_float point_y, type_float point_z,
		type_float plane[4], bool plane_normalized=false)
	{
		return distance_point_plane(
			point_x,point_y,point_z,
			plane[0],plane[1],plane[2],plane[3],plane_normalized);
	}
	template <typename type_float> inline type_float distance_point_plane(
		type_float point[3],
		type_float a, type_float b, type_float c, type_float d, bool plane_normalized=false)
	{
		return distance_point_plane(
			point[0],point[1],point[2],
			a,b,c,d,plane_normalized);
	}
	template <typename type_float> inline type_float distance_point_plane(
		type_float point_x, type_float point_y, type_float point_z,
		type_float a, type_float b, type_float c, type_float d, bool plane_normalized=false)
	{
		return (plane_normalized) ? 
			(a*point_x + b*point_y + c*point_z + d)
			:
			((a*point_x + b*point_y + c*point_z + d)  /  sqrt(a*a+b*b+c*c));
	}

	template <typename type_float> inline type_float distance_point_line_segment(
		type_float point[3],
		type_float line_start,
		type_float line_end)
	{
		return distance_point_line_segment(
			point[0],point[1],point[2],
			line_start[0],line_start[1],line_start[2],
			line_end[0],line_end[1],line_end[2]);
	}
	template <typename type_float> inline type_float distance_point_line_segment(
		type_float point_x, type_float point_y, type_float point_z,
		type_float line_start,
		type_float line_end)
	{
		return distance_point_line_segment(
			point_x,point_y,point_z,
			line_start[0],line_start[1],line_start[2],
			line_end[0],line_end[1],line_end[2]);
	}
	template <typename type_float> inline type_float distance_point_line_segment(
		type_float point[3],
		type_float line_start_x, type_float line_start_y, type_float line_start_z,
		type_float line_end_x, type_float line_end_y, type_float line_end_z)
	{
		return distance_point_line_segment(
			point[0],point[1],point[2],
			line_start_x,line_start_y,line_start_z,
			line_end_x,line_end_y,line_end_z);
	}
	/*template <typename type_float> type_float distance_point_line_segment(
		type_float point_x, type_float point_y, type_float point_z,
		type_float line_start_x, type_float line_start_y, type_float line_start_z,
		type_float line_end_x, type_float line_end_y, type_float line_end_z)
	{
		//http://softsurfer.com/Archive/algorithm_0102/algorithm_0102.htm#Distance%20to%20Ray%20or%20Segment
		type_float v[3] = { line_end_x-line_start_x, line_end_y-line_start_y, line_end_z-line_start_z };
		type_float w[3] = { point_x-line_start_x, point_y-line_start_y, point_z-line_start_z };
		type_float c1 = glLibMathVec::dot(w,v);

		if (c1<=0.0) {
			return glLibMathVec::length(w,3);
		}

		type_float c2 = glLibMathVec::dot(v,v);
		if (c2<=c1) {
			type_float temp[3] = {point_x-line_end_x, point_y-line_end_y, point_z-line_end_z};
			return length(temp,3);
		}

		type_float b = c1 / c2;
		type_float pb[3] = { line_start_x+b*v[0], line_start_y+b*v[1], line_start_z+b*v[2] };
		type_float temp[3] = { point[0]-pb[0], point[1]-pb[1], point[2]-pb[2] };
		return glLibMathVec::length(temp);
	}*/
	//TODO: distance_point_line

	template <typename type_float> type_float area_triangle(
		type_float point1[3],
		type_float point2[3],
		type_float point3[3])
	{
		type_float vec1[3],vec2[3],temp[3];
		glLibMathVec::subtract(point2,point1,vec1,3);
		glLibMathVec::subtract(point3,point1,vec2,3);
		glLibMathVec::cross(vec1,vec2,temp);
		return glLibMathVec::length(temp) / 2;
	}
	template <typename type_float> type_float area_triangle(
		type_float point1_x, type_float point1_y, type_float point1_z,
		type_float point2_x, type_float point2_y, type_float point2_z,
		type_float point3_x, type_float point3_y, type_float point3_z)
	{
		type_float point1[3] = {point1_x,point1_y,point1_z};
		type_float point2[3] = {point2_x,point2_y,point2_z};
		type_float point3[3] = {point3_x,point3_y,point3_z};
		return area_triangle(
			point1[0],point1[1],point1[2],
			point2[0],point2[1],point2[2],
			point3[0],point3[1],point3[2]);
	}
};