#include "RelationDetector.h"
#include <algorithm>
#include <numeric>
Q_DECLARE_METATYPE(Vector3)

bool isTaged(const PairRelation &pr)
{
	return pr.tag;
}
bool isTagedGr(const GroupRelation &gr)
{
	return gr.tag;
}

void saveToFile(QString filename, QVector<PairRelation>& prs)
{
	QFile file(filename);
	if (!file.open(QIODevice::WriteOnly | QIODevice::Text)) return;
		
	QTextStream out(&file);
	int i(0);
	out << "pair relations number: " << prs.size() << "\n";
	out << "    TRANS: " << countByType(prs,TRANS) << "\n";
	out << "    REF: " << countByType(prs,REF) << "\n\n";

	foreach(PairRelation pr, prs)
	{
		out << i << " " << pr.type << ": " << pr.n1->id << ", " << pr.n2->id << ", ";
		if ( pr.type == TRANS)
			out << "Trans: ["<<pr.trans_vec[0]<<", "<<pr.trans_vec[1]<<", "<<pr.trans_vec[2]<< "]\n";		
		else
			out << "\n";
		out << ", diameter: " << pr.diameter << "\n\n";
		++i;
	}
	file.close();
}
void saveToFile(QString filename, QVector<GroupRelation>& grs)
{
	QFile file(filename);
	if (!file.open(QIODevice::WriteOnly | QIODevice::Text)) return;
		
	QTextStream out(&file);
	// output groups info
	int i(0);
	foreach(GroupRelation gr, grs)
	{
		out << i << " <group-" << gr.type << ">: ";
		if ( gr.type == REF_SYMMETRY)
		{
			out << "\n Point: " << gr.point.x() << "," << gr.point.y() << "," << gr.point.z() << "\n";
			out << "normal: " << gr.normal.x() << "," << gr.normal.y() << "," << gr.normal.z() << "\n";
		}
		else if ( gr.type == AXIS_SYMMETRY)
		{
			out << "\n Center: " << gr.center.x() << "," << gr.center.y() << "," << gr.center.z() << "\n";
			out << "direction: " << gr.direction.x() << "," << gr.direction.y() << "," << gr.direction.z() << "\n";
		}
		else
			out << "\n";

		foreach(QString id, gr.ids){
			out << id << ", ";
		}
		out << "</group>\n";
		out << "diameter: " << gr.diameter << "\n";
		out << "deviation: " << gr.deviation << "\n\n";
		++i;
	}
	file.close();
}

QTextStream& operator << (QTextStream& os, const PairRelation& pr)
{    
	os << pr.type << " Pair <" << pr.n1->id << ", " << pr.n2->id << "> size <" << pr.n1->bbox().diagonal().norm() << ", " << pr.n2->bbox().diagonal().norm() << 
		"> deviation: " << pr.deviation << "\n";
    return os;
}
QTextStream& operator << (QTextStream& os, const GroupRelation& gr)
{    
    os << "Group <";
	for (int i = 0; i < gr.ids.size(); ++i)
	{
		os << gr.ids[i] << ", ";
	}
	os << gr.type << ">\n";
	
	if ( gr.type == AXIS_SYMMETRY)
	{
		os << "Center: " << gr.center.x() << ", " << gr.center.y() << ", " << gr.center.z() << "\n";
		os << "Direction: " << gr.direction.x() << ", " << gr.direction.y() << ", " << gr.direction.z() << "\n";
	}
	else if ( gr.type == REF_SYMMETRY)
	{
		os << "normal of plane: " << gr.normal.x() << ", " << gr.normal.y() << ", " << gr.normal.z() << "\n";
		os << "point of plane: " << gr.point.x() << ", " << gr.point.y() << ", " << gr.point.z() << "\n";
	}

	os << "Diameter: " << gr.diameter << "\n";
	os << "Deviation: " << gr.deviation << "\n\n";
    return os;
}
bool GroupRelation::equal(GroupRelation& gr)
{
    if (type.compare(gr.type))
        return false;

    if (ids.size() != gr.ids.size())
        return false;

    QVector<QString>::iterator it2 = gr.ids.begin();
    for ( QVector<QString>::iterator it1=ids.begin(); it1 != ids.end(); ++it1, ++it2)
    {
        if ( *it1 != *it2)
            return false;
    }
    return true;
}

