#include <list>
#include <queue>

#include <wrap/callback.h>

#ifndef MESHLAB_ALIGNGLOBAL_H
#define MESHLAB_ALIGNGLOBAL_H

namespace vcg {
    class AlignGlobal {
    public:

        /**
         * Forward declaration for the `VirtAlign` class.
         */
        class Node;

        /**
         * Allineamento virtuale tra due mesh (estratto da un alignresult).
         * Nota Importante: la trasformazione e i punti qui memorizzati si intendono al netto delle trasf di base delle due mesh in gioco.
         * Quindi se qualcuno sposta una mesh le pos dei punti sono ancora valide ma non la trasf da applicarvi.
         */
        class VirtAlign
        {
        public:

            AlignGlobal::Node *Fix, *Mov; // allineamento tra i e j
            std::vector<vcg::Point3d> FixP; // punti su Fix
            std::vector<vcg::Point3d> MovP; // punti su Mov
            std::vector<vcg::Point3d> FixN; // Normali su Fix
            std::vector<vcg::Point3d> MovN; // Normali su Mov
            vcg::Matrix44d M2F; //la matrice da applicare ai punti di Mov per ottenere quelli su Fix
            vcg::Matrix44d F2M; //la matrice da applicare ai punti di Fix per ottenere quelli su Mov
            /*
                Nel caso semplificato che le mesh avessero come trasf di base l'identita' deve valere:

                         N2A(N).Apply(   P(N)) ~= AdjP(N)
                         A2N(N).Apply(AdjP(N)) ~=    P(N)

                In generale un nodo N qualsiasi dell'VirtAlign vale che:

                    N2A(N).Apply(       N->M.Apply(   P(N)) ) ~= AdjN(N)->M.Apply( AdjP(N) );
                    A2M(N).Apply( AdjN(N)->M.Apply(AdjP(N)) ) ~=       N->M.Apply(    P(N) );

                in cui il ~= significa uguale al netto dell'errore di allineamento.

                Per ottenere i virtualmate relativi ad un nodo n:
            */

            inline vcg::Matrix44d       &N2A(AlignGlobal::Node *n) {if(n==Fix) return F2M; else return M2F;}
            inline vcg::Matrix44d       &A2N(AlignGlobal::Node *n) {if(n==Fix) return M2F; else return F2M;}

            inline std::vector<vcg::Point3d>    &P(AlignGlobal::Node *n) {if(n==Fix) return FixP; else return MovP;}
            inline std::vector<vcg::Point3d>    &N(AlignGlobal::Node *n) {if(n==Fix) return FixN; else return MovN;}

            inline std::vector<vcg::Point3d> &AdjP(AlignGlobal::Node *n) {if(n==Fix) return MovP; else return FixP;}
            inline std::vector<vcg::Point3d> &AdjN(AlignGlobal::Node *n) {if(n==Fix) return MovN; else return FixN;}

            AlignGlobal::Node *Adj(Node *n) const {

                assert(n == Fix || n == Mov);
                if (n == Fix) return Mov;

                return Fix;
            }

            bool Check() const {

                if (FixP.size() != MovP.size()) return false;

                Point3d mp, fp;

                double md = 0, fd = 0;
                double md2 = 0, fd2 = 0;

                Matrix44d &MovTr=Mov->M;
                Matrix44d &FixTr=Fix->M;

                for (std::size_t i = 0; i < FixP.size(); ++i) {

                    mp = MovTr * MovP[i];
                    fp = FixTr * FixP[i];

                    md  +=        Distance(fp, M2F * mp);
                    md2 += SquaredDistance(fp, M2F * mp);

                    fd  +=        Distance(mp, F2M * fp);
                    fd2 += SquaredDistance(mp, F2M * fp);
                }

                int nn = static_cast<int>(MovP.size());

                std::fprintf(stdout, "Arc %3i -> %3i : %i pt\n", Fix->id, Mov->id, nn);
                std::fprintf(stdout, "SquaredSum Distance %7.3f %7.3f Avg %7.3f %7.3f\n", fd2, md2, fd2/nn, md2/nn);
                std::fprintf(stdout, "       Sum Distance %7.3f %7.3f Avg %7.3f %7.3f\n", fd , md ,  fd/nn, md/nn);
                return true;
            }
        };

