#include "stdafx.h"
#include "KDtree.h"
#include <cmath>
#include <vector>
#include <queue>
#include <map>
#include <iostream>
#include <fstream>
#include <sstream>
using namespace std;

// 卡箍信息
struct Clip
{
	string name;			 // 名字
	P coord, dir;			 // 坐标和方向,dir的方向视为正面
	int emc;				 // 卡箍类型,未确定为0
	double dia;				 // 通过卡箍的线束捆直径
	double maxDia;			 // 卡箍允许通过的最大线束捆直径
	int id;					 // 卡箍标号
	int bundleNodeID;		 // 卡箍暂时属于哪个Bundle
	vector<double> diaset;	 // 已经通过卡箍的线缆半径集合
	vector<int> channelEdge; // 在近似通道中连接到的卡箍标号
	bool used;				 // 卡箍是否被使用

	Clip()
	{
		emc = 0;
		bundleNodeID = 0;
		maxDia = MAXDia;
		id = 0;
		used = 0;
	}

	// 清除所有通过卡箍的线和通道信息
	void clear()
	{
		diaset.clear();
		bundleNodeID = 0;
		used = 0;
	}

	// 在卡箍里加入直径为d的线缆
	inline void addwire(double d)
	{
		diaset.push_back(d);
		dia = sqrt(dia * dia + d * d);
	}

	// 返回通过卡箍的线束捆直径
	inline double getDia()
	{
		double dia1 = 1.154 * dia;
		if (dia1 >= 10)
			dia1 *= 1.235 / 1.154;
		return dia1;
	}

	// 判断直径为d的线缆能否通过
	inline bool transitable(double d)
	{
		double dia1 = sqrt(d * d + dia * dia);
		dia1 = 1.154 * dia;
		if (dia1 >= 10)
			dia1 *= 1.235 / 1.154;

		if (dia1 <= maxDia)
			return true;
		return false;
	}

	// 判断从p点到卡箍，应该从哪一面进入，正面为0，反面为1
	inline int inDir(P p)
	{
		double angle = Angle(coord - p, dir);
		if (angle > pi / 2)
			return 1;
		return 0;
	}
};

// 卡箍集，保存所有的卡箍信息
struct ClipSet
{
	Clip c[N];			// 卡箍集合，下标从1~clipnum
	int clipnum;		// 卡箍总和,卡箍标号范围1`clipnum
	map<P, int> clipid; // 根据点坐标找到卡箍标号

	ClipSet()
	{
		clipnum = 0;
	}

	// 清除通过卡箍的线缆信息
	void clearWire()
	{
		for (int i = 1; i <= clipnum; i++)
			c[i].clear();
	}

	// 新加入一个卡箍
	inline void addclip(P p, P d, string name, int emc)
	{
		clipnum++;
		c[clipnum].coord = p;
		c[clipnum].coord.ref = clipnum;
		c[clipnum].coord.type = 0;
		c[clipnum].coord.setDir(p - d);

		c[clipnum].id = clipnum;
		c[clipnum].dir = p - d;
		c[clipnum].name = name;
		c[clipnum].emc = emc;
		clipid[p] = clipnum;
	}

	// 添加通过卡箍的导线
	inline void addwire(int id, double d)
	{
		c[id].addwire(d);
	}
	inline void addwire(P &p, double d)
	{
		int id = clipid[p];
		c[id].addwire(d);
	}

	// 获取指向编号为id的卡箍的指针
	Clip *getClipPointer(int id, int bundleNodeID)
	{
		if (bundleNodeID != -1)
			c[id].bundleNodeID = bundleNodeID;
		return &c[id];
	}

	// 判断两个卡箍是否互相为最近(已知i最近的卡箍是j)
	inline bool bothConnect(int i, int j)
	{
		if (c[j].channelEdge.empty())
			return false;
		if (c[j].channelEdge[0] == i)
			return true;
		if (c[j].channelEdge.size() == 2 &&
			c[j].channelEdge[1] == i)
			return true;
		return false;
	}