Structure::Link* RelationDetector::findLink(Structure::Node *n1, Structure::Node * n2, Structure::Graph * graph)
{
	Structure::Link* link(0);
	int tmp1 = graph->edges.size();
	for ( int i = 0; i < tmp1; ++i)
	{
		Structure::Link* tmpLink = graph->edges[i];
		if (tmpLink->n1 == n1 && tmpLink->n2 == n2 || tmpLink->n1 == n2 && tmpLink->n2 == n1)
		{
			link = tmpLink;
			break;
		}		
	}
	return link;
}
double RelationDetector::computeDeviationByLink(Structure::Link* link)
{
	double deviation(0.0);
	if (link)
	{
		SurfaceMesh::Vector3 p1 = link->position(link->n1->id);
		SurfaceMesh::Vector3 p2 = link->position(link->n2->id);
		deviation = (p1-p2).norm();
		//deviation = link->property["blendedDelta"].value<Vector3>().norm();
	}
	return deviation;
}

RelationDetector::RelationDetector(Structure::Graph* g, const QString& logprefix, int ith, double normalizeCoef, int pointLevel, int logLevel)
	                              :graph_(g),logLevel_(logLevel), normalizeCoef_(normalizeCoef), pointLevel_(pointLevel)
{
	thRadiusRadio_ = 1.1;

    thTransRadio_ = 0.01; //0.3
    thRefRadio_ = 0.03;
    thAxisDeviationRadio_ = 0.9;
    thCopla_ = 0.002;//0.1
    thParal_ = 0.001;
    thOthog_ = 0.001;//0.1

    thAxisGroup_ = 0.01;
    thRefGroup_ = 0.01;
    thCoplaGroup_ = 0.003;


	if ( logLevel_ > 0)
	{			
		logFile_.setFileName(logprefix + QString::number(ith) + ".log");
		if (!logFile_.open(QIODevice::WriteOnly | QIODevice::Text)) return;		
		logStream_.setDevice(&logFile_);
	}
}

