// GenFem - A high-level finite element library
// Copyright (C) 2010-2026 Eric Bechet
//
// See the LICENSE file for license information and contributions.
// Please report all bugs and problems to <bechet@cadxfem.org>.



#include "SpaceReducerForStableMultipliers.h"

#include "MTriangle.h"
#include "MQuadrangle.h"
#include "MLine.h"
#include "MTetrahedron.h"
#include "MHexahedron.h"
#include "MElement.h"
#include <cassert>

void SpaceReducerForStableMultipliers::SortNodes (void)
{

    std::set< EdgeType > Se; // edges
    std::set< NodeType > Sn; // level set nodes kept
    std::map < NodeType, int > NodesScore;
    std::set< NodeType >::iterator itn;

    // initialization
    fillNodeToEdgeMap (Se,Sn);

    _AllEdges = Se;

    std::cout << "all level set nodes size : " << Sn.size() << std::endl;

    bool allcompute = false;
    if (allcompute)
    {
        //-- decimation algorithm --
        while (!Sn.empty())
        {
            // compute score of Sn
            ComputeScore(Se,Sn,NodesScore);
            // take the lowest score
            NodeType v = getNodeWithLowestScore(NodesScore);
            // if NOT marked as looser then it s a winner
            if (_looser_nodes.find(v) == _looser_nodes.end())
            {
                _winner_nodes.insert(v);
                _PhV.insert(_NodeToEdgeMap[v].first->getNum()); // keep the first edge node for ddl
                _PhV.insert(_NodeToEdgeMap[v].second->getNum());
            }
            // kill all connected edges and nodes (then mark them as loosers)
            killConnectedEdges (Se,Sn,v);
        }
    }
    else
    {

        // compute score of Sn
        ComputeScore(Se,Sn,NodesScore);
        //-- decimation algorithm --
        while (!Sn.empty())
        {
            // take the lowest score
            NodeType v = getNodeWithLowestScore(NodesScore);
            // if NOT marked as looser then it s a winner
            if (_looser_nodes.find(v) == _looser_nodes.end())
            {
                _winner_nodes.insert(v);
                _PhV.insert(_NodeToEdgeMap[v].first->getNum()); // keep the first edge node for ddl
                _PhV.insert(_NodeToEdgeMap[v].second->getNum());
                NodesScore.erase(v);
            }
            // kill all connected edges and nodes (then mark them as loosers)
            killConnectedEdges (Se,Sn,v,NodesScore);

            //std::cout << "Sn size : " << Sn.size() << std::endl;
        }

    }

    std::cout << "_PhV size : " << _PhV.size() << std::endl;
    std::cout << "_winner_nodes size : " << _winner_nodes.size() << std::endl;
    std::cout << "_looser_nodes size : " << _looser_nodes.size() << std::endl;

}

void SpaceReducerForStableMultipliers::fillNodeToEdgeMap(std::set< EdgeType > & Se , std::set< NodeType > & Sn)
{
    std::set<MElement*>::const_iterator it;
    for (it = _LevelSetElements->begin(); it != _LevelSetElements->end(); it++)
    {
        MElement *e = *it;
	if (e->getParent() == NULL)
	  continue;
	
        //std::cout << "getNumVertices : " << e->getNumVertices() << std::endl ;
        for (int i = 0 ; i < e->getNumVertices() ; i ++)  // warning getnumvertices polygon : vertices + inner ?
        {
            NodeType v = e->getVertex(i);
            EdgeType edge = findEdge(v,e);

            if (edge.first != NULL && edge.second != NULL)
                if (Sn.find(v) == Sn.end())
                {
                    Sn.insert(v);
                    Se.insert(edge);
                    _NodeToEdgeMap[v] = edge;
                }
        }
    }
}