        class Node {
        public:

            int id;  // id della mesh a cui corrisponde il nodo
            int sid; // Subgraph id;
            Matrix44d M; // La matrice che mette la mesh nella sua posizione di base;
            std::list<VirtAlign*> Adj;

            /***
             * True if the node is inside the active set
             */
            bool Active;

            /***
             * False if it's dormant
             */
            bool Queued;
            bool Discarded;

            Node() : id{-1}, Active{false}, Discarded{false}, Queued{false} {}

            // Allinea un nodo con tutti i suoi vicini
            double AlignWithActiveAdj(bool Rigid) {

                std::printf("--- AlignWithActiveAdj --- \nMoving node %i with respect to nodes:", id);

                for (auto li = std::begin(Adj); li != std::end(Adj); ++li) {
                    if ((*li)->Adj(this)->Active) {
                        std::printf(" %i,", (*li)->Adj(this)->id);
                    }
                }

                std::printf("\n");

                //printf("Base Matrix of Node %i\n",id);print(M);

                // Step 1; Costruiamo le due liste di punti da allineare
                std::vector<Point3d> FixP, MovP, FixN, MovN;
                Box3d FixBox, MovBox;
                FixBox.SetNull(); MovBox.SetNull();

                for (auto li = std::begin(Adj); li != std::end(Adj); ++li) {

                    // scorro tutti i nodi adiacenti attivi
                    if ((*li)->Adj(this)->Active) {
                        //printf("Base Matrix of Node %i adj to %i\n",id,(*li)->Adj(this)->id);
                        //print((*li)->Adj(this)->M);
                        std::vector<Point3d> &AP=(*li)->AdjP(this);   // Punti sul nodo adiacente corrente;
                        std::vector<Point3d> &AN=(*li)->AdjN(this);   // Normali sul nodo adiacente corrente;

                        //printf("Transf that bring points of %i onto %i\n",id,(*li)->Adj(this)->id);
                        //print((*li)->A2N(this));
                        //printf("Transf that bring points of %i onto %i\n",(*li)->Adj(this)->id,id);
                        //print((*li)->N2A(this));
                        vcg::Point3d pf, nf;
                        vcg::Point3d pm;

                        for (std::size_t i = 0; i < AP.size(); ++i) {

                            pf = (*li)->Adj(this)->M*AP[i]; // i punti fissi sono quelli sulla sup degli adiacenti messi nella loro pos corrente
                            FixP.push_back(pf);
                            FixBox.Add(pf);
                            nf=(*li)->Adj(this)->M*Point3d(AP[i]+AN[i])-pf;
                            nf.Normalize();
                            FixN.push_back(nf);

                            pm = (*li)->A2N(this)*pf;
                            MovP.push_back(pm); // i punti che si muovono sono quelli sul adj trasformati in modo tale da cascare sul nodo corr.
                            MovBox.Add(pm);
                        }
                    }
                }

                vcg::Matrix44d out;
                //if(Rigid) ret=ComputeRigidMatchMatrix(out,FixP,MovP);
                //else ret=ComputeMatchMatrix2(out,FixP,FixN,MovP);
                if (Rigid) {
                    ComputeRigidMatchMatrix(FixP,MovP,out);
                }
                else {
                    vcg::PointMatchingScale::computeRotoTranslationScalingMatchMatrix(out, FixP, MovP);
                }

                Matrix44d outIn=vcg::Inverse(out);
                //double maxdiff = MatrixNorm(out);
                double maxdiff = MatrixBoxNorm(out,FixBox);

                //	printf("Computed Transformation:\n"); print(out);printf("--\n");
                //	printf("Collected %i points . Err = %f\n",FixP.size(),maxdiff);

                // La matrice out calcolata e' quella che applicata ai punti MovP li porta su FixP, quindi i punti della mesh corrente
                // La nuova posizione di base della mesh diventa quindi
                // M * out
                // infatti se considero un punto della mesh originale applicarci la nuova matricie significa fare
                // p * M * out

                // M=M*out; //--Orig
                M = out * M;

                // come ultimo step occorre applicare la matrice trovata a tutti gli allineamenti in gioco.
                // scorro tutti i nodi adiacenti attivi
                for (auto li = std::begin(Adj); li != std::end(Adj); ++li) {
                    //print((*li)->N2A(this));
                    //print((*li)->A2N(this));printf("--\n");

                    (*li)->N2A(this)=(*li)->N2A(this)*outIn;
                    (*li)->A2N(this)=(*li)->A2N(this)*out  ;
                    //print((*li)->N2A(this));
                    //print((*li)->A2N(this));printf("--\n");
                }

                return maxdiff;
            }