double RelationDetector::computePairDiameter(PairRelation& pr)
{
	Eigen::AlignedBox3d bbox1 = pr.n1->bbox();
	Eigen::AlignedBox3d bbox2 = pr.n2->bbox();
	Eigen::AlignedBox3d bbox = bbox2.merged(bbox1);
	return bbox.diagonal().norm();
}
double RelationDetector::computeGroupDiameter(GroupRelation& gr)
{
    Eigen::AlignedBox3d bbox;
    for ( QVector<QString>::iterator it = gr.ids.begin(); it!=gr.ids.end(); ++it)
    {
        Eigen::AlignedBox3d bbox1 = graph_->getNode(*it)->bbox();
        bbox = bbox.merged(bbox1);
    }
    //return bbox.volume();
    return bbox.diagonal().norm();
}
void RelationDetector::computeGroupCenter(GroupRelation& gr)
{
    Eigen::Vector3d center(0,0,0);
    for ( QVector<QString>::iterator it = gr.ids.begin(); it != gr.ids.end(); ++it)
    {
        Structure::Node* n1 = graph_->getNode(*it);
		center += computeCptsCenter(n1);
        //center += n1->center();
    }
    center = center / gr.ids.size();
    gr.center = Point_3(center.x(), center.y(), center.z());
}
double RelationDetector::fixDeviationByPartName(const QString& s1, const QString& s2, double deviation, double times)
{
	double ndeviation = deviation;
    int idx = s1.indexOf(QRegExp("\\d"), 0);
    QString str1 = s1.left(idx);
    QString str2 = s2.left(idx);
    if ( str1 == str2)
        ndeviation *= times;

	return ndeviation;
}
Eigen::MatrixXd RelationDetector::node2matrix(Structure::Node* node, int pointLevel)
{
	std::vector<Eigen::Vector3d> nodeCptsV;
	extractCpts( node, nodeCptsV, pointLevel);
	Eigen::MatrixXd nodeCptsM;
	vectorPts2MatrixPts(nodeCptsV, nodeCptsM);
	return nodeCptsM;
}
int RelationDetector::extractCpts( Structure::Node * n, std::vector<Eigen::Vector3d>& mcpts, int pointsLevel)
{
    if ( pointsLevel == 2)
    {
    	SurfaceMesh::Model * m1 = n->property["mesh"].value< QSharedPointer<SurfaceMeshModel> >().data();
    	SurfaceMesh::Vector3VertexProperty pts1 = m1->vertex_coordinates();
    	double* tmp;
    	foreach(Vertex v1, m1->vertices())
    	{
    		tmp = pts1[v1].data();
    		mcpts.push_back( Eigen::Vector3d(tmp[0], tmp[1], tmp[2]) );
    	}
    }
	else
	{
		if ( Structure::CURVE == n->type() )
		{
			Structure::Curve* c = dynamic_cast<Structure::Curve *>(n);
			if ( pointsLevel == 1)
			{
				for (int i = 0; i < (int) c->numCtrlPnts(); ++i)
				{
					mcpts.push_back( c->controlPoint(i));
				}
			}
			else
			{
				mcpts.push_back( c->controlPoint(0) );
				mcpts.push_back( c->controlPoint( c->numCtrlPnts()-1 ) );
			}
		}
		else
		{
			Structure::Sheet* s = dynamic_cast<Structure::Sheet *>(n);
			if ( pointsLevel == 1)
			{
				for (int i = 0; i < (int) s->numCtrlPnts(); ++i)
				{
					mcpts.push_back( s->controlPoint(i));
				}
			}
			else
			{
				int nu = s->numUCtrlPnts(), nv = s->numVCtrlPnts();
				mcpts.push_back( s->surface.GetControlPoint(0,0) );
				mcpts.push_back( s->surface.GetControlPoint(nu-1,0) );
				mcpts.push_back( s->surface.GetControlPoint(nu-1,nv-1) );
				mcpts.push_back( s->surface.GetControlPoint(0,nv-1) );
			}
		}
	}

	return mcpts.size();
}
void RelationDetector::vectorPts2MatrixPts(const std::vector<Eigen::Vector3d>& ptsin, Eigen::MatrixXd& ptsout)
{
	ptsout.resize(ptsin.size(), 3);
    for ( int i = 0; i < (int)ptsin.size(); ++i)
	{
		ptsout.row(i) = ptsin[i];
	}
}
std::vector<Eigen::Vector3d> RelationDetector::matrixPts2VectorPts(Eigen::MatrixXd& ptsin)
{
	std::vector<Eigen::Vector3d> ptsout;
	for ( int i = 0; i < ptsin.rows(); ++i)
	{
		ptsout.push_back( ptsin.row(i));
	}
	return ptsout;
}

Eigen::Vector3d RelationDetector::curve2vectorNormalized(Structure::Node * n)
{
	const Vector3& bpn = n->controlPoint(0);
	const Vector3& epn = n->controlPoint(n->numCtrlPnts()-1);
	Eigen::Vector3d vec = bpn - epn;
	vec.normalize();
	return vec;
}
std::vector<Point_3> RelationDetector::sheet2rect(Structure::Node * n)
{
	Structure::Sheet* s = dynamic_cast<Structure::Sheet *>(n);
	std::vector<Point_3> result;
	Vector3 v = s->surface.GetControlPoint(0,0); 
	result.push_back( Point_3(v.x(), v.y(), v.z()) );
	v = s->surface.GetControlPoint(s->surface.GetNumCtrlPoints(0)-1,0); 
	result.push_back( Point_3(v.x(), v.y(), v.z()) );
	v = s->surface.GetControlPoint(0,s->surface.GetNumCtrlPoints(1)-1); 
	result.push_back( Point_3(v.x(), v.y(), v.z()) );
	v = s->surface.GetControlPoint(s->surface.GetNumCtrlPoints(0)-1,s->surface.GetNumCtrlPoints(1)-1); 
	result.push_back( Point_3(v.x(), v.y(), v.z()) );
	return result;
}
Segment_3 RelationDetector::curve2segment(Structure::Node * n)
{
	const Vector3& bpn = n->controlPoint(0);
	const Vector3& epn = n->controlPoint(n->numCtrlPnts()-1);

	Segment_3 seg(Point_3(bpn.x(),bpn.y(),bpn.z()), Point_3(epn.x(),epn.y(),epn.z()));
	return seg;
}
Line_3 RelationDetector::curve2line(Structure::Node * n)
{
	return Line_3(curve2segment(n));
}
void RelationDetector::sheet2plane(Structure::Sheet * s, Vector3& pos, Vector3& normal)
{
	std::vector<Vector3> nf = noFrame();
	s->get(Vector4d(0.5,0.5,0,0), pos, nf);
	normal = nf[2];
}
bool RelationDetector::node2direction(Structure::Node * n, Vector_3& result)
{
	bool iscurve(true);
	if ( 0 == Structure::CURVE.compare( n->type()) )
	{
		Line_3 line = curve2line(n);
		result = line.direction;		
	}
	else
	{
		Vector3 point, normal;
		sheet2plane(dynamic_cast<Structure::Sheet *>(n), point, normal);
		result = Vector_3(normal.x(), normal.y(), normal.z());
		iscurve = false;
	}	
	result.normalize();
	return iscurve;
}

