#include "stdafx.h"
#include "Point.h"
#include "Geometry.h"
#include "Intersection.h"
#include<cmath>
#include<vector>
#include<queue>
#include<map>
#include<iostream>
#include<fstream>
#include<sstream>

using namespace std;

//该文件定义一些基本常量和通用函数

const int M=10010;  //连接器数量
const int N=5010;	//卡箍数量
const int MaxSTL=210000;	//STL文件最大面片数
const double MAXDia=50;	//卡箍能容纳的最大直径
const int maxCap=50;	//卡箍能容纳的最大线缆数
const double pi=acos(-1.0);	//Π
const double minAngle=pi/144;	//判断平行和垂直的偏差阈值
const double minDis=30;	//
const double R=2000;	//飞机横截面的近似半径
const double MARGIN=1000;	//布线的空间范围，从STL模型向外延申的长度
//----------------------mark----------------------
const int MAXBranchPointNumOnSegment=2;	//分支上的最大分支点数
const int MAXPointNum=N+MAXBranchPointNumOnSegment*N;	//最大分支点数加最大卡箍数
double segmentLength=0.8;	//卡箍到卡箍之间的最长距离
const double MinClipToBranchPointDistance=55;	//卡箍到分支点的最短距离
const double MinBranchPointDistance=55;	//分支点到分支点的最短距离

P DX=P(1,0,0);
P DY=P(0,1,0);
P DZ=P(0,0,1);
//X，Y，Z的正方向

double Ycenter,Zcenter;	//飞机在Y，Z两轴的中心
double MAXX=-1e9,MINX=1e9,MAXY=-1e9,MINY=1e9,MAXZ=-1e9,MINZ=1e9; //卡箍的坐标范围


vector<Vec3f> vertices;//VS2008
vector<Vec3u> indices;//VS2008
Mesh mesh;
const double intersection_distance=1;
int intersection_model =0;

typedef P Point3;
typedef P Vector3;


//判断y是否在[x-margin,z+margin]的范围内
inline bool inmid(double x,double y,double z,double margin=MARGIN){
	return y>=x-margin&&y<=z+margin;
}
inline bool inbox(P& p){
	return 
	inmid(MINX,p.x,MAXX)
	&&inmid(MINY,p.y,MAXY)
	&&inmid(MINZ,p.z,MAXZ);
}

//将d与0比较，返回正负
inline int dcmp(double d){
	if(d<-eps)return -1;
	else if(d>eps)return 1;
	return 0;
}

//真正的距离函数

inline double distan1(P &p1,P &p2){
	double x=p1.x-p2.x,y=p1.y-p2.y,z=p1.z-p2.z;
	return sqrt(x*x+y*y+z*z);
}
/*
inline double distan2(P &p1,P &p2){
	static double penalty_par=1;
	if(intersection_model==1)
	{
		LineSegment lineSegment(Vec3f(p1.x, p1.y, p1.z),Vec3f(p2.x, p2.y, p2.z));
		static BVH_intersection bvh(mesh);
		bool hit = bvh.intersectWithLineSegment(lineSegment);
		if(hit==0)
			penalty_par=1;
		else 
			penalty_par=100;
	}
	double x=p1.x-p2.x,y=p1.y-p2.y,z=p1.z-p2.z;
	return sqrt(x*x+y*y+z*z)*penalty_par;
}
*/
inline double Dot(Vector3 &A,Vector3 &B){return A.x*B.x+A.y*B.y+A.z*B.z;}
inline double Length(Vector3 &A){return sqrt(Dot(A,A));}
inline double Angle(Vector3 &A,Vector3 &B){return acos(Dot(A,B)/Length(A)/Length(B));}
inline double DistanceToPlane(Point3 &p,Point3 &p0,Vector3 &n)
{
    return fabs(Dot(p-p0,n));//如果不取绝对值，得到的是有向距离 
} 
inline int ParallelorVertical(P& p1,P& p2){
	double angel=Angle(p1,p2);
	if(angel<=minAngle||angel>=pi-minAngle)return 1;
	else if(angel>=pi/2-minAngle&&angel<=pi/2+minAngle)return 2;
	return 0;
	
}
inline Point3 GetPlaneProjection(Point3 &p,Point3 &p0,Vector3 &n)
{
    return p-n*Dot(p-p0,n);
}
inline Point3 LinePlaneIntersection(Point3 &p1,Point3 &p2,Point3 &p0,Vector3 &n)
{
    Vector3 v=p2-p1;
    double t=(Dot(n,p0-p1)/Dot(n,p2-p1));//判断分母是否为 0
    return p1+v*t;//如果是线段，判断 t 是不是在 0 和 1 之间 
}
inline Vector3 Cross(Vector3 A,Vector3 B)
{
    return Vector3(A.y*B.z-A.z*B.y,A.z*B.x-A.x*B.z,A.x*B.y-A.y*B.x);
}
inline double Area2(Point3 &A,Point3 &B,Point3 &C){return Length(Cross(B-A,C-A));}