	// 判断卡箍能否通过
	inline bool transitable(int id, int emc, double d)
	{
		if (c[id].emc != emc)
			return false;
		return c[id].transitable(d);
	}

	// 根据卡箍位置估计飞机的近似中心线
	inline void computeCenter()
	{
		Ycenter = 0;
		Zcenter = 0;
		for (int i = 1; i <= clipnum; i++)
		{
			Clip clip = c[i];
			P tp = c[i].coord;
			Ycenter += tp.y;
			Zcenter += tp.z;
			MAXX = max(MAXX, tp.x);
			MINX = min(MINX, tp.x);

			MAXY = max(MAXY, tp.y);
			MINY = min(MINY, tp.y);

			MAXZ = max(MAXZ, tp.z);
			MINZ = min(MINZ, tp.z);
		}
		Ycenter /= clipnum;
		Zcenter /= clipnum;
	}

	// 打印卡箍距离近似中心线的距离
	inline void printDis()
	{
		for (int i = 1; i <= clipnum; i++)
		{
			Clip clip = c[i];
			P tp = c[i].coord;
			double dis = (Ycenter - tp.y) * (Ycenter - tp.y) + (Zcenter - tp.z) * (Zcenter - tp.z);
			cout << sqrt(dis) << endl;
		}
	}

	// 根据卡箍位置先创建一个近似的通道，每个卡箍连到距离最近的两个卡箍
	inline void buildChannel()
	{
		vector<int> pointSet;
		bool flag = false; // flag=true时代表这个卡箍需要重新计算
		double sumDis = 300;
		int sum = 1;
		for (int i = 1; i <= clipnum; i++)
		{
			if (pointSet.empty() || flag)
			{
				pointSet = kdtree.search_by_dis(c[i].coord, segmentLength);
				c[i].channelEdge.clear();
			}
			vector<pair<double, int>> vp;
			for (int j = 0; j < pointSet.size(); j++)
			{
				int cid = pointSet[j];
				if (cid == i)
					continue;
				if (c[i].coord == c[cid].coord)
					continue;
				double dis = distan(c[i].coord, c[cid].coord);
				vp.push_back(make_pair(dis, cid));
			}
			sort(vp.begin(), vp.end());
			P dir;
			for (int j = 0; j < vp.size(); j++)
			{
				int cid = vp[j].second;
				if (vp[j].first > 2 * sumDis / sum)
					break;
				if (j == 0)
				{
					c[i].channelEdge.push_back(cid);
					dir = c[cid].coord - c[i].coord;
					sum++;
					sumDis += vp[j].first;
				}
				else
				{
					P dir2 = c[cid].coord - c[i].coord;
					if (Angle(dir, dir2) >= pi / 2 - minAngle)
					{
						c[i].channelEdge.push_back(cid);
						sum++;
						sumDis += vp[j].first;
						break;
					}
				}
			}
			if (c[i].channelEdge.size() < 2 && !flag)
				flag = true, i--;
			else
				flag = false;
		}
	}

	// 打印所有卡箍信息到clipDir文件，卡箍坐标后面会加上卡箍类型
	/*
		0代表航向卡箍
		1代表垂直向卡箍
		2代表其他方向卡箍
	*/
	inline void print()
	{
		ofstream ofs;
		ofs.open("clipDir.txt", ios::out);
		for (int i = 1; i <= clipnum; i++)
		{
			Clip clip = c[i];
			P tp = c[i].coord;
			P dirtp = c[i].dir;
			ofs << tp.x << "  " << tp.y << "  " << tp.z;
			double angel = Angle(dirtp, DX);
			// if(angel>pi/2)angel=pi-angel;
			// angel=min(angel,pi/2-angel);
			if (angel <= minAngle || angel >= pi - minAngle)
				ofs << 0 << endl;
			else if (angel >= pi / 2 - minAngle && angel <= pi / 2 + minAngle)
				ofs << 1 << endl;
			else
				ofs << 2 << endl;
		}
	}

} clipSet;