// bSource == true means that the id is from target shape, & we want to find its corresponded node in source shape, then graph should be source
std::vector<Structure::Node*> RelationDetector::findNodesInST(QString id, Structure::Graph *graph, QVector<PART_LANDMARK> &corres, bool bSource)
{	
	QVector<QString> ids;
    if ( bSource)
    {
        for ( int i = 0; i < (int) corres.size(); ++i)
        {
            QVector<QString> ids2 = corres[i].second;
			for ( int j = 0; j < ids2.size(); ++j)
            {
				if ( ids2[j] == id)
				{
					ids = corres[i].first;					
					break;
				}
            }
			if ( !ids.isEmpty() )
				break;
		}
	}
	else
    {
        for ( int i = 0; i < (int) corres.size(); ++i)
        {
            QVector<QString> ids1 = corres[i].first;
			for ( int j = 0; j < ids1.size(); ++j)
            {
				if ( ids1[j] == id)
				{
					 ids = corres[i].second;
					 break;
				}
            }
			if ( !ids.isEmpty() )
				break;
		}
	}

	std::vector<Structure::Node*> result;
	for ( int i = 0; i < (int) ids.size(); ++i)
	{
		result.push_back( graph->getNode(ids[i]) );
	}
	return result;
}

std::vector<Structure::Node*> RelationDetector::findNodesInB(QString id, Structure::Graph *graph, QVector<PART_LANDMARK> &corres, bool bSource)
{
    std::vector<Structure::Node*> result;

    if ( bSource)
    {
        for ( int i = 0; i < (int) corres.size(); ++i)
        {
            QVector<QString> ids1 = corres[i].first;
			for ( int j = 0; j < ids1.size(); ++j)
			{
				if ( id == ids1[j])
				{
					QVector<QString> ids2 = corres[i].second;
					if ( ids1.size() >= ids2.size()) // 1-1 or *-1
					{
						Structure::Node* tmpNode = graph->getNode(id);
						if ( tmpNode != NULL)
							result.push_back( tmpNode );
					}
					else // 1-*
					{
						for ( int k = 0; k < ids2.size(); ++k)
						{
							QString str; str.setNum(k);  
							Structure::Node* tmpNode = graph->getNode(id + "_" + str );
							if ( tmpNode != NULL)
								result.push_back( tmpNode);
						}						
					}
					return result;
				}
			}
        }
		// 1-0
        Structure::Node* tmpNode = graph->getNode(id);
        if ( tmpNode != NULL)
            result.push_back( tmpNode );            
    }
    else // is target
    {
        for ( int i = 0; i < (int) corres.size(); ++i)
        {
			QVector<QString> ids2 = corres[i].second;
			for ( int j = 0; j < ids2.size(); ++j)
			{
				if ( id == ids2[j])
				{
					 QVector<QString> ids1 = corres[i].first;
					 if ( ids1.size() >= ids2.size()) // 1-1 or *-1
					 {
						 for ( int k = 0; k < ids1.size(); ++k)
						 {
							Structure::Node* tmpNode = graph->getNode(ids1[k]);
							if ( tmpNode != NULL)
								result.push_back( tmpNode );
						 }
					 }
					 else // 1-*
					 {
						QString str; str.setNum(j);  
						Structure::Node* tmpNode = graph->getNode(ids1[0] + "_" + str );
						if ( tmpNode != NULL)
							result.push_back( tmpNode);
					 }
					 return result;
				}
			}
        }
        // 0-1
        Structure::Node* tmpNode = graph->getNode(id + "_null");
        if ( tmpNode != NULL)
            result.push_back( tmpNode );            
    }
    //result.push_back( graph->getNode(id) );   
	return result;
}