            double MatrixNorm(vcg::Matrix44d &NewM) const {

                double maxDiff = 0;

                vcg::Matrix44d diff;
                diff.SetIdentity();
                diff = diff-NewM;

                for (int i = 0; i < 4; ++i) {
                    for (int j = 0; j < 4; ++j) {
                        maxDiff += (diff[i][j] * diff[i][j]);
                    }
                }

                return maxDiff;
            }

            double MatrixBoxNorm(vcg::Matrix44d &NewM, vcg::Box3d &bb) const {

                double maxDiff = 0;
                vcg::Point3d pt;

                pt = Point3d(bb.min[0], bb.min[1], bb.min[2]); maxDiff = std::max(maxDiff, Distance(pt, NewM * pt));
                pt = Point3d(bb.max[0], bb.min[1], bb.min[2]); maxDiff = std::max(maxDiff, Distance(pt, NewM * pt));
                pt = Point3d(bb.min[0], bb.max[1], bb.min[2]); maxDiff = std::max(maxDiff, Distance(pt, NewM * pt));
                pt = Point3d(bb.max[0], bb.max[1], bb.min[2]); maxDiff = std::max(maxDiff, Distance(pt, NewM * pt));
                pt = Point3d(bb.min[0], bb.min[1], bb.max[2]); maxDiff = std::max(maxDiff, Distance(pt, NewM * pt));
                pt = Point3d(bb.max[0], bb.min[1], bb.max[2]); maxDiff = std::max(maxDiff, Distance(pt, NewM * pt));
                pt = Point3d(bb.min[0], bb.max[1], bb.max[2]); maxDiff = std::max(maxDiff, Distance(pt, NewM * pt));
                pt = Point3d(bb.max[0], bb.max[1], bb.max[2]); maxDiff = std::max(maxDiff, Distance(pt, NewM * pt));

                return maxDiff;
            }

            int PushBackActiveAdj(std::queue<Node *> &Q) {

                assert(Active);

                int count = 0;
                AlignGlobal::Node *pt;

                for (auto li = std::begin(Adj); li != std::end(Adj); ++li) {

                    pt = (*li)->Adj(this);

                    if (pt->Active && !pt->Queued) {
                        ++count;
                        pt->Queued=true;
                        Q.push(pt);
                    }
                }
                return count;
            }

            int DormantAdjNum() {

                int count = 0;

                for (auto li = std::begin(Adj); li != std::end(Adj); ++li) {
                    if (!(*li)->Adj(this)->Active) ++count;
                }

                return count;
            }

            int ActiveAdjNum() {

                int count = 0;

                for (auto li = std::begin(Adj); li != std::end(Adj); ++li) {
                    if ((*li)->Adj(this)->Active) ++count;
                }

                return count;
            }
        };

        class SubGraphInfo {
        public:
            int sid;
            int size;
            Node *root;
        };

        std::list<Node> N;
        std::list<VirtAlign*> A;

        /**
         * Descrittori delle componenti connesse, riempito dalla ComputeConnectedComponents
         */
        std::list<SubGraphInfo> CC;

        static inline void LOG( FILE *fp, const char * f, ... ) {

            if (fp == 0) return;

            va_list marker;
            va_start(marker, f);
            std::vfprintf(fp, f, marker);
            va_end(marker);
            std::fflush(fp);
        }

        inline int DormantNum() const {

            int count = 0;

            for (auto li = std::begin(N); li != std::end(N); ++li) {
                if (!(*li).Active) ++count;
            }

            return count;
        }