/*考虑了连线方向，卡箍方向，连线长度的综合权值函数
	A或者B type!=0时代表它们不是卡箍，对应的inOut没有意义
	inOut1=0代表沿着A的dir，inOut1=1代表逆着A的dir
	inOut2=0代表沿着B的dir，inOut2=1代表逆着B的dir
*/
inline double distan(P A,P B,int inOut1,int inOut2){
	//static int out1;
	//cout<<"out:"<<out1++<<endl;
	static double penalty_par_intersection=1;
	static double penalty_par_distance=1;

	//cout<<"out1"<<endl;
	
	double angel=Angle(A-B,DX);
	if(angel>pi/2)angel=pi-angel;
	angel=min(angel,pi/2-angel);
	double len=distan1(A,B);

		if(intersection_model==1)
	{
		LineSegment lineSegment(Vec3f(A.x, A.y, A.z),Vec3f(B.x, B.y, B.z));
		static BVH_intersection bvh(mesh);
		bool hit = bvh.intersectWithLineSegment(lineSegment);
		if(hit==0)
			penalty_par_intersection=1;
		else 
			penalty_par_intersection=400;
		if(len>intersection_distance)
			penalty_par_distance=8;
		else
			penalty_par_distance=1;


	}

	double len1=sqrt((A.y-Ycenter)*(A.y-Ycenter)+(A.z-Zcenter)*(A.z-Zcenter));
	double len2=sqrt((B.y-Ycenter)*(B.y-Ycenter)+(B.z-Zcenter)*(B.z-Zcenter));
	if(len1<R||len2<R){
		double angel1=Angle(A-B,DX);
		if(angel1>pi/2)angel1=pi-angel1;

		double angel2=Angle(A-B,DY);
		if(angel2>pi/2)angel2=pi-angel2;

		double angel3=Angle(A-B,DZ);
		if(angel3>pi/2)angel3=pi-angel3;
		
		angel=min(angel1,min(angel2,angel3));
	}
	
	P C;
	if(inOut1)A.reverse();
	C.x=A.dx;C.y=A.dy;C.z=A.dz;
	double angel2=Angle(B-A,C);
	if(A.isend==1||A.type!=0)angel2=0;

	if(inOut2)B.reverse();
	C.x=B.dx;C.y=B.dy;C.z=B.dz;
	double angel3=Angle(B-A,C);
	if(B.isend==1||B.type!=0)angel3=0;

	//return pow(len,angel*3+1);
	double orign_distance=len*(angel*4+1)+300*600*(angel2+angel3)/len;
	
	//cout<<"dir:"<<orign_distance<<" "<<"patch_par"<<patch_par<<endl;
	//return len*(angel*4+1)+300*600*(angel2+angel3)/len;
	return orign_distance*penalty_par_intersection*penalty_par_distance;
}

/*考虑了连线方向，卡箍方向，连线长度的综合权值函数
	A和B自动选择最合适的dir
*/
//----------------------mark----------------------
inline double distan(P A,P B){
	//static int without1;
	//cout<<"without:"<<without1++<<endl;
	static double penalty_par_intersection=1;
	static double penalty_par_distance=1;
	double angel=Angle(A-B,DX);
	if(angel>pi/2)angel=pi-angel;
	angel=min(angel,pi/2-angel);
	double len=distan1(A,B);

	if(intersection_model==1)
	{
		LineSegment lineSegment(Vec3f(A.x, A.y, A.z),Vec3f(B.x, B.y, B.z));
		static BVH_intersection bvh(mesh);
		bool hit = bvh.intersectWithLineSegment(lineSegment);
		if(hit==0)
			penalty_par_intersection=1;
		else 
			penalty_par_intersection=400;
		if(len>intersection_distance)
			penalty_par_distance=8;
		else
			penalty_par_distance=1;
	}

	double len1=sqrt((A.y-Ycenter)*(A.y-Ycenter)+(A.z-Zcenter)*(A.z-Zcenter));
	double len2=sqrt((B.y-Ycenter)*(B.y-Ycenter)+(B.z-Zcenter)*(B.z-Zcenter));
	if(len1<R||len2<R){
		double angel1=Angle(A-B,DX);
		if(angel1>pi/2)angel1=pi-angel1;

		double angel2=Angle(A-B,DY);
		if(angel2>pi/2)angel2=pi-angel2;

		double angel3=Angle(A-B,DZ);
		if(angel3>pi/2)angel3=pi-angel3;
		
		angel=min(angel1,min(angel2,angel3));
	}
	

	P C;
	C.x=A.dx;C.y=A.dy;C.z=A.dz;
	double angel2=Angle(B-A,C);
	angel2=min(angel2,pi-angel2);
	if(A.isend==1||A.type!=0)angel2=0;

	C.x=B.dx;C.y=B.dy;C.z=B.dz;
	double angel3=Angle(B-A,C);
	angel3=min(angel3,pi-angel3);
	if(B.isend==1||B.type!=0)angel3=0;

	//return pow(len,angel*3+1);
	double orign_distance=len*(angel*4+1)+300*600*(angel2+angel3)/len;
	return orign_distance*penalty_par_intersection;

}


//打印路径信息
void printPath(vector<P> vecp){
	for(int j=0;j<vecp.size();j++){
		P pp=vecp[j];
		cout<<setprecision(10)<<"("<<pp.x<<","<<pp.y<<","<<pp.z<<")";
		if(j!=vecp.size()-1)cout<<"->";
	}
	cout<<endl;
	return;
}