/******************************************************************************
 *                         Create Cluster Graph                               *
 ******************************************************************************/
static
void create_cluster_graph(Graph::Graph& g,
    std::vector<int> clusters,
    int nCluster,
    std::vector<WgtType>& radii,
    Graph::Graph& cg)
{
    // Steps
    //   1. get the linkage between cluster
    //   2. link the edge
    using namespace std;
    vector< vector<int> > cls_connection(nCluster, vector<int>(nCluster, 0));
    vector<int> cls_nodes;
    vector<int> nbors;


    // Step 1
    for (int c=0; c<nCluster; ++c)
    {
        cls_nodes.clear();
        cls_nodes.resize(0);

        for (int i=0; i<g.get_num_vtxs(); ++i)
            if (clusters.at(i) == c) cls_nodes.push_back(i);


        for (int i=0; i<cls_nodes.size(); ++i)
        {
            nbors = g.adj(cls_nodes.at(i));
            for (int n=0; n<nbors.size(); ++n)
            {
                if (clusters.at(nbors.at(n)) != c)
                    cls_connection.at(c).at(clusters.at(nbors.at(n))) += 1;
            }
        }
    }
    // Step 2
    for (int i=0; i<nCluster; ++i)
    {
        for (int j=i+1; j<nCluster; ++j)
            if (cls_connection.at(i).at(j) != 0) cg.add_edge(i, j, radii.at(i) + radii.at(j));
    }
}
static
void calculate_rotate_angle(Graph::Graph& g, int cls, std::vector<int>& cluster_nodes,
	std::vector<int>& clusters,
	std::vector< std::vector<CoordType> >& coord,
	std::vector< std::vector<CoordType> >& center_coord,
	std::vector<WgtType>& radii,
	std::vector< WgtType >& rotate_angles)
{
	using namespace std;
	// torque
    int r_u; // current node id
    int r_v; // adjacent node id
    CoordType n_c_x; // center of neighbor's in different cluster
    CoordType n_c_y;
    CoordType r_u_x; // x components of raidus of u
    CoordType r_u_y;
    CoordType c_x;  // center of u's x coord
    CoordType c_y;
    vector<VtxType> nbors; // current node id
    double sin_coeff = 0.0;
    double cos_coeff = 0.0;
    pair<CoordType, CoordType> force;
    pair<CoordType, CoordType> arm;
    double force_val;
    double arm_val;
    double t_angle; // angle of torque

    // topo algo
    for (int j=0; j<cluster_nodes.size(); ++j)
    {
		r_u = cluster_nodes.at(j);
        r_u_x = coord.at(r_u).at(0);
        r_u_y = coord.at(r_u).at(1);
        c_x = center_coord.at(cls).at(0);
        c_y = center_coord.at(cls).at(1);
        arm = make_pair(r_u_x - c_x, r_u_y - c_y);
        arm_val = sqrt( pow(arm.first, 2) + pow(arm.second, 2));

		nbors = g.adj(r_u);
		for (int n=0; n<nbors.size(); ++n)
		{
			r_v = nbors.at(n);
			if ( clusters.at(r_u) != clusters.at(r_v) )
			{
				n_c_x = center_coord.at(clusters.at(r_v)).at(0);
            	n_c_y = center_coord.at(clusters.at(r_v)).at(1);

				// Rotate Step 1: calculate force and angle
				force = make_pair(n_c_x-r_u_x, n_c_y-r_u_y);
                force_val = sqrt( pow(force.first, 2) + pow(force.second, 2));
                force = make_pair(force.first/force_val, force.second/force_val);
                t_angle = (arm_val/radii.at(cls))*M_PI/2*sgn(arm.first*force.second-arm.second*force.first)*(arm.first*force.first+arm.second*force.second);
                rotate_angles.at(cls) += t_angle;
			}
		}

    }

	// for (int j=0; j<cluster_nodes.size(); ++j)
	// {
	// 	r_u = cluster_nodes.at(j);
 //        r_u_x = coord.at(r_u).at(0);
 //        r_u_y = coord.at(r_u).at(1);
 //        c_x = center_coord.at(cls).at(0);
 //        c_y = center_coord.at(cls).at(1);
 //        arm = make_pair(r_u_x - c_x, r_u_y - c_y);
 //        arm_val = sqrt( pow(arm.first, 2) + pow(arm.second, 2));

	// 	nbors = g.adj(r_u);
	// 	for (int n=0; n<nbors.size(); ++n)
	// 	{
	// 		r_v = nbors.at(n);
	// 		if ( clusters.at(r_u) != clusters.at(r_v) )
	// 		{
	// 			n_c_x = center_coord.at(clusters.at(r_v)).at(0);
 //            	n_c_y = center_coord.at(clusters.at(r_v)).at(1);

	// 			// Rotate Step 1: calculate force and angle
	// 			force = make_pair(n_c_x-r_u_x, n_c_y-r_u_y);
 //                force_val = sqrt( pow(force.first, 2) + pow(force.second, 2));
 //                t_angle = acos( (arm.first*force.first+arm.second*force.second) / (arm_val*force_val) );

 //                // Rotate Step 2
 //                // force_val = 1; // make force to be unit
 //                force_val = 1/force_val; // make force to be inverse to the distance
 //                sin_coeff += arm_val*force_val*cos(t_angle);
 //                cos_coeff += arm_val*force_val*sin(t_angle);
	// 		}
	// 	}
	// }

	// // Step 3
 //    cout << sin_coeff << " " << cos_coeff << endl;
 //    rotate_angles.at(cls) = atan(-cos_coeff/sin_coeff) * M_PI / 180;

}
static
void calculate_nodes_radii(Graph::Graph& g,
    DenseMat& distMat,
    std::vector<int>& clusters,
    std::vector<int>& cluster_nodes,
    std::vector< std::vector<CoordType> >& intra_coord,
    std::vector<WgtType>& nodes_radii)
{
    // Steps
    // 1. Calculate cluster radius
    // 2. get the inter/intra cluster degree of each vtxs
    // 3. calculate radial constriants
    using namespace std;

    // Step 1
    // get the corresponding cluster distance
    // minus central node

    // [modified!] clusters_nodes

    DenseMat clsDistMat(intra_coord.size()-1, intra_coord.size()-1);
    VtxType rr;
    VtxType cc;
    for (int c=0; c<clsDistMat.cols(); ++c)
    {
        for (int r=0; r<clsDistMat.rows(); ++r)
        {
            rr = cluster_nodes.at(r);
            cc = cluster_nodes.at(c);
            clsDistMat(c, r) = distMat(cc, rr);
        }
    }
    cout << "cls distance matrix" << endl;
    cout << clsDistMat << endl;
    // find the maximum pair distance
    double cls_radius = clsDistMat.maxCoeff()/2;

    // Step 2
    vector<VtxType> intra_deg(cluster_nodes.size(), 0);
    vector<VtxType> inter_deg(cluster_nodes.size(), 0);
    vector<VtxType> nbors;
    for (int i=0; i<cluster_nodes.size(); ++i)
    {
        nbors = g.adj( cluster_nodes.at(i) );
        for (int n=0; n<nbors.size(); ++n)
        {
            if (clusters.at( cluster_nodes.at(i) ) == clusters.at( nbors.at(n) ))
            {
                intra_deg.at(i) += 1;
            }
            else
            {
                inter_deg.at(i) += 1;
            }
        }
    }
    cout << "intra_deg=" << intra_deg.size() << endl;
    cout << "inter_deg=" << inter_deg.size() << endl;


    // Step 3
    const int offset = 1;
    double min_inter = *min_element(inter_deg.begin(), inter_deg.end());
    double max_inter = *max_element(inter_deg.begin(), inter_deg.end());
    cout << "radius" << endl;
    for (int i=0; i<nodes_radii.size(); ++i)
    {
        nodes_radii.at(i) = cls_radius*( (inter_deg.at(i)-min_inter+offset) / (max_inter-min_inter+offset) );
    }



}