        inline int ActiveNum() const {

            int count = 0;

            for (auto li = std::begin(N); li != std::end(N); ++li) {
                if ((*li).Active) ++count;
            }

            return count;
        }

        bool CheckGraph() {

            std::vector<bool> Visited(N.size(), false);
            std::stack<AlignGlobal::Node*> st;

            st.push(&(*N.begin()));
            while (!st.empty()) {

                AlignGlobal::Node *cur=st.top();
                st.pop();
                // std::printf("Visiting node %i\n",cur->id);

                for (auto li = std::begin(cur->Adj); li != std::end(cur->Adj); ++li) {
                    if (!Visited[(*li)->Adj(cur)->id]) {
                        Visited[(*li)->Adj(cur)->id] = true;
                        st.push((*li)->Adj(cur));
                    }
                }
            }

            size_t cnt = std::count(std::begin(Visited), std::end(Visited), true);
            std::printf("Nodes that can be reached from root %zu on %zu \n", cnt, N.size());

            return (cnt == N.size());
        }

        void Dump(FILE *fp) const {
            std::fprintf(fp, "Alignment Graph of %lu nodes and %lu arcs\n", N.size(), A.size());
            //  list<VirtAlign *>::iterator li;
            //	for(li=A.begin();li!=A.end();++li)
            //		printf("Arc : %3i ->%3i\n",(*li)->Fix->id,(*li)->Mov->id);
        }

        int ComputeConnectedComponents() {

            std::printf("Building Connected Components on a graph with %lu nodes and %lu arcs\n", N.size(), A.size());

            CC.clear();

            std::stack<AlignGlobal::Node *> ToReach; // nodi ancora da visitare
            std::stack<AlignGlobal::Node *> st;      // nodi che si stanno visitando

            for (auto li = std::begin(N); li != std::end(N); ++li) {
                (*li).sid = -1;
                ToReach.push(&*li);
            }

            int cnt = 0;

            while (!ToReach.empty()) {

                SubGraphInfo sg;
                st.push(&*ToReach.top());
                ToReach.pop();

                assert(st.top()->sid==-1);

                sg.root=st.top();
                sg.sid=cnt;
                sg.size=0;
                st.top()->sid=cnt;

                while (!st.empty()) {

                    AlignGlobal::Node *cur=st.top();
                    st.pop();
                    ++sg.size;

                    assert(cur->sid==cnt);
					// std::printf("Visiting node %2i %2i\n",cur->id,cur->sid);

                    for (auto li = std::begin(cur->Adj); li != std::end(cur->Adj); ++li) {

                        if ((*li)->Adj(cur)->sid == -1) {
                            (*li)->Adj(cur)->sid=cnt;
                            st.push((*li)->Adj(cur));
                        }
                        else {
                            assert((*li)->Adj(cur)->sid == cnt);
                        }
                    }

                }

                cnt++;
                CC.push_back(sg);

                while (!ToReach.empty() && ToReach.top()->sid != -1) ToReach.pop();
            }

            return cnt;
        }

        void Clear() {

            for (auto li = std::begin(A); li != std::end(A); ++li) {
                delete (*li);
            }

            N.clear();
            A.clear();
        }

        void MakeAllDormant() {
            for (auto li = std::begin(N); li != std::end(N); ++li) {
                (*li).Active=false;
            }
        }