void RelationDetector::createPlane(Structure::Node *n1,Structure::Node *n2, Eigen::Vector3d& point, Eigen::Vector3d& normal)
{
    Segment_3 s0 = curve2segment(n1);
    Segment_3 s2 = curve2segment(n2);

    // use middle point of 2 segment to generate a new segment, avoiding that s0 & s2 are parallel
    Point_3 p0( 0.5*(s0.point(0).x() + s0.point(1).x()),
            0.5*(s0.point(0).y() + s0.point(1).y()),
            0.5*(s0.point(0).z() + s0.point(1).z())  );
    Point_3 p2(0.5*(s2.point(0).x() + s2.point(1).x()),
            0.5*(s2.point(0).y() + s2.point(1).y()),
            0.5*(s2.point(0).z() + s2.point(1).z()) );
    Segment_3 s1 = Segment_3(p0,p2);

    ///////
    Point_3 cp( 0.25*(s0.point(0).x() + s0.point(1).x() + s2.point(0).x() + s2.point(1).x()),
        0.25*(s0.point(0).y() + s0.point(1).y() + s2.point(0).y() + s2.point(1).y()),
        0.25*(s0.point(0).z() + s0.point(1).z() + s2.point(0).z() + s2.point(1).z()) );
    point[0] = cp.x(); point[1] = cp.y(); point[2] = cp.z();

    Vector_3 v0 = cross_product(s1,s0);
    Vector_3 v2 = cross_product(s1,s2);
    if ( v0.squaredNorm() > v2.squaredNorm())
    {
        normal[0] = v0.x(); normal[1] = v0.y(); normal[2] = v0.z();
    }
    else
    {
        normal[0] = v2.x(); normal[1] = v2.y(); normal[2] = v2.z();
    }
    normal.normalize();
}

double RelationDetector::computeRefSymmetryGroupDeviation(GroupRelation& gr, int pointLevel)
{
    std::vector<Structure::Node*> nodes;
    for ( int i = 0; i < (int) gr.ids.size(); ++i)
    {
        nodes.push_back( graph_->getNode(gr.ids[i]) );
    }

    double minErr = std::numeric_limits<double>::max();
    std::pair<int, int> minNodes;
    Eigen::Vector3d point, normal;
    double err;
    for ( int i = 0; i < (int) gr.ids.size(); ++i)
    {
        for ( int j = i+1; j < (int) gr.ids.size(); ++j)
        {
            findRefPlane(nodes[i], nodes[j], point,normal);
			if ( normal.squaredNorm() == 0)
				continue;

			err = errorOfRefSymmGroup(nodes, point, normal, pointLevel);

            if ( err < minErr)
            {
                minErr = err;
                minNodes = std::make_pair(i,j);
            }
        }
    }

    findRefPlane(nodes[minNodes.first], nodes[minNodes.second], gr.point, gr.normal);
	if ( gr.normal.squaredNorm() == 0)
		minErr = 0;
	else
		minErr = errorOfRefSymmGroup(nodes, gr.point, gr.normal, pointLevel);

    return minErr/gr.ids.size();
}

