#ifndef _FP_PATH_HPP_
#define _FP_PATH_HPP_

#include <math.h>
#include <assert.h>
#include <vector>
#include <iostream>
#include "base_data.h"
namespace POS_21
{


//std::ostream& operator<< (std::ostream&stm, const point&pt)
//{
//	return stm<<"("<<pt.x_<<", "<<pt.y_<<")";
//}

//猴车路径顶点
	struct fixed_point:point_2
	{
		fixed_point(double x,double y)
			:point_2(x,y)
			,cos_(0),sin_(0)
			,dist_(0)
		{}

	fixed_point(const point_2&pt)
		:point_2(pt)
		,cos_(0),sin_(0)
		,dist_(0)
		{}

	fixed_point(const fixed_point&pt)
		:point_2(pt)
		,cos_(pt.cos_),sin_(pt.sin_)
		,dist_(pt.dist_)
		{}

	double cos_;	//当前点与下一点构成直线的余弦
	double sin_;    //当前点与下一点构成直线的正弦
	double dist_;   //当前点与下一点之间的距离

	//根据下一路径点，计算this与下一路径点连接线段的斜率(cos,sin)和长度(dist)
	double set_next_point(const point_2&p)
	{
		dist_ = dist_to(p);

		assert(dist_>0);

		double deta_x = p.x_-x_;
		cos_ = deta_x/dist_;

		double deta_y = p.y_-y_;
		sin_ = deta_y/dist_;
		return dist_;
	}
	};

//std::ostream& operator<< (std::ostream&stm, const fixed_point&pt)
//{
//	return stm<<"("<<pt.x_<<", "<<pt.y_<<", dist="<<pt.dist_<<")";
//}


//一维、二维坐标进行转换的工具类
//使用this->add_point按照路径顺序加入途径点
//使用this->map进行一维、二维坐标之间的相互转换
	struct fp_path
	{
	private:
		std::vector<fixed_point> path_;
		fp_path(const fp_path&);
		double m_total_length;
	private:
		static inline bool eq(double x1,double x2)
		{
			return fabs(x1-x2)<0.1; //小于10厘米的误差认为是一个点
		}

	public:
		fp_path()
			:m_total_length(0)
		{
			path_.reserve(8);
		}
	
		template<typename iterator>
		fp_path(iterator begin,iterator end)
			:m_total_length(0)
		{
			for (;begin != end; ++ begin)
			{
				add_point(*begin);
			}
		}

		//加入路径坐标
		void add_point(double x,double y)
		{
			add_point(point_2(x,y));
		}

		//加入路径坐标
		void add_point(const point_2&pt)
		{
			if(!path_.empty()) 
			{
				 double d = path_.back().set_next_point(pt); 
				 m_total_length += d;
			}

			path_.push_back(pt);
		}

		/*
			输入路径上的二维点(x,y)，输出该点的一维坐标
			return -1. : (x,y) 不在路径上或者当前对象不包含合法路径
			return >=0 : (x,y) 一维坐标
		*/
		double map(double x,double y)
		{
			
			if(path_.size()<2)
				return -1.;

			double rc=0.;

			auto it1=path_.begin(); 
			double l1=it1->dist_to(x,y);
			
			auto it2=path_.begin(); ++it2;
			for(; it2!=path_.end();++it2)
			{
				double l2=it2->dist_to(x,y);

				if(eq(l1+l2, it1->dist_)) //当前点在两点之间的连线上
				{
					
					return rc+l1;
				}

				rc+=it1->dist_;

				it1=it2;
				l1=l2;
			}

			return -1.;
		}

		double  map(const point_2& p)
		{
			return map(p.x_, p.y_);
		}


	/*
		输入一维坐标(dist),转换为二维坐标输出(point)
		如果给定一维坐标超出当前路径长度，返回 point::invalid_point() 
	*/
		point_2  map(double dist)
		{
			if(path_.size()>1)
			{
				for(auto it=path_.begin(), _e=path_.end()-1; it!=_e;++it)
				{
					if(dist<=it->dist_)
					{
						return point_2(it->x_+it->cos_*dist,it->y_+it->sin_*dist);
					}

					dist-=it->dist_;

					if(dist<0) 
						break;
				}
			}

			return point_2::invalid_point();
		}
		double get_total_length()
		{
			return m_total_length;
		}
		//void debug_out()
		//{
		//	for(auto p:path_)
		//	{
		//		std::cout<<p<<"\n";
		//	}
		//}
	};
}


#endif