        bool GlobalAlign(const std::map<int, std::string> &Names, const double epsilon, int maxiter, bool Rigid, FILE *elfp, vcg::CallBackPos* cb) {

            double change;
            int step = 0, localmaxiter;

            if (cb != NULL) cb(0, "Global Alignment...");
            AlignGlobal::LOG(elfp,"----------------\n----------------\nGlobalAlignment (target eps %7.3f)\n", epsilon);

            std::queue<AlignGlobal::Node *> Q;
            MakeAllDormant();
            AlignGlobal::Node *curr = ChooseDormantWithMostDormantLink();
            curr->Active = true;

            int cursid = curr->sid;
            AlignGlobal::LOG(elfp, "Root node %i '%s' with %i dormant link\n", curr->id, Names.find(curr->id)->second.c_str(), curr->DormantAdjNum());

            while (DormantNum() > 0) {

                AlignGlobal::LOG(elfp,"---------\nGlobalAlignment loop DormantNum = %i\n", DormantNum());

                curr = ChooseDormantWithMostActiveLink();
                if (!curr) {
                    // la componente connessa e' finita e si passa alla successiva cercando un dormant con tutti dormant.
                    AlignGlobal::LOG(elfp,"\nCompleted Connected Component %i\n", cursid);
                    AlignGlobal::LOG(elfp,"\nDormant Num: %i\n", DormantNum());

                    curr = ChooseDormantWithMostDormantLink();
                    if (curr == nullptr) {
                        AlignGlobal::LOG(elfp,"\nFailed ChooseDormantWithMostDormantLink, chosen id:%i\n" ,0);
                        break; // non ci sono piu' componenti connesse composte da piu' di una singola mesh.
                    }
                    else {
                        AlignGlobal::LOG(elfp,"\nCompleted ChooseDormantWithMostDormantLink, chosen id:%i\n" ,curr->id);
                    }

                    curr->Active = true;
                    cursid = curr->sid;
                    curr = ChooseDormantWithMostActiveLink ();
                    if (curr == nullptr) {
                        AlignGlobal::LOG(elfp, "\nFailed    ChooseDormantWithMostActiveLink, chosen id:%i\n", 0);
                    }
                    else {
                        AlignGlobal::LOG(elfp, "\nCompleted ChooseDormantWithMostActiveLink, chosen id:%i\n", curr->id);
                    }
                }

                AlignGlobal::LOG(elfp,"\nAdded node %i '%s' with %i/%i Active link\n",curr->id,Names.find(curr->id)->second.c_str(),curr->ActiveAdjNum(),curr->Adj.size());
                curr->Active=true;
                curr->Queued=true;

                // Si suppone, ad occhio, che per risistemare un insieme di n mesh servano al piu' 10n passi;
                localmaxiter = ActiveNum() * 10;
                Q.push(curr);
                step = 0;

                // Ciclo interno di allineamento
                while (!Q.empty()) {

                    curr = Q.front();
                    Q.pop();

                    curr->Queued=false;
                    change = curr->AlignWithActiveAdj(Rigid);
                    step++;

                    AlignGlobal::LOG(elfp, "     Step %5i Queue size %5i Moved %4i  err %10.4f\n", step, Q.size(), curr->id, change);

                    if (change > epsilon) {

                        curr->PushBackActiveAdj(Q);
                        AlignGlobal::LOG(elfp,"         Large Change pushing back active nodes adj to %i to Q (new size %i)\n",curr->id,Q.size());

                        if (change > epsilon * 1000) {
                            std::printf("Large Change Warning\n\n");
                        }
                    }
                    if (step > localmaxiter) return false;
                    if (step > maxiter) return false;
                }
            }

            if (!curr) {

                AlignGlobal::LOG(elfp,"Alignment failed for %i meshes:\n",DormantNum());
                for (auto li = std::begin(N); li != std::end(N); ++li){
                    if (!(*li).Active) {
                        //(*li).M.SetIdentity();
                        (*li).Discarded=true;
                        AlignGlobal::LOG(elfp, "%5i\n", (*li).id);
                    }
                }
            }

            AlignGlobal::LOG(elfp,"Completed Alignment in %i steps with error %f\n",step,epsilon);
            return true;
        }

        AlignGlobal::Node* ChooseDormantWithMostDormantLink() {

            int MaxAdjNum = 0;
            AlignGlobal::Node *BestNode = nullptr;

            for (auto li = std::begin(N); li != std::end(N); ++li) {
                if (!(*li).Active) {
                    int AdjNum = (*li).DormantAdjNum();
                    if (AdjNum > MaxAdjNum) {
                        MaxAdjNum = AdjNum;
                        BestNode = &(*li);
                    }
                }
            }

            if (!BestNode){
                std::printf("Warning! Unable to find a Node with at least a dormant link!!\n");
                return nullptr;
            }

            assert(BestNode);
            assert(!BestNode->Queued);
            assert(!BestNode->Active);

            return BestNode;
        }