double RelationDetector::computeAxisSymmetryGroupDeviationSortParts(GroupRelation& gr, int pointLevel)
{
    double result(0.0);
    int n = gr.ids.size();
    double angle = 2*3.1415926/n;

    QVector<QString> ids;
    ids.push_back(gr.ids.last());
    gr.ids.pop_back();

	double min_dist, mean_dist, max_dist;
    while(gr.ids.size())
    {
        Structure::Node* n1 = graph_->getNode(ids.last());
		Eigen::MatrixXd verts1 = node2matrix(n1, pointLevel);

        double err(std::numeric_limits<double>::max());
        int k(0);
        for ( int i = 0; i < (int) gr.ids.size(); ++i)
        {
            Structure::Node* n2 = graph_->getNode(gr.ids[i]);
			Eigen::MatrixXd verts2 = node2matrix(n2, pointLevel);
			Eigen::MatrixXd newverts2;            
			rotate_points3d(verts2, gr.center, gr.direction, angle, newverts2);
			distanceBetween(verts1, newverts2, min_dist, mean_dist, max_dist);
            if ( mean_dist < err)
			{
                err = mean_dist;
                k = i;
            }
        }
        result += err;
        ids.push_back(gr.ids[k]);
        gr.ids.remove(k);
    }
    for ( QVector<QString>::iterator it = ids.begin(); it != ids.end(); ++it)
        gr.ids.push_back(*it);

    return result/(ids.size()-1);
}
double RelationDetector::computeAxisSymmetryGroupDeviation(const GroupRelation& gr, int pointLevel)
{
    double result(0.0);
    int n = gr.ids.size();
    double angle = 2*3.1415926/n;

	double min_dist, mean_dist, max_dist;
    for ( int i = 0; i < n; ++i)
    {
		Structure::Node* n1 = graph_->getNode(gr.ids[i]);
		Eigen::MatrixXd verts1 = node2matrix(n1, pointLevel);

        Structure::Node* n2 = graph_->getNode(gr.ids[(i+1)%n]);
		Eigen::MatrixXd verts2 = node2matrix(n2, pointLevel);

		Eigen::MatrixXd newverts2;            
		rotate_points3d(verts2, gr.center, gr.direction, angle, newverts2);
		distanceBetween(verts1, newverts2, min_dist, mean_dist, max_dist);
		result += mean_dist;
    } 

    return result/n;
}

void RelationDetector::findRefPlane(Structure::Node* n1, Structure::Node* n2, Eigen::Vector3d& center,Eigen::Vector3d& normal)
{
    Eigen::Vector3d c1 = computeCptsCenter(n1);
    Eigen::Vector3d c2 = computeCptsCenter(n2);
    normal = c1 - c2;
	if ( normal.squaredNorm() != 0)
		normal.normalize();
    center = (c1+c2)*0.5;
}
Eigen::Vector3d RelationDetector::computeCptsCenter(Structure::Node* nn)
{
    Eigen::MatrixXd verts;
    vectorPts2MatrixPts(nn->controlPoints(), verts);
    Eigen::Vector3d center( verts.col(0).mean(), verts.col(1).mean(),verts.col(2).mean() );
    return center;
}
double RelationDetector::errorOfRefSymmGroup(std::vector<Structure::Node*> &nodes, Eigen::Vector3d& center, Eigen::Vector3d& normal, int pointLevel)
{
    std::vector<double> error;
    std::vector<bool> bDone(nodes.size(),false);
	double min_dist, mean_dist, max_dist;

    for ( int i = 0; i < (int) nodes.size(); ++i)
    {
        if ( bDone[i] ) continue;

		Eigen::MatrixXd newverts1;
        reflect_points3d(node2matrix(nodes[i], pointLevel), center, normal, newverts1);
        double err = std::numeric_limits<double>::max();
        int minIdx(0);

        for ( int j = i; j < (int) nodes.size(); ++j)
        {				
            distanceBetween( node2matrix(nodes[j], pointLevel), newverts1, min_dist, mean_dist, max_dist);
            if ( mean_dist < err)
            {
                err = mean_dist;
                minIdx = j;
            }
        }
        error.push_back(err);
        bDone[i] = true; bDone[minIdx] = true;
    }
    return std::accumulate( error.begin(), error.end(), 0.0);
}

void RelationDetector::computeTransGroupInfo(GroupRelation &gr, QSet<QString>& ids)
{
    QSet<QString>::iterator it = ids.begin();
    Structure::Node* n = graph_->getNode(*it);    
    Vector_3 direction(0,0,0);
    node2direction(n, direction);
    gr.ids.push_back(*it);
    ++it;
    Vector_3 dc;
    int k(1);
    for (; it!=ids.end(); ++it,++k )
    {
        Structure::Node* nn = graph_->getNode(*it);        

        node2direction(nn, dc);
        if ( dot(direction, dc) > 0)
            direction = direction + dc;
        else
            direction = direction - dc;

        direction = direction/sqrt(direction.squaredNorm());
        gr.ids.push_back(*it);
    }
    
    gr.direction = direction;
    gr.diameter = computeGroupDiameter(gr);
	computeGroupCenter(gr);
}