std::pair <MVertex * , MVertex * > SpaceReducerForStableMultipliers::findEdge(NodeType v, MElement * e)
{
    MElement *ep;
    if (e->getParent())
      ep = e->getParent();
    
//     printf("parent type --> %d\n", ep->getType());

    switch (ep->getType())
    {
    case TYPE_TRI :
    {
        EdgeType edge;
        for (int i = 0 ; i < 3 ; i ++)  // for all edges
        {
            int n1,n2;
            n1 = i;
            n2 = (i+1)%3;
            // if it s an element vertex
            if (v->getNum() == ep->getVertex(n1)->getNum())
            {
                edge = EdgeType (ep->getVertex(n1),ep->getVertex(n1));
                return edge;
            }
            if (v->getNum() == ep->getVertex(n2)->getNum())
            {
                edge = EdgeType (ep->getVertex(n2),ep->getVertex(n2));
                return edge;
            }
            // else if it s not an element vertex
            edge = EdgeType(ep->getVertex(n1),ep->getVertex(n2));
            if (NodeBelongToEdge(v,edge))
	      return edge;
        }
        break;
    }
    case TYPE_QUA :
    {
        EdgeType edge;
        for (int i = 0 ; i < 4 ; i ++)  // for all edges
        {
            int n1,n2;
            n1 = i;
            n2 = (i+1)%4;
            // if it s an element vertex
            if (v->getNum() == ep->getVertex(n1)->getNum())
            {
                edge = EdgeType (ep->getVertex(n1),ep->getVertex(n1));
                return edge;
            }
            if (v->getNum() == ep->getVertex(n2)->getNum())
            {
                edge = EdgeType (ep->getVertex(n2),ep->getVertex(n2));
                return edge;
            }
            // else if it s not an element vertex
            edge = EdgeType(ep->getVertex(n1),ep->getVertex(n2));
            if (NodeBelongToEdge(v,edge))
	      return edge;
        }
    }
    case TYPE_TET :
    {
        //std::cout << "type tet" << std::endl ;
        int tab[6][2] = {{0, 1}, {0, 3}, {0, 2}, {1, 2}, {2, 3}, {3, 1}};  // edges
        EdgeType edge;
        for (int i = 0 ; i < 6 ; i ++)  // for all edges
        {
            int n1,n2;
            n1 = tab[i][0];
            n2 = tab[i][1];
            // if it s an element vertex
            if (v->getNum() == ep->getVertex(n1)->getNum())
            {
                edge = EdgeType (ep->getVertex(n1),ep->getVertex(n1));
                return edge;
            }
            if (v->getNum() == ep->getVertex(n2)->getNum())
            {
                edge = EdgeType (ep->getVertex(n2),ep->getVertex(n2));
                return edge;
            }
            // else if it s not an element vertex
            edge = EdgeType(ep->getVertex(n1),ep->getVertex(n2));
            if (NodeBelongToEdge(v,edge))
	      return edge;
        }
        break;
    }
    case TYPE_HEX :
    {
        //std::cout << "type hex" << std::endl ;
        int tab[12][2] = {{0, 1}, {0, 4}, {0, 3}, {1, 2}, {1, 5},
            {2, 3},{2,6},{3, 7}, {4, 7}, {4, 5},{5, 6},{6, 7}
        };// edges
        EdgeType edge;
        for (int i = 0 ; i < 12 ; i ++)  // for all edges
        {
            int n1,n2;
            n1 = tab[i][0];
            n2 = tab[i][1];
            // if it s an element vertex
            if (v->getNum() == ep->getVertex(n1)->getNum())
            {
                edge = EdgeType (ep->getVertex(n1),ep->getVertex(n1));
                return edge;
            }
            if (v->getNum() == ep->getVertex(n2)->getNum())
            {
                edge = EdgeType (ep->getVertex(n2),ep->getVertex(n2));
                return edge;
            }
            // else if it s not an element vertex
            edge = EdgeType(ep->getVertex(n1),ep->getVertex(n2));
            if (NodeBelongToEdge(v,edge))
	      return edge;
        }
        break;
    }
    default :
      printf("Unknow Element type ...\n");
      assert(0);
    }
    return EdgeType(NULL, NULL);
}