        AlignGlobal::Node* ChooseDormantWithMostActiveLink() {

            int MaxAdjNum = 0;
            AlignGlobal::Node* BestNode = nullptr;

            for (auto li = std::begin(N); li != std::end(N); ++li) {
                if (!(*li).Active) {
                    int AdjNum = (*li).ActiveAdjNum();
                    if (AdjNum > MaxAdjNum) {
                        MaxAdjNum = AdjNum;
                        BestNode = &(*li);
                    }
                }
            }

            if (!BestNode){
                // Abbiamo finito di sistemare questa componente connessa.
                std::printf("Warning! Unable to find a Node with at least an active link!!\n");
                return nullptr;
            }

            assert(BestNode);
            assert(!BestNode->Queued);
            assert(!BestNode->Active);

            return BestNode;
        }

        void BuildGraph(std::vector<AlignPair::Result *> &Res, std::vector<Matrix44d> &Tr, std::vector<int> &Id) {

            Clear();
            // si suppone che la matrice Tr[i] sia relativa ad un nodo con id Id[i];
            int mn = static_cast<int>(Tr.size());

            //	printf("building graph\n");
            AlignGlobal::Node rgn;
            rgn.Active = false;
            rgn.Queued = false;
            rgn.Discarded = false;

            std::map<int, AlignGlobal::Node *> Id2N;
            std::map<int, int> Id2I;

            for (int i = 0; i < mn; ++i) {
                rgn.id = Id[i];
                rgn.M = Tr[i];
                N.push_back(rgn);
                Id2N[rgn.id] = &(N.back());
                Id2I[rgn.id] = i;
            }

            std::printf("building %zu graph arcs\n",Res.size());
            AlignGlobal::VirtAlign *tv;

            // Ciclo principale in cui si costruiscono i vari archi
            // Si assume che i result siano fatti nel sistema di riferimento della matrici fix.

            for (auto rii = std::begin(Res); rii != std::end(Res); ++rii) {

                AlignPair::Result *ri = *rii;
                tv = new VirtAlign();
                tv->Fix = Id2N[(*ri).FixName];
                tv->Mov = Id2N[(*ri).MovName];
                tv->Fix->Adj.push_back(tv);
                tv->Mov->Adj.push_back(tv);
                tv->FixP = (*ri).Pfix;
                tv->MovP = (*ri).Pmov;
                tv->FixN = (*ri).Nfix;
                tv->MovN = (*ri).Nmov;

                /*

                    Siano:
                    Pf e Pm   i punti sulle mesh fix e mov nei sist di rif originali
                    Pft e Pmt i punti sulle mesh fix e mov dopo le trasf correnti;
                    Mf e Mm  le trasf che portano le  mesh fix e mov nelle posizioni correnti;
                    If e Im  le trasf inverse di cui sopra
                    Vale:
                    Pft = Mf*Pf  e Pmt = Mm*Pm
                     Pf = If*Pft e Pm = Im*Pmt

                                 Res *   Pm     =         Pf;
                                 Res * Im * Pmt =      If * Pft
                        Mf * Res * Im * Pmt = Mf * If * Pft
                    (Mf * Res * Im) * Pmt = Pft

                */
                Matrix44d Mm = Tr[Id2I[(*ri).MovName]];
                Matrix44d Mf = Tr[Id2I[(*ri).FixName]];
                Matrix44d Im = Inverse(Mm);
                Matrix44d If = Inverse(Mf);

                Matrix44d NewTr = Mf * (*ri).Tr * Im; // --- orig

                tv->M2F = NewTr;
                tv->F2M = Inverse(NewTr);

                assert(tv->Check());
                A.push_back(tv);
            }

            ComputeConnectedComponents();
        }

        bool GetMatrixVector(std::vector<Matrix44d> &Tr, std::vector<int> &Id) {

            std::map<int, AlignGlobal::Node *> Id2N;

            Tr.clear();

            for (auto li = std::begin(N); li != std::end(N); ++li) {
                Id2N[(*li).id] = &*li;
            }

            Tr.resize(Id.size());

            for (std::size_t i = 0; i < Id.size(); ++i) {

                if (Id2N[Id[i]] == 0) return false;
                Tr[i] = Id2N[Id[i]]->M;
            }

            return false;
        }

    };
}

#endif //MESHLAB_ALIGNGLOBAL_H