bool SpaceReducerForStableMultipliers::NodeBelongToEdge(NodeType v, EdgeType edge)
{

    double eps = 1e-10;

    double distn, edgelength;

    edgelength = edge.first->distance(edge.second);
    distn = v->distance(edge.first) + v->distance(edge.second);

//     std::cout << "std::abs(edgelength - distn)..." << std::abs(edgelength - distn) << std::endl;

    if (std::abs(edgelength - distn) < eps)
      return true;
    
    return false;
}


void SpaceReducerForStableMultipliers::ComputeScore(std::set< EdgeType > & Se, std::set< NodeType > & Sn, std::map < NodeType , int > & NodesScore)
{
    NodesScore.clear();
    std::set < NodeType >::iterator it;
    
    for (it = Sn.begin(); it != Sn.end(); it++)
    {
	NodeType n = *it;
        int score;
        std::map< NodeType, EdgeType >::iterator itm;
	itm =  _NodeToEdgeMap.find(n);
	assert(itm != _NodeToEdgeMap.end());
	
	EdgeType edge(itm->second.first, itm->second.second);

	if (edge.first->getNum() == edge.second->getNum())
	  score = 0;  // regular mesh nodes score
        else
        {
            score = ComputeIncidentEdges(Se,edge.first) + ComputeIncidentEdges(Se,edge.second);
        }
        NodesScore.insert(std::pair<NodeType, int>(n, score));
    }
}


int SpaceReducerForStableMultipliers::ComputeIncidentEdges(std::set< EdgeType > & Se, NodeType & v)
{
    int n = 0;
    std::set < EdgeType >::iterator it;
    for (it = Se.begin(); it!=Se.end(); it++)
    {
	EdgeType edge = (*it);
        if ((edge.first == v) || (edge.second == v))
	  n++;
    }
    return n;
}

int SpaceReducerForStableMultipliers::ComputeIncidentEdges(std::set< EdgeType > & Se, NodeType & v, std::set< NodeType > & Nr)
{
    int n = 0;
    Nr.clear();
    std::set < EdgeType >::iterator it;
    for (it = Se.begin(); it != Se.end(); it++)
    {
        EdgeType edge = (*it);
        if ((edge.first == v) || (edge.second == v))
        {
            n++;
            if (edge.first == v)
                Nr.insert(edge.second);
            else
                Nr.insert(edge.first);
        }
    }
    return n;
}


MVertex * SpaceReducerForStableMultipliers::getNodeWithLowestScore(std::map < NodeType , int > & NodesScore)
{
    NodeType v1 = NodesScore.begin()->first;
    int lowestscore = NodesScore.begin()->second;
    std::map < NodeType , int >::iterator it;
    for (it = NodesScore.begin(); it != NodesScore.end(); it++)
    {
	int tmpscore = it->second;
        if (tmpscore < lowestscore)
	{
	  v1 = it->first ;
	  lowestscore = it->second;
	}
    }
    return v1;
}

void SpaceReducerForStableMultipliers::killConnectedEdges (std::set< EdgeType > & Se , std::set< NodeType > & Sn, NodeType v)
{

    std::vector < EdgeType > EdgesToErase;
    std::vector < NodeType > NodesToErase;
    
    std::map < NodeType , EdgeType >::iterator it;
    it = _NodeToEdgeMap.find(v);
    
    assert(it != _NodeToEdgeMap.end());
    
    EdgeType edge = it->second;
    NodeType v1 = edge.first;
    NodeType v2 = edge.second;
    
    assert(v1 != NULL);
    assert(v2 != NULL);

    std::set < EdgeType >::iterator ite;
    for (ite = Se.begin(); ite != Se.end(); ite++)
    {
	EdgeType tryedge = (*ite);
        if ((tryedge.first == v1) || (tryedge.second == v1))
	  Se.erase(ite--);
// 	  EdgesToErase.push_back(tryedge);  
        else if ((tryedge.first == v2) || (tryedge.second == v2))
	  Se.erase(ite--);
// 	  EdgesToErase.push_back(tryedge);
    }
//     for (unsigned int i = 0 ; i < EdgesToErase.size() ; i ++)
//     {
//         Se.erase(EdgesToErase[i]);
//     }

    std::set < NodeType >::iterator itn;
    for (itn = Sn.begin(); itn != Sn.end() ; itn ++)
    {
	NodeType trynode = (*itn);
        it = _NodeToEdgeMap.find(trynode);
        assert(it != _NodeToEdgeMap.end());
    
	EdgeType tryedge = it->second;
	
        if (Se.find(tryedge) == Se.end())
	{  
	  if (trynode->getNum() != v->getNum())
	    _looser_nodes.insert(trynode);
	  Sn.erase(itn--);
// 	  NodesToErase.push_back(*itn);
	}
    }
//     for (unsigned int i = 0 ; i < NodesToErase.size() ; i ++)
//     {
//         if (NodesToErase[i]->getNum() != v->getNum()) _looser_nodes.insert(NodesToErase[i]);
//         Sn.erase(NodesToErase[i]);
//     }
}

void SpaceReducerForStableMultipliers::killConnectedEdges (std::set< EdgeType > & Se , std::set< NodeType > & Sn, NodeType v, std::map<NodeType, int > & NodesScore)
{

    std::vector < EdgeType > EdgesToErase;
    std::vector < NodeType > NodesToErase;

    NodeType v1 = _NodeToEdgeMap[v].first;
    NodeType v2 = _NodeToEdgeMap[v].second;

    std::set < EdgeType >::iterator ite = Se.begin();
    for (;ite!=Se.end();ite++)
    {
        if (((*ite).first == v1) | ((*ite).second == v1)) EdgesToErase.push_back(*ite);
        if (((*ite).first == v2) | ((*ite).second == v2)) EdgesToErase.push_back(*ite);
    }
    for (unsigned int i = 0 ; i < EdgesToErase.size() ; i ++)
    {
        Se.erase(EdgesToErase[i]);
    }


    std::set < NodeType >::iterator itn = Sn.begin();
    for (; itn != Sn.end() ; itn ++)
    {
        if (Se.find(_NodeToEdgeMap[(*itn)]) == Se.end()) NodesToErase.push_back(*itn);
    }
    for (unsigned int i = 0 ; i < NodesToErase.size() ; i ++)
    {
        if (NodesToErase[i]->getNum() != v->getNum()) _looser_nodes.insert(NodesToErase[i]);
        Sn.erase(NodesToErase[i]);
        NodesScore.erase(NodesToErase[i]);
    }

}

void SpaceReducerForStableMultipliers::BuildLinearConstraints(bool comp[], dofManager<double> *pAssembler)
{
    int nb = 0 ;

    DofAffineConstraint<double> constraint;
    std::set<MElement*>::const_iterator it;
    for (it = _LevelSetElements->begin(); it != _LevelSetElements->end(); it++)
    {
        MElement *e = *it;
        for (int i = 0 ; i < e->getNumVertices() ; i ++)  // warning getnumvertices polygon : vertices + inner ?
        {
            NodeType v = e->getVertex(i);
            EdgeType edge = _NodeToEdgeMap[v];

            if (edge.first != NULL && edge.second != NULL)
            {
                if (_winner_nodes.find(v) != _winner_nodes.end()) // if e->getVertex(i) is a winner nodes
                {
                    //std::cout << "winner node : " << std::endl;
                    if (edge.first->getNum() != edge.second->getNum()) // if not a "volume" mesh node constraint between nodes
                    {
                        double fac = 1;
                        for (int j =0; j < 3 ; j ++ )  // for x, y, z  VectorLagrangeFunctionSpace
                        {
                            if (comp[j])
                            {
                                constraint.linear.clear();
                                int type = Dof::createTypeWithTwoInts(j , _FieldTag);
                                Dof ddlp(edge.first->getNum(),type);
                                Dof ddlc(edge.second->getNum() ,type);
                                std::pair<Dof, double >  linDof(ddlp,fac);
                                constraint.linear.push_back(linDof);
                                constraint.shift = 0;
                                pAssembler->setLinearConstraint (ddlc, constraint);
//                std::cout << "nndlc : " << edge.second->getNum() << " = " << fac <<  " " << edge.first->getNum() << std::endl;
//                std::cout << "nndlc : " << edge.second->x() << " " <<  edge.second->y() << std::endl;
//                std::cout << "nndlp : " << edge.first->x() << " " <<  edge.first->y() << std::endl;
                                nb++;
                            }
                        }
                    }
                }
                else  // not winner node, maybe a node as to be added to lin comb
                {

                    NodeType vi = edge.first;
                    if (_PhV.find(vi->getNum()) == _PhV.end() ) // if edge.first not in PhV
                    {
                        //std::cout << "NOT winner node but added !" << std::endl;
                        std::set< NodeType > Nr;
                        double fac;
                        int n = ComputeIncidentEdges(_AllEdges,vi,Nr);
                        fac = 1./n;
                        for (int j =0; j < 3 ; j ++ )  // for x, y, z  VectorLagrangeFunctionSpace
                        {
                            if (comp[j])
                            {
                                constraint.linear.clear();
                                int type = Dof::createTypeWithTwoInts(j , _FieldTag);
                                Dof ddlc(vi->getNum() ,type);
//                std::cout << "nndlc : " << vi->getNum() << " = " << fac << std::endl;
//                std::cout << "nndlc : " << vi->x() << " " <<  vi->y() << std::endl;
                                std::set < NodeType >::iterator itn = Nr.begin();
                                for ( ; itn != Nr.end() ; itn++)
                                {
                                    Dof ddlp((*itn)->getNum(),type);
                                    std::pair<Dof, double >  linDof(ddlp,fac);
                                    constraint.linear.push_back(linDof);
                                    constraint.shift = 0;
//                  std::cout << "nndlp : " << (*itn)->getNum() << std::endl ;
//                  std::cout << "nndlp : " << (*itn)->x() << " " <<  (*itn)->y() << std::endl;
                                    nb++;
                                }
                                pAssembler->setLinearConstraint (ddlc, constraint);
                            }
                        }
                    }
                    vi = edge.second;
                    if (_PhV.find(vi->getNum()) == _PhV.end() ) // if edge.first not in PhV
                    {
                        //std::cout << "NOT winner node but added !" << std::endl;
                        std::set< NodeType > Nr;
                        double fac;
                        int n = ComputeIncidentEdges(_AllEdges,vi,Nr);
                        fac = 1./n;
                        for (int j =0; j < 3 ; j ++ )  // for x, y, z  VectorLagrangeFunctionSpace
                        {
                            if (comp[j])
                            {
                                constraint.linear.clear();
                                int type = Dof::createTypeWithTwoInts(j , _FieldTag);
                                Dof ddlc(vi->getNum() ,type);
//                std::cout << "nndlc : " << vi->getNum() << " = " << fac << std::endl;
//                std::cout << "nndlc : " << vi->x() << " " <<  vi->y() << std::endl;
                                std::set < NodeType >::iterator itn = Nr.begin();
                                for ( ; itn != Nr.end() ; itn++)
                                {
                                    Dof ddlp((*itn)->getNum(),type);
                                    std::pair<Dof, double >  linDof(ddlp,fac);
                                    constraint.linear.push_back(linDof);
                                    constraint.shift = 0;
//                  std::cout << "nndlp : " << (*itn)->getNum() << std::endl ;
//                  std::cout << "nndlp : " << (*itn)->x() << " " <<  (*itn)->y() << std::endl;
                                    nb++;
                                }
                                pAssembler->setLinearConstraint (ddlc, constraint);
                            }
                        }
                    }
                }
            }
        }
    }
}