/*
interaction.cpp

Copyright (c) 2014, 2015, 2016 Terumasa Tadano

This file is distributed under the terms of the MIT license.
Please see the file 'LICENCE.txt' in the root directory
or http://opensource.org/licenses/mit-license.php for information.
*/

#include <iostream>
#include <iomanip>
#include <vector>
#include <algorithm>
#include <set>
#include <boost/lexical_cast.hpp>
#include "interaction.h"
#include "memory.h"
#include "system.h"
#include "error.h"
#include "symmetry.h"
#include "combination.h"
#include "constants.h"
#include "listcomparison.h"
#include "files.h"
#include "timer.h"
#include "fcs.h"
#include <cmath>

using namespace ALM_NS;

Interaction::Interaction(ALM *alm) : Pointers(alm) {
    nsize[0] = nsize[1] = nsize[2] = 1;
}

Interaction::~Interaction()
{
    memory->deallocate(x_image);
    memory->deallocate(exist_image);
    memory->deallocate(str_order);
    memory->deallocate(nbody_include);
    memory->deallocate(pairs);
    memory->deallocate(mindist_pairs);
    memory->deallocate(interaction_pair);
    memory->deallocate(mindist_cluster);
    memory->deallocate(distall);
}

void Interaction::init()
{
    int i, j, k;
    int nat = system->nat;
    int nkd = system->nkd;

    std::cout << " INTERACTION" << std::endl;
    std::cout << " ===========" << std::endl << std::endl;

    memory->allocate(str_order, maxorder);
    set_ordername();

    std::cout << "  +++ Cutoff Radii Matrix in Bohr Unit (NKD x NKD matrix) +++" << std::endl;

    for (i = 0; i < maxorder; ++i) {
        std::cout << "  " <<  std::setw(9) << str_order[i] << std::endl;
        for (j = 0; j < nkd; ++j) {
            for (k = 0; k < nkd; ++k) {
                if (rcs[i][j][k] < 0.0) {
                    std::cout << std::setw(9) << "None";
                } else {
                    std::cout << std::setw(9) << rcs[i][j][k];
                }
            }
            std::cout << std::endl;
        }
        std::cout << std::endl;
    }

    nneib = (2 * nsize[0] + 1) * (2 * nsize[1] + 1) * (2 * nsize[2] + 1);
    memory->allocate(x_image, nneib, nat, 3);
    memory->allocate(exist_image, nneib);
    memory->allocate(distall, nat, nat);
    memory->allocate(mindist_pairs, nat, nat);
    memory->allocate(interaction_pair, maxorder, symmetry->natmin);
    memory->allocate(mindist_cluster, maxorder, symmetry->natmin);
    memory->allocate(pairs, maxorder);

    generate_coordinate_of_periodic_images(nat, system->xcoord, is_periodic, x_image, exist_image);
    get_pairs_of_minimum_distance(nat, x_image, exist_image, distall, mindist_pairs);
    print_neighborlist(mindist_pairs);
    search_interactions(interaction_pair, pairs);
    //    calc_mindist_clusters2(interaction_pair, mindist_pairs, distall, exist_image, mindist_cluster);
    calc_mindist_clusters(interaction_pair, mindist_pairs, distall, exist_image, mindist_cluster);
    generate_pairs(pairs, mindist_cluster);

    /*
    for (int order = 0; order < maxorder; ++order) {
    for (i = 0; i < symmetry->natmin; ++i) {
    for (std::set<MinimumDistanceCluster>::const_iterator it = mindist_cluster[order][i].begin();
    it != mindist_cluster[order][i].end(); ++it) {
    std::cout << " order = " << std::setw(5) << order;
    std::cout << " iat = " << std::setw(5) << i << " : ";
    for (j = 0; j < (*it).atom.size(); ++j) {
    std::cout << std::setw(5) << (*it).atom[j];
    }
    std::cout << std::endl;
    for (j = 0; j < (*it).cell.size(); ++j) {
    for (k = 0; k < (*it).cell[j].size(); ++k) {
    std::cout << std::setw(5) << (*it).cell[j][k];
    }
    std::cout << std::endl;
    }
    std::cout << std::endl;
    }
    }
    }

    for (int order = 0; order < maxorder; ++order) {
    std::cout << "order = " << order << " pairs_size = " << pairs[order].size() << std::endl;
    for (std::set<IntList>::const_iterator it = pairs[order].begin(); it != pairs[order].end(); ++it) {
    for (i = 0; i < (*it).iarray.size(); ++i) {
    std::cout << std::setw(5) << (*it).iarray[i];
    }
    std::cout << std::endl;
    }
    }
    */

    timer->print_elapsed();
    std::cout << " --------------------------------------------------------------" << std::endl;
    std::cout << std::endl;
}

void Interaction::generate_pairs(std::set<IntList> *pair_out, std::set<MinimumDistanceCluster> **mindist_cluster)
{
    int i, j;
    int iat;
    int order;
    int natmin = symmetry->natmin;
    int nat = system->nat;

    int *pair_tmp;

    for (order = 0; order < maxorder; ++order) {

        pair_out[order].clear();

        if (order + 2 > nbody_include[order]) {
            std::cout << "  For " << std::setw(8) << interaction->str_order[order] << ", ";
            std::cout << "interactions related to more than" << std::setw(2) << nbody_include[order];
            std::cout << " atoms will be neglected." << std::endl;
        }

        memory->allocate(pair_tmp, order + 2);

        for (i = 0; i < natmin; ++i) {

            iat = symmetry->map_p2s[i][0];

            for (std::set<MinimumDistanceCluster>::const_iterator it = mindist_cluster[order][i].begin();
                it != mindist_cluster[order][i].end(); ++it) {

                    pair_tmp[0] = iat;
                    for (j = 0; j < order + 1; ++j) {
                        pair_tmp[j+1] = (*it).atom[j];
                    }

                    insort(order+2, pair_tmp);

                    // Ignore many-body case
                    if (nbody(order + 2, pair_tmp) > nbody_include[order]) continue;
                    pair_out[order].insert(IntList(order + 2, pair_tmp));
            }
        }
        memory->deallocate(pair_tmp);
    }
}

void Interaction::generate_coordinate_of_periodic_images(const unsigned int nat, double **xf_in,
                                                         const int periodic_flag[3], double ***xc_out,
                                                         int *is_allowed)
{
    //
    // Generate Cartesian coordinates of atoms in the neighboring 27 supercells
    //

    unsigned int i, j;
    int ia, ja, ka;
    int icell;

    icell = 0;
    for (i = 0; i < nat; ++i) {
        for (j = 0; j < 3; ++j) {
            xc_out[0][i][j] = xf_in[i][j];
        }
    }
    // Convert to Cartesian coordinate
    system->frac2cart(xc_out[0]);

    for (ia = -1; ia <= 1; ++ia) {
        for (ja = -1; ja <= 1; ++ja) {
            for (ka = -1; ka <= 1; ++ka) {

                if (ia == 0 && ja == 0 && ka == 0) continue;

                ++icell;
                for (i = 0; i < nat; ++i) {
                    xc_out[icell][i][0] = xf_in[i][0] + static_cast<double>(ia);
                    xc_out[icell][i][1] = xf_in[i][1] + static_cast<double>(ja);
                    xc_out[icell][i][2] = xf_in[i][2] + static_cast<double>(ka);
                }
                // Convert to Cartesian coordinate
                system->frac2cart(xc_out[icell]);
            }
        }
    }

    icell = 0;
    is_allowed[0] = 1;

    for (ia = -1; ia <= 1; ++ia) {
        for (ja = -1; ja <= 1; ++ja) {
            for (ka = -1; ka <= 1; ++ka) {

                ++icell;

                // When periodic flag is zero along an axis,
                // periodic images along that axis cannot be considered.
                if (((std::abs(ia) == 1) && (periodic_flag[0] == 0)) ||
                    ((std::abs(ja) == 1) && (periodic_flag[1] == 0)) ||
                    ((std::abs(ka) == 1) && (periodic_flag[2] == 0)) ) {

                        is_allowed[icell] = 0;

                } else {

                    is_allowed[icell] = 1;
                }
            }
        }
    }

}

double Interaction::distance(double *x1, double *x2)
{
    double dist;
    dist = std::pow(x1[0] - x2[0], 2) + std::pow(x1[1] - x2[1], 2) + std::pow(x1[2] - x2[2], 2);
    dist = std::sqrt(dist);

    return dist;
}

void Interaction::get_pairs_of_minimum_distance(int nat, double ***xc_in, int *exist, std::vector<DistInfo> **distall,
                                                std::vector<DistInfo> **mindist_pairs)
{
    int icell = 0;
    int i, j, k;
    double dist_tmp;
    double vec[3];

    for (i = 0; i < nat; ++i) {
        for (j = 0; j < nat; ++j) {

            distall[i][j].clear();

            for (icell = 0; icell < nneib; ++icell) {

                if (exist[icell]) {

                    dist_tmp = distance(xc_in[0][i], xc_in[icell][j]);

                    for (k = 0; k < 3; ++k) vec[k] = xc_in[icell][j][k] - xc_in[0][i][k];

                    distall[i][j].push_back(DistInfo(icell, dist_tmp, vec));
                }
            }
            std::sort(distall[i][j].begin(), distall[i][j].end());
        }
    }

    // Construct pairs of minimum distance.

    double dist_min;
    for (i = 0; i < nat; ++i) {
        for (j = 0; j < nat; ++j) {
            mindist_pairs[i][j].clear();

            dist_min = distall[i][j][0].dist;
            for (std::vector<DistInfo>::const_iterator it = distall[i][j].begin(); it != distall[i][j].end(); ++it) {
                if (std::abs((*it).dist - dist_min) < eps8) {
                    mindist_pairs[i][j].push_back(DistInfo(*it));
                }
            }
        }

        /*
        for (j = 0; j < nat; ++j) {
        std::cout << "Mindist : " << std::setw(5) << i + 1 << " <--> " << std::setw(5) << j + 1 << std::endl;
        for (k = 0; k < mindist_pairs[i][j].size(); ++k) {
        std::cout << " cell_j = " << std::setw(5) << mindist_pairs[i][j][k].cell;
        std::cout << " dist = " << std::setw(5) << mindist_pairs[i][j][k].dist;
        std::cout << std::endl;
        }
        std::cout << std::endl;
        }
        */
    }
}

void Interaction::print_neighborlist(std::vector<DistInfo> **mindist)
{
    int i, j, k;
    int iat;
    int nat = system->nat;
    int icount;

    double dist_tmp;
    std::vector<DistList> *neighborlist;

    memory->allocate(neighborlist, symmetry->natmin);

    for (i = 0; i < symmetry->natmin; ++i) {
        neighborlist[i].clear();

        iat = symmetry->map_p2s[i][0];

        for (j = 0; j < nat; ++j) {
            neighborlist[i].push_back(DistList(j, mindist[iat][j][0].dist));
        }
        std::sort(neighborlist[i].begin(), neighborlist[i].end());
    }

    std::cout << std::endl;
    std::cout << "  List of neighboring atoms below." << std::endl;
    std::cout << "  Format [N th-nearest shell, distance in Bohr (Number of atoms on the shell)]" << std::endl << std::endl;

    int nthnearest;
    std::vector<int> atomlist;

    for (i = 0; i < symmetry->natmin; ++i) {

        nthnearest = 0;
        atomlist.clear();

        iat = symmetry->map_p2s[i][0];
        std::cout << std::setw(5) << iat + 1 << " (" << std::setw(3) << system->kdname[system->kd[iat] - 1] << "): ";

        dist_tmp = 0.0;

        for (j = 0; j < nat; ++j) {

            if (neighborlist[i][j].dist < eps8) continue; // distance is zero

            if (std::abs(neighborlist[i][j].dist - dist_tmp) > eps6) {

                if (atomlist.size() > 0) {
                    nthnearest += 1;

                    if (nthnearest > 1) std::cout << std::setw(13) << " ";

                    std::cout << std::setw(3) << nthnearest << std::setw(10) << dist_tmp
                        << " (" << std::setw(3) << atomlist.size() << ") -";

                    icount = 0;
                    for (k = 0; k < atomlist.size(); ++k) {

                        if (icount % 4 == 0 && icount > 0) {
                            std::cout << std::endl;
                            std::cout << std::setw(34) << " ";
                        }
                        ++icount;

                        std::cout << std::setw(4) << atomlist[k] + 1;
                        std::cout <<  "(" << std::setw(3) << system->kdname[system->kd[atomlist[k]] - 1] << ")";

                    }
                    std::cout << std::endl;
                }

                dist_tmp = neighborlist[i][j].dist;
                atomlist.clear();
                atomlist.push_back(neighborlist[i][j].atom);
            } else {
                atomlist.push_back(neighborlist[i][j].atom);
            }
        }
        if (atomlist.size() > 0) {
            nthnearest += 1;

            if (nthnearest > 1) std::cout << std::setw(13) << " ";

            std::cout << std::setw(3) << nthnearest << std::setw(10) << dist_tmp
                << " (" << std::setw(3) << atomlist.size() << ") -";

            icount = 0;
            for (k = 0; k < atomlist.size(); ++k) {

                if (icount % 4 == 0 && icount > 0) {
                    std::cout << std::endl;
                    std::cout << std::setw(34) << " ";
                }
                ++icount;

                std::cout << std::setw(4) << atomlist[k] + 1;
                std::cout <<  "(" << std::setw(3) << system->kdname[system->kd[atomlist[k]] - 1] << ")";

            }
            std::cout << std::endl;
        }
        std::cout << std::endl;
    }
    atomlist.clear();
    memory->deallocate(neighborlist);
}

void Interaction::search_interactions(std::vector<int> **interaction_list_out, std::set<IntList> *pair_out)
{
    int i;
    int order;
    int natmin = symmetry->natmin;
    int iat, jat;
    int nat = system->nat;
    int ikd, jkd;

    double cutoff_tmp;
    std::set<IntList> *interacting_atom_pairs;
    std::vector<int> intlist;

    memory->allocate(interacting_atom_pairs, maxorder);

    for (order = 0; order < maxorder; ++order) {
        for (i = 0; i < natmin; ++i) {
            interaction_list_out[order][i].clear();

            iat = symmetry->map_p2s[i][0];
            ikd = system->kd[iat] - 1;

            for (jat = 0; jat < nat; ++jat) {

                jkd = system->kd[jat] - 1;

                cutoff_tmp = rcs[order][ikd][jkd];

                if (cutoff_tmp < 0.0) {

                    interaction_list_out[order][i].push_back(jat);

                } else {

                    if (mindist_pairs[iat][jat][0].dist <= cutoff_tmp) {
                        interaction_list_out[order][i].push_back(jat);
                    }
                }
            }
        }
    }

    intlist.clear();
    std::cout << std::endl;
    std::cout << "  List of interacting atom pairs considered for each order:" << std::endl;

    for (order = 0; order < maxorder; ++order) {

        interacting_atom_pairs[order].clear();

        std::cout << std::endl << "   ***" << str_order[order] << "***" << std::endl;

        for (i = 0; i < natmin; ++i) {

            if (interaction_list_out[order][i].size() == 0) {
                std::cout << "   No interacting atoms! Skipped." << std::endl;
                continue; // no interaction
            }

            iat = symmetry->map_p2s[i][0];

            intlist.clear();
            for (std::vector<int>::const_iterator it = interaction_list_out[order][i].begin();
                it != interaction_list_out[order][i].end(); ++it) {
                    intlist.push_back((*it));
            }
            std::sort(intlist.begin(), intlist.end()); // Necessarily to sort here

            // write atoms inside the cutoff radius
            int id = 0;
            std::cout << "    Atom " << std::setw(5) << iat + 1
                << "(" << std::setw(3) << system->kdname[system->kd[iat]-1] << ")" << " interacts with atoms ... " << std::endl;

            for (int id = 0; id < intlist.size(); ++id) {
                if (id%6 == 0) {
                    if (id == 0) {
                        std::cout << "   ";
                    } else {
                        std::cout << std::endl;
                        std::cout << "   ";
                    }
                }
                std::cout << std::setw(5) << intlist[id] + 1 << "("
                    << std::setw(3) << system->kdname[system->kd[intlist[id]]-1] << ")";
            }

            std::cout << std::endl << std::endl;
            std::cout << "    Number of total interaction pairs = "
                << interaction_list_out[order][i].size() << std::endl << std::endl;

            int *intarr;
            memory->allocate(intarr, order + 2);

            if (intlist.size() > 0) {
                if (order == 0) {
                    for (unsigned int ielem = 0; ielem < intlist.size(); ++ielem) {
                        intarr[0] = iat;
                        intarr[1] = intlist[ielem];
                        insort(2, intarr);

                        interacting_atom_pairs[0].insert(IntList(2, intarr));
                    }
                } else if (order > 0) {
                    CombinationWithRepetition<int> g(intlist.begin(), intlist.end(), order + 1);
                    do {
                        std::vector<int> data = g.now();
                        intarr[0] = iat;
                        intarr[1] = data[0];
                        for (unsigned int isize = 1; isize < data.size() ; ++isize) {
                            intarr[isize + 1] = data[isize];
                        }
                        if (!is_incutoff(order+2, intarr, order)) continue;

                        insort(order+2, intarr);
                        interacting_atom_pairs[order].insert(IntList(order + 2, intarr));

                    } while(g.next());
                }
            }
            intlist.clear();
            memory->deallocate(intarr);
        }
    }

    std::cout << std::endl;

    /*
    int *pair_tmp;

    for (order = 0; order < maxorder; ++order) {

    pair_out[order].clear();

    if (order + 2 > nbody_include[order]) {
    std::cout << "  For " << std::setw(8) << interaction->str_order[order] << ", ";
    std::cout << "interactions related to more than" << std::setw(2) << nbody_include[order];
    std::cout << " atoms will be neglected." << std::endl;
    }

    memory->allocate(pair_tmp, order + 2);

    for (std::set<IntList>::const_iterator it = interacting_atom_pairs[order].begin();
    it != interacting_atom_pairs[order].end(); ++it) {
    for (j = 0; j < order + 2; ++j) {
    pair_tmp[j] = (*it).iarray[j];
    }

    // Ignore many-body case
    if (nbody(order + 2, pair_tmp) > nbody_include[order]) continue;

    pair_out[order].insert(IntList(order + 2, pair_tmp));
    }
    memory->deallocate(pair_tmp);
    }
    */

    memory->deallocate(interacting_atom_pairs);
}

bool Interaction::is_incutoff(const int n, int *atomnumlist, const int order)
{
    int i, j;
    int iat, jat;
    int ikd, jkd;
    int ncheck = n - 1;
    double cutoff_tmp;
    std::vector<DistInfo>::const_iterator it, it2;

    iat = atomnumlist[0];
    ikd = system->kd[iat] - 1;

    for (i = 0; i < n; ++i) {
        iat = atomnumlist[i];
        ikd = system->kd[iat] - 1;

        for (j = i + 1; j < n; ++j) {
            jat = atomnumlist[j];
            jkd = system->kd[jat] - 1;

            cutoff_tmp = rcs[order][ikd][jkd];

            if (cutoff_tmp >= 0.0 &&
                (mindist_pairs[iat][jat][0].dist > cutoff_tmp)) return false;

        }
    }
    return true;

    /*
    for (i = 0; i < ncheck; ++i) {

    jat = atomnumlist[i + 1];
    jkd = system->kd[jat] - 1;

    if (rcs[order][ikd][jkd] >= 0.0 &&
    (mindist_pairs[iat][jat][0].dist > rcs[order][ikd][jkd])) return false;

    for (j = i + 1; j < ncheck; ++j) {

    kat = atomnumlist[j + 1];
    kkd = system->kd[kat] - 1;

    if (rcs[order][ikd][kkd] >= 0.0 &&
    (mindist_pairs[iat][kat][0].dist > rcs[order][ikd][kkd])) return false;

    cutoff_tmp = rcs[order][jkd][kkd];

    if (cutoff_tmp >= 0.0) {

    in_cutoff_tmp = false;

    for (it = mindist_pairs[iat][jat].begin(); it != mindist_pairs[iat][jat].end(); ++it) {
    for (it2 = mindist_pairs[iat][kat].begin(); it2 != mindist_pairs[iat][kat].end(); ++it2) {
    dist_tmp = distance(x_image[(*it).cell][jat], x_image[(*it2).cell][kat]);

    if (n == 4) {
    if (iat == 0 && jat == 4 && kat == 20) {
    std::cout << "DEBUG :" << std::setw(5) << (*it).cell;
    std::cout << std::setw(5) << (*it2).cell << std::setw(15) << dist_tmp << std::endl;
    }
    }

    if (dist_tmp <= cutoff_tmp) {
    in_cutoff_tmp = true;
    }
    }
    }
    if (!in_cutoff_tmp) return false;
    }
    }
    }
    return true;
    */

}

bool Interaction::is_incutoff2(const int n, int *atomnumlist, const int order)
{
    int i, j;
    int iat, jat, kat;
    int ikd, jkd, kkd;
    int ncheck = n - 1;
    //    int order = n - 2;
    bool in_cutoff_tmp;
    double cutoff_tmp;
    double dist_tmp;
    std::vector<DistInfo>::const_iterator it, it2;

    iat = atomnumlist[0];
    ikd = system->kd[iat] - 1;

    for (i = 0; i < ncheck; ++i) {

        jat = atomnumlist[i + 1];
        jkd = system->kd[jat] - 1;

        if (rcs[order][ikd][jkd] >= 0.0 &&
            (mindist_pairs[iat][jat][0].dist > rcs[order][ikd][jkd])) return false;

        for (j = i + 1; j < ncheck; ++j) {

            kat = atomnumlist[j + 1];
            kkd = system->kd[kat] - 1;

            if (rcs[order][ikd][kkd] >= 0.0 &&
                (mindist_pairs[iat][kat][0].dist > rcs[order][ikd][kkd])) return false;

            cutoff_tmp = rcs[order][jkd][kkd];

            if (cutoff_tmp >= 0.0) {

                in_cutoff_tmp = false;

                for (it = mindist_pairs[iat][jat].begin(); it != mindist_pairs[iat][jat].end(); ++it) {
                    for (it2 = mindist_pairs[iat][kat].begin(); it2 != mindist_pairs[iat][kat].end(); ++it2) {
                        dist_tmp = distance(x_image[(*it).cell][jat], x_image[(*it2).cell][kat]);

                        if (dist_tmp <= cutoff_tmp) {
                            in_cutoff_tmp = true;
                        }
                    }
                }
                if (!in_cutoff_tmp) return false;
            }
        }
    }

    return true;
}

void Interaction::set_ordername()
{
    std::string strnum;

    str_order[0] = "HARMONIC";

    for (int i = 1;  i < maxorder; ++i) {
        strnum = boost::lexical_cast<std::string>(i+2);
        str_order[i] = "ANHARM" + strnum;
    }
}

int Interaction::nbody(const int n, const int *arr)
{
    std::vector<int> v;
    v.clear();
    int ret;

    for (unsigned int i = 0; i < n; ++i) {
        v.push_back(arr[i]);
    }
    std::sort(v.begin(), v.end());
    v.erase(std::unique(v.begin(), v.end()), v.end());

    ret = v.size();
    v.clear();

    return ret;
}

void Interaction::calc_mindist_clusters(std::vector<int> **interaction_pair_in, std::vector<DistInfo> **mindist_pair_in,
                                        std::vector<DistInfo> **distance_image, int *exist,
                                        std::set<MinimumDistanceCluster> **mindist_cluster_out)
{
    std::vector<MinDistList> distance_list;

    int natmin = symmetry->natmin;
    int i, j, k;
    int iat, jat;
    int order;
    int ii;
    int ikd, jkd;
    //    int *intpair_tmp;
    std::vector<int> intlist;

    double dist_tmp, rc_tmp;
    double distmax;

    bool isok;

    std::vector<int> cell_vector;
    std::vector<double> dist_vector;
    std::vector<std::vector<int> > pairs_icell, comb_cell, comb_cell_min;
    std::vector<std::vector<int> > comb_cell_atom_center;
    std::vector<int> accum_tmp;
    std::vector<int> atom_tmp, cell_tmp;
    std::vector<int> intpair_uniq, cellpair;
    std::vector<int> group_atom;

    int icount;

    //    memory->allocate(intpair_tmp, maxorder + 2);

    for (order = 0; order < maxorder; ++order) {
        for (i = 0; i < symmetry->natmin; ++i) {

            mindist_cluster_out[order][i].clear();

            iat = symmetry->map_p2s[i][0];
            ikd = system->kd[iat] - 1;

            // List of 2-body interaction pairs
            intlist.clear();
            for (std::vector<int>::const_iterator it  = interaction_pair_in[order][i].begin();
                it != interaction_pair_in[order][i].end(); ++it) {
                    intlist.push_back((*it));
            }
            std::sort(intlist.begin(), intlist.end()); // Need to sort here

            if (intlist.size() > 0) {
                if (order == 0){
                    for (unsigned int ielem = 0; ielem < intlist.size(); ++ielem) {

                        comb_cell_min.clear();
                        atom_tmp.clear();

                        jat = intlist[ielem];
                        atom_tmp.push_back(jat);

                        for (j = 0; j < mindist_pair_in[iat][jat].size(); ++j) {
                            cell_tmp.clear();
                            cell_tmp.push_back(mindist_pair_in[iat][jat][j].cell);
                            comb_cell_min.push_back(cell_tmp);
                        }
                        distmax = mindist_pair_in[iat][jat][0].dist;
                        mindist_cluster_out[order][i].insert(MinimumDistanceCluster(atom_tmp, comb_cell_min, distmax));
                    }
                } else if (order > 0) {
                    CombinationWithRepetition<int> g(intlist.begin(), intlist.end(), order + 1);
                    do {
                        std::vector<int> data = g.now();

                        // Uniq the list of atoms in data like as follows:
                        // cubic   term : (i, i) --> (i) x 2
                        // quartic term : (i, i, j) --> (i, j) x (2, 1)
                        intpair_uniq.clear();
                        group_atom.clear();
                        icount = 1;

                        for (j = 0; j < order; ++j) {
                            if (data[j] == data[j+1]) {
                                ++icount;
                            } else {
                                group_atom.push_back(icount);
                                intpair_uniq.push_back(data[j]);
                                icount = 1;
                            }
                        }
                        group_atom.push_back(icount);
                        intpair_uniq.push_back(data[order]);

                        pairs_icell.clear();
                        for (j = 0; j < intpair_uniq.size(); ++j) {
                            jat = intpair_uniq[j];
                            jkd = system->kd[jat] - 1;

                            rc_tmp = rcs[order][ikd][jkd];
                            cell_vector.clear();

                            // Loop over the cell images of atom 'jat' and add to the list
                            // as a candidate for the minimum distance cluster
                            for (std::vector<DistInfo>::const_iterator it = distance_image[iat][jat].begin();
                                it != distance_image[iat][jat].end(); ++it) {
                                    if (exist[(*it).cell]) {
                                        if (rc_tmp < 0.0 || (*it).dist <= rc_tmp) {
                                            cell_vector.push_back((*it).cell);
                                        }
                                    }
                            }
                            pairs_icell.push_back(cell_vector);
                        }

                        accum_tmp.clear();
                        comb_cell.clear();
                        cell_combination(pairs_icell, 0, accum_tmp, comb_cell);

                        distance_list.clear();
                        for (j = 0; j < comb_cell.size(); ++j) {

                            cellpair.clear();

                            for (k = 0; k < group_atom.size(); ++k) {
                                for (ii = 0; ii < group_atom[k]; ++ii) {
                                    cellpair.push_back(comb_cell[j][k]);
                                }
                            }

                            dist_vector.clear();

                            for (k = 0; k < cellpair.size(); ++k) {
                                dist_tmp = distance(x_image[cellpair[k]][data[k]], x_image[0][iat]);
                                //   if (dist_tmp > eps15) dist_vector.push_back(dist_tmp);
                                dist_vector.push_back(dist_tmp);
                            }

                            // Flag to check if the distance is smaller than the cutoff radius
                            isok = true;

                            for (k = 0; k < cellpair.size(); ++k) {
                                for (ii = k + 1; ii < cellpair.size(); ++ii) {
                                    dist_tmp = distance(x_image[cellpair[k]][data[k]], x_image[cellpair[ii]][data[ii]]);
                                    rc_tmp = rcs[order][system->kd[data[k]]-1][system->kd[data[ii]]-1];
                                    if (rc_tmp >= 0.0 && dist_tmp > rc_tmp) {
                                        isok = false;
                                    }
                                    //                                    if (dist_tmp > eps15) dist_vector.push_back(dist_tmp);
                                    dist_vector.push_back(dist_tmp);
                                }
                            }
                            if (isok) {
                                // This combination is a candidate of the minimum distance cluster
                                distance_list.push_back(MinDistList(cellpair, dist_vector));
                            }
                        } // close loop over the mirror image combination

                        if (distance_list.size() > 0) {
                            // If the distance_list is not empty, there is a set of mirror images
                            // that satisfies the condition of the interaction.

                            pairs_icell.clear();
                            for (j = 0; j < intpair_uniq.size(); ++j) {
                                jat = intpair_uniq[j];
                                cell_vector.clear();

                                for (ii = 0; ii < mindist_pair_in[iat][jat].size(); ++ii) {
                                    cell_vector.push_back(mindist_pair_in[iat][jat][ii].cell);
                                }
                                pairs_icell.push_back(cell_vector);
                            }

                            accum_tmp.clear();
                            comb_cell.clear();
                            comb_cell_atom_center.clear();
                            cell_combination(pairs_icell, 0, accum_tmp, comb_cell);

                            for (j = 0; j < comb_cell.size(); ++j) {
                                cellpair.clear();
                                for (k = 0; k < group_atom.size(); ++k) {
                                    for (ii = 0; ii < group_atom[k]; ++ii) {
                                        cellpair.push_back(comb_cell[j][k]);
                                    }
                                }
                                comb_cell_atom_center.push_back(cellpair);
                            }

                            std::sort(distance_list.begin(), distance_list.end(), MinDistList::compare_max_distance);
                            distmax = *std::max_element(distance_list[0].dist.begin(), distance_list[0].dist.end());
                            mindist_cluster_out[order][i].insert(MinimumDistanceCluster(data, comb_cell_atom_center,distmax));
                            /*
                            std::sort(distance_list.begin(), distance_list.end(), MinDistList::compare_sum_distance);
                            comb_cell_min.clear();

                            sum_dist_min = 0.0;
                            for (j = 0; j < distance_list[0].dist.size(); ++j) {
                            sum_dist_min += distance_list[0].dist[j];
                            }

                            for (j = 0; j < distance_list.size(); ++j) {
                            sum_dist = 0.0;

                            for (k = 0; k < distance_list[j].dist.size(); ++k) {
                            sum_dist += distance_list[j].dist[k];
                            }

                            // In the following, only pairs having minimum sum of distances
                            // are stored. However, we found that this treatment didn't
                            // return a reliable value of phonon linewidth.
                            if (std::abs(sum_dist - sum_dist_min) < eps6) {
                            comb_cell_min.push_back(distance_list[j].cell);
                            } else {
                            break;
                            }
                            // Therefore, we consider all duplicate pairs
                            //comb_cell_min.push_back(distance_list[j].cell);
                            }
                            //                            mindist_cluster_out[order][i].insert(MinimumDistanceCluster(data, comb_cell_min));
                            */

                        }

                    } while(g.next());
                }
            }
            intlist.clear();
        }
    }

    //    memory->deallocate(intpair_tmp);
}

void Interaction::calc_mindist_clusters2(std::vector<int> **interaction_pair_in, std::vector<DistInfo> **mindist_pair_in,
                                         std::vector<DistInfo> **distance_image, int *exist,
                                         std::set<MinimumDistanceCluster> **mindist_cluster_out)
{
    std::vector<MinDistList> distance_list;

    int natmin = symmetry->natmin;
    int i, j, k;
    int iat, jat;
    int ikd, jkd;
    int order;
    int ii;
    std::vector<int> intlist;

    double rc_tmp;
    double dist_tmp;
    double sum_dist_min, sum_dist;
    double dist_max;

    std::vector<int> cell_vector;
    std::vector<double> dist_vector;
    std::vector<std::vector<int> > pairs_icell, comb_cell, comb_cell_min;
    std::vector<int> accum_tmp;
    std::vector<int> atom_tmp, cell_tmp;
    std::vector<int> intpair_uniq, cellpair;
    std::vector<int> group_atom;

    int icount;
    bool isok;

    for (order = 0; order < maxorder; ++order) {
        for (i = 0; i < symmetry->natmin; ++i) {

            mindist_cluster_out[order][i].clear();
            iat = symmetry->map_p2s[i][0];
            ikd = system->kd[iat] - 1;

            intlist.clear();
            for (std::vector<int>::const_iterator it  = interaction_pair_in[order][i].begin();
                it != interaction_pair_in[order][i].end(); ++it) {
                    intlist.push_back((*it));
            }
            std::sort(intlist.begin(), intlist.end()); // Need to sort here

            if (intlist.size() > 0) {

                if (order == 0){
                    // For harmonic case, the minimum distance cluster is equivalen to the
                    // minimum distance pairs.
                    for (unsigned int ielem = 0; ielem < intlist.size(); ++ielem) {

                        comb_cell_min.clear();
                        atom_tmp.clear();

                        jat = intlist[ielem];
                        atom_tmp.push_back(jat);

                        for (j = 0; j < mindist_pair_in[iat][jat].size(); ++j) {
                            cell_tmp.clear();
                            cell_tmp.push_back(mindist_pair_in[iat][jat][j].cell);
                            comb_cell_min.push_back(cell_tmp);
                        }
                        dist_max = mindist_pair_in[iat][jat][0].dist;
                        mindist_cluster_out[order][i].insert(MinimumDistanceCluster(atom_tmp, comb_cell_min));
                    }

                } else if (order > 0) {

                    // For anharmonic cases, the minimum distance cluster is the set of
                    // cell images having the smallest value for the sum of distance.

                    // Generate all possible combination of atoms from intlist
                    CombinationWithRepetition<int> g(intlist.begin(), intlist.end(), order + 1);
                    do {
                        std::vector<int> data = g.now();

                        // Uniq the list of atoms in data like as follows:
                        // cubic   term : (i, i) --> (i) x 2
                        // quartic term : (i, i, j) --> (i, j) x (2, 1)
                        intpair_uniq.clear();
                        group_atom.clear();
                        icount = 1;

                        for (j = 0; j < order; ++j) {
                            if (data[j] == data[j+1]) {
                                ++icount;
                            } else {
                                group_atom.push_back(icount);
                                intpair_uniq.push_back(data[j]);
                                icount = 1;
                            }
                        }
                        group_atom.push_back(icount);
                        intpair_uniq.push_back(data[order]);

                        pairs_icell.clear();
                        for (j = 0; j < intpair_uniq.size(); ++j) {
                            jat = intpair_uniq[j];
                            jkd = system->kd[jat] - 1;

                            rc_tmp = rcs[order][ikd][jkd];
                            cell_vector.clear();

                            // Loop over the cell images of atom 'jat' and add to the list
                            // as a candidate for the minimum distance cluster
                            for (std::vector<DistInfo>::const_iterator it = distance_image[iat][jat].begin();
                                it != distance_image[iat][jat].end(); ++it) {
                                    if (exist[(*it).cell]) {
                                        if (rc_tmp < 0.0 || (*it).dist <= rc_tmp) {
                                            cell_vector.push_back((*it).cell);
                                            // std::cout << " iat = " << std::setw(5) << iat;
                                            // std::cout << " jat = " << std::setw(5) << jat;
                                            // std::cout << " cell = " << (*it).cell;
                                            // std::cout << " dist = " << (*it).dist;
                                            // std::cout << " rc = " << rc_tmp << std::endl;
                                        }
                                    }
                            }
                            pairs_icell.push_back(cell_vector);
                        }

                        // Generate all possible combinations of mirror images
                        accum_tmp.clear();
                        comb_cell.clear();
                        cell_combination(pairs_icell, 0, accum_tmp, comb_cell);

                        distance_list.clear();

                        // Loop over the combination of the mirror images and
                        // generate the list of atomic distances of all pairs for each combination.
                        // Add to list only if the distance is smaller than the cutoff radius.

                        for (j = 0; j < comb_cell.size(); ++j) {

                            cellpair.clear();

                            for (k = 0; k < group_atom.size(); ++k) {
                                for (ii = 0; ii < group_atom[k]; ++ii) {
                                    cellpair.push_back(comb_cell[j][k]);
                                }
                            }

                            dist_vector.clear();

                            for (k = 0; k < cellpair.size(); ++k) {
                                dist_tmp = distance(x_image[cellpair[k]][data[k]], x_image[0][iat]);
                                if (dist_tmp > eps15) dist_vector.push_back(dist_tmp);
                            }

                            // Flag to check if the distance is smaller than the cutoff radius
                            isok = true;

                            for (k = 0; k < cellpair.size(); ++k) {
                                for (ii = k + 1; ii < cellpair.size(); ++ii) {
                                    dist_tmp = distance(x_image[cellpair[k]][data[k]], x_image[cellpair[ii]][data[ii]]);
                                    rc_tmp = rcs[order][system->kd[data[k]]-1][system->kd[data[ii]]-1];
                                    if (rc_tmp >= 0.0 && dist_tmp > rc_tmp) {
                                        isok = false;
                                        //                                            break;
                                    }
                                    if (dist_tmp > eps15) dist_vector.push_back(dist_tmp);
                                }
                            }
                            if (isok) {
                                // This combination a candidate for the minimum distance cluster
                                distance_list.push_back(MinDistList(cellpair, dist_vector));
                            }
                        }

                        std::sort(distance_list.begin(), distance_list.end(), MinDistList::compare_sum_distance);

                        comb_cell_min.clear();

                        sum_dist_min = 0.0;
                        dist_max = 0.0;
                        for (j = 0; j < distance_list[0].dist.size(); ++j) {
                            sum_dist_min += distance_list[0].dist[j];
                            dist_max = std::max<double>(dist_max, distance_list[0].dist[j]);
                        }

                        for (j = 0; j < distance_list.size(); ++j) {
                            sum_dist = 0.0;

                            for (k = 0; k < distance_list[j].dist.size(); ++k) {
                                sum_dist += distance_list[j].dist[k];
                            }

                            // In the following, only pairs having minimum sum of distances
                            // are stored.
                            if (std::abs(sum_dist - sum_dist_min) < eps6) {
                                comb_cell_min.push_back(distance_list[j].cell);
                            } else {
                                break;
                            }
                        }

                        if (comb_cell_min.size() > 0) {
                            mindist_cluster_out[order][i].insert(MinimumDistanceCluster(data, comb_cell_min));
                        }

                    } while(g.next());
                }
            }
            intlist.clear();
        }
    }
}

void Interaction::cell_combination(std::vector<std::vector<int> > array, int i,
                                   std::vector<int> accum, std::vector<std::vector<int> > &comb)
{
    if (i == array.size())  {
        comb.push_back(accum);
    } else  {
        std::vector<int> row = array[i];
        for (int j = 0; j < row.size(); ++j) {
            std::vector<int> tmp(accum);
            tmp.push_back(row[j]);
            cell_combination(array,i+1,tmp, comb);
        }
    }
}


// Copyright 2015-2022 The ALMA Project Developers
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//   http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
// implied. See the License for the specific language governing
// permissions and limitations under the License.

/// @file
/// Definitions corresponding to vc.hpp.

#include <Eigen/Dense>
#include <exceptions.hpp>
#include <periodic_table.hpp>
#include <vc.hpp>

namespace alma {
std::unique_ptr<Crystal_structure> vc_mix_structures(
    const std::vector<Crystal_structure>& components,
    const std::vector<double>& ratios) {
    auto ncomponents = components.size();

    // Perform some sanity checks. Note that even passing all of
    // those does not guarantee that the resulting virtual crystal
    // will make sense.
    if (ratios.size() != ncomponents)
        throw value_error("different numbers of components and ratios");

    if (ncomponents == 0)
        throw value_error("no components in the virtual crystal");

    for (auto r : ratios)
        if (r <= 0)
            throw value_error("all atomic ratios must be positive");
    auto natoms = components[0].get_natoms();

    for (decltype(ncomponents) i = 1; i < ncomponents; ++i) {
        if (components[i].get_natoms() != natoms)
            throw value_error("all components must have the same"
                              " number of atoms");
    }
    // Build the weighted lattice vectors.
    double total{ratios[0]};
    Eigen::Matrix3d lattvec(ratios[0] * components[0].lattvec);

    for (decltype(ncomponents) i = 1; i < ncomponents; ++i) {
        total += ratios[i];
        lattvec += ratios[i] * components[i].lattvec;
    }
    lattvec /= total;
    // Build the weighted atomic positions.
    Eigen::Matrix<double, 3, Eigen::Dynamic> positions(ratios[0] *
                                                       components[0].positions);

    for (decltype(ncomponents) i = 1; i < ncomponents; ++i)
        positions += ratios[i] * components[i].positions;
    positions /= total;
    // And the set of virtual elements.
    std::vector<std::string> vnames;
    std::vector<int> numbers;

    for (decltype(natoms) ia = 0; ia < natoms; ++ia) {
        std::vector<std::string> symbols;

        for (auto& c : components)
            symbols.emplace_back(c.get_element(ia));
        // Adjacent atoms belonging to the same element are grouped
        // together.
        auto name = Virtual_element(symbols, ratios).get_name();

        if ((vnames.size() == 0) || (vnames.back() != name)) {
            vnames.emplace_back(name);
            numbers.emplace_back(1);
        }
        else
            ++numbers.back();
    }
    return std::make_unique<Crystal_structure>(
        lattvec, positions, vnames, numbers);
}

std::unique_ptr<Dielectric_parameters> vc_mix_dielectric_parameters(
    const std::vector<Dielectric_parameters>& components,
    const std::vector<double>& ratios) {
    auto ncomponents = components.size();

    // Basic sanity checks.
    if (ratios.size() != ncomponents)
        throw value_error("different numbers of components and ratios");

    if (ncomponents == 0)
        throw value_error("no components in the virtual crystal");

    for (auto r : ratios)
        if (r <= 0)
            throw value_error("all atomic ratios must be positive");
    auto nborn = components[0].born.size();
    double total{ratios[0]};
    Eigen::Matrix3d epsilon{ratios[0] * components[0].epsilon};
    std::vector<Eigen::MatrixXd> born;

    for (decltype(nborn) ib = 0; ib < nborn; ++ib)
        born.emplace_back(Eigen::MatrixXd{ratios[0] * components[0].born[ib]});

    for (decltype(ncomponents) i = 1; i < ncomponents; ++i) {
        if (components[i].born.size() != nborn)
            throw value_error("all components must contain the same"
                              " number of Born charge tensors");
        total += ratios[i];
        epsilon += ratios[i] * components[i].epsilon;

        for (decltype(nborn) ib = 0; ib < nborn; ++ib)
            born[ib] += ratios[i] * components[i].born[ib];
    }
    epsilon /= total;

    for (auto& b : born)
        b /= total;
    return std::make_unique<Dielectric_parameters>(born, epsilon);
}

std::unique_ptr<Harmonic_ifcs> vc_mix_harmonic_ifcs(
    const std::vector<Harmonic_ifcs>& components,
    const std::vector<double>& ratios) {
    auto ncomponents = components.size();

    // Basic sanity checks.
    if (ratios.size() != ncomponents)
        throw value_error("different numbers of components and ratios");

    if (ncomponents == 0)
        throw value_error("no components in the virtual crystal");

    for (auto r : ratios)
        if (r <= 0)
            throw value_error("all atomic ratios must be positive");
    // Only the ifcs themselves are averaged. The remainin
    // attributes must be common to all objects.
    double total{ratios[0]};
    auto na = components[0].na;
    auto nb = components[0].nb;
    auto nc = components[0].nc;
    auto nblocks = components[0].ifcs.size();
    std::vector<Triple_int> pos{components[0].pos};
    std::vector<Eigen::MatrixXd> blocks;

    for (decltype(nblocks) ib = 0; ib < nblocks; ++ib)
        blocks.emplace_back(
            Eigen::MatrixXd{ratios[0] * components[0].ifcs[ib]});

    for (decltype(ncomponents) i = 1; i < ncomponents; ++i) {
        if (components[i].ifcs.size() != nblocks)
            throw value_error("all components must contain the same"
                              " number of IFC matrices");
        total += ratios[i];

        for (decltype(nblocks) ib = 0; ib < nblocks; ++ib) {
            if (components[i].pos[ib] != pos[ib])
                throw value_error("all components must contain IFC matrices"
                                  " for the same cell pairs");
            blocks[ib] += ratios[i] * components[i].ifcs[ib];
        }
    }

    for (auto& b : blocks)
        b /= total;
    return std::make_unique<Harmonic_ifcs>(pos, blocks, na, nb, nc);
}

std::unique_ptr<std::vector<Thirdorder_ifcs>> vc_mix_thirdorder_ifcs(
    const std::vector<std::vector<Thirdorder_ifcs>>& components,
    const std::vector<double>& ratios) {
    auto ncomponents = components.size();

    // Basic sanity checks.
    if (ratios.size() != ncomponents)
        throw value_error("different numbers of components and ratios");

    if (ncomponents == 0)
        throw value_error("no components in the virtual crystal");

    for (auto r : ratios)
        if (r <= 0)
            throw value_error("all atomic ratios must be positive");
    auto nifcs = components[0].size();

    for (decltype(ncomponents) i = 1; i < ncomponents; ++i)
        if (components[i].size() != nifcs)
            throw value_error("all components must contain the same number"
                              " of IFC blocks");
    // Create the final object.
    auto nruter = std::make_unique<std::vector<Thirdorder_ifcs>>();
    // We average the Cartesian coordinates of the unit cells and
    // the IFCs. We require the atom indices to be common to all
    // inputs.
    double total = std::accumulate(ratios.begin(), ratios.end(), 0.);

    for (decltype(nifcs) ic = 0; ic < nifcs; ++ic) {
        auto i = components[0][ic].i;
        auto j = components[0][ic].j;
        auto k = components[0][ic].k;
        Eigen::VectorXd rj{ratios[0] * components[0][ic].rj};
        Eigen::VectorXd rk{ratios[0] * components[0][ic].rk};

        for (decltype(ncomponents) ir = 1; ir < ncomponents; ++ir) {
            if ((components[ir][ic].i != i) || (components[ir][ic].j != j) ||
                (components[ir][ic].k != k))
                throw value_error("all components must contain IFC blocks"
                                  " for the same atom triplets");
            rj += ratios[ir] * components[ir][ic].rj;
            rk += ratios[ir] * components[ir][ic].rk;
        }
        rj /= total;
        rk /= total;
        Thirdorder_ifcs block{rj, rk, i, j, k};

        for (std::size_t alpha = 0; alpha < 3; ++alpha)
            for (std::size_t beta = 0; beta < 3; ++beta)
                for (std::size_t gamma = 0; gamma < 3; ++gamma) {
                    block.ifc(alpha, beta, gamma) =
                        ratios[0] * components[0][ic].ifc(alpha, beta, gamma);

                    for (decltype(ncomponents) ir = 1; ir < ncomponents; ++ir)
                        block.ifc(alpha, beta, gamma) +=
                            ratios[ir] *
                            components[ir][ic].ifc(alpha, beta, gamma);
                    block.ifc(alpha, beta, gamma) /= total;
                }
        nruter->emplace_back(block);
    }
    return nruter;
}
} // namespace alma


/*
 * Copyright (c) 2011-2019, The DART development contributors
 * All rights reserved.
 *
 * The list of contributors can be found at:
 *   https://github.com/dartsim/dart/blob/master/LICENSE
 *
 * This file is provided under the following "BSD-style" License:
 *   Redistribution and use in source and binary forms, with or
 *   without modification, are permitted provided that the following
 *   conditions are met:
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
 *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
 *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
 *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
 *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *   POSSIBILITY OF SUCH DAMAGE.
 */

#include "dart/constraint/PgsBoxedLcpSolver.hpp"

#include <cmath>
#include <cstring>
#include <Eigen/Dense>
#include "dart/external/odelcpsolver/matrix.h"
#include "dart/external/odelcpsolver/misc.h"
#include "dart/math/Constants.hpp"

#define PGS_EPSILON 10e-9

namespace dart {
namespace constraint {

//==============================================================================
PgsBoxedLcpSolver::Option::Option(
    int maxIteration,
    double deltaXTolerance,
    double relativeDeltaXTolerance,
    double epsilonForDivision,
    bool randomizeConstraintOrder)
  : mMaxIteration(maxIteration),
    mDeltaXThreshold(deltaXTolerance),
    mRelativeDeltaXTolerance(relativeDeltaXTolerance),
    mEpsilonForDivision(epsilonForDivision),
    mRandomizeConstraintOrder(randomizeConstraintOrder)
{
  // Do nothing
}

//==============================================================================
const std::string& PgsBoxedLcpSolver::getType() const
{
  return getStaticType();
}

//==============================================================================
const std::string& PgsBoxedLcpSolver::getStaticType()
{
  static const std::string type = "PgsBoxedLcpSolver";
  return type;
}

//==============================================================================
bool PgsBoxedLcpSolver::solve(
    int n,
    double* A,
    double* x,
    double* b,
    int nub,
    double* lo,
    double* hi,
    int* findex,
    bool /*earlyTermination*/)
{
  const int nskip = dPAD(n);

  // If all the variables are unbounded then we can just factor, solve, and
  // return.R
  if (nub >= n)
  {
    mCacheD.resize(n);
    std::fill(mCacheD.begin(), mCacheD.end(), 0);

    dFactorLDLT(A, mCacheD.data(), n, nskip);
    dSolveLDLT(A, mCacheD.data(), b, n, nskip);
    std::memcpy(x, b, n * sizeof(double));

    return true;
  }

  mCacheOrder.clear();
  mCacheOrder.reserve(n);

  bool possibleToTerminate = true;
  for (int i = 0; i < n; ++i)
  {
    // mOrderCacheing
    if (A[nskip * i + i] < mOption.mEpsilonForDivision)
    {
      x[i] = 0.0;
      continue;
    }

    mCacheOrder.push_back(i);

    // Initial loop
    const double* A_ptr = A + nskip * i;
    const double old_x = x[i];

    double new_x = b[i];

    for (int j = 0; j < i; ++j)
      new_x -= A_ptr[j] * x[j];

    for (int j = i + 1; j < n; ++j)
      new_x -= A_ptr[j] * x[j];

    new_x /= A[nskip * i + i];

    if (findex[i] >= 0)
    {
      const double hi_tmp = hi[i] * x[findex[i]];
      const double lo_tmp = -hi_tmp;

      if (new_x > hi_tmp)
        x[i] = hi_tmp;
      else if (new_x < lo_tmp)
        x[i] = lo_tmp;
      else
        x[i] = new_x;
    }
    else
    {
      if (new_x > hi[i])
        x[i] = hi[i];
      else if (new_x < lo[i])
        x[i] = lo[i];
      else
        x[i] = new_x;
    }

    // Test
    if (possibleToTerminate)
    {
      const double deltaX = std::abs(x[i] - old_x);
      if (deltaX > mOption.mDeltaXThreshold)
        possibleToTerminate = false;
    }
  }

  if (possibleToTerminate)
  {
    return true;
  }

  // Normalizing
  for (const auto& index : mCacheOrder)
  {
    const double dummy = 1.0 / A[nskip * index + index];
    b[index] *= dummy;
    for (int j = 0; j < n; ++j)
      A[nskip * index + j] *= dummy;
  }

  for (int iter = 1; iter < mOption.mMaxIteration; ++iter)
  {
    if (mOption.mRandomizeConstraintOrder)
    {
      if ((iter & 7) == 0)
      {
        for (std::size_t i = 1; i < mCacheOrder.size(); ++i)
        {
          const int tmp = mCacheOrder[i];
          const int swapi = dRandInt(i + 1);
          mCacheOrder[i] = mCacheOrder[swapi];
          mCacheOrder[swapi] = tmp;
        }
      }
    }

    possibleToTerminate = true;

    // Single loop
    for (const auto& index : mCacheOrder)
    {
      const double* A_ptr = A + nskip * index;
      double new_x = b[index];
      const double old_x = x[index];

      for (int j = 0; j < index; j++)
        new_x -= A_ptr[j] * x[j];

      for (int j = index + 1; j < n; j++)
        new_x -= A_ptr[j] * x[j];

      if (findex[index] >= 0)
      {
        const double hi_tmp = hi[index] * x[findex[index]];
        const double lo_tmp = -hi_tmp;

        if (new_x > hi_tmp)
          x[index] = hi_tmp;
        else if (new_x < lo_tmp)
          x[index] = lo_tmp;
        else
          x[index] = new_x;
      }
      else
      {
        if (new_x > hi[index])
          x[index] = hi[index];
        else if (new_x < lo[index])
          x[index] = lo[index];
        else
          x[index] = new_x;
      }

      if (possibleToTerminate
          && std::abs(x[index]) > mOption.mEpsilonForDivision)
      {
        const double relativeDeltaX = std::abs((x[index] - old_x) / x[index]);
        if (relativeDeltaX > mOption.mRelativeDeltaXTolerance)
          possibleToTerminate = false;
      }
    }

    if (possibleToTerminate)
      break;
  }

  return possibleToTerminate;
}

#ifndef NDEBUG
//==============================================================================
bool PgsBoxedLcpSolver::canSolve(int n, const double* A)
{
  const int nskip = dPAD(n);

  // Return false if A has zero-diagonal or A is nonsymmetric matrix
  for (auto i = 0; i < n; ++i)
  {
    if (A[nskip * i + i] < PGS_EPSILON)
      return false;

    for (auto j = 0; j < n; ++j)
    {
      if (std::abs(A[nskip * i + j] - A[nskip * j + i]) > PGS_EPSILON)
        return false;
    }
  }

  return true;
}
#endif

//==============================================================================
void PgsBoxedLcpSolver::setOption(const PgsBoxedLcpSolver::Option& option)
{
  mOption = option;
}

//==============================================================================
const PgsBoxedLcpSolver::Option& PgsBoxedLcpSolver::getOption() const
{
  return mOption;
}

} // namespace constraint
} // namespace dart


#include <ceres/ceres.h>
#include <ceres/rotation.h>

#include "debug.h"
#include "types.h"
#include "imgproc.h"
#include "photobundle.h"
#include "sample_eigen.h"
#include "utils.h"

#if defined(WITH_CEREAL)
#include "ceres_cereal.h"
#include "eigen_cereal.h"
#include <fstream>
#include <cereal/archives/binary.hpp>
#include <cereal/types/memory.hpp>
#endif

#include <cassert>
#include <cmath>
#include <type_traits>
#include <iterator>
#include <algorithm>
#include <map>

// this is just for YCM to stop highlighting openmp as error (there is no openmp
// in clang3.5)
#define HAS_OPENMP __GNUC__ >= 4 && __clang__ == 0

#if HAS_OPENMP
#include <omp.h>
#endif

#include <Eigen/Geometry>

static PhotometricBundleAdjustment::Options::DescriptorType
DescriptorTypeFromString(std::string s)
{
  if(utils::icompare("Intensity", s))
    return PhotometricBundleAdjustment::Options::DescriptorType::Intensity;
  else if(utils::icompare("IntensityAndGradient", s))
    return PhotometricBundleAdjustment::Options::DescriptorType::IntensityAndGradient;
  else if(utils::icompare("BitPlanes", s))
    return PhotometricBundleAdjustment::Options::DescriptorType::BitPlanes;
  else {
    Warn("Unknown descriptorType '%s'\n", s.c_str());
    return PhotometricBundleAdjustment::Options::DescriptorType::Intensity;
  }
}

PhotometricBundleAdjustment::Result
PhotometricBundleAdjustment::Result::FromFile(std::string filename)
{
#if defined(WITH_CEREAL)
  std::ifstream ifs(filename);
  if(ifs.is_open()) {
    cereal::BinaryInputArchive ar(ifs);
    Result ret;
    ar(ret);
    return ret;
  } else {
    Fatal("Failed to open %s\n", filename.c_str());
  }
#else
  UNUSED(filename);
  Fatal("compile WITH_CEREAL\n");
#endif
}

bool PhotometricBundleAdjustment::Result::Writer::add(const Result& result)
{
  bool ret = false;

#if defined(WITH_CEREAL)
  std::ofstream ofs(utils::Format("%s/%05d.out", _prefix.c_str(), _counter++));
  if(ofs.is_open()) {
    cereal::BinaryOutputArchive ar(ofs);
    ar(result);
    ret = true;
  }
#else
  utils::UNUSED(result);
#endif

  return ret;
}

PhotometricBundleAdjustment::Options::Options(const utils::ConfigFile& cf)
  : maxNumPoints(cf.get<int>("maxNumPoints", 4096)),
    slidingWindowSize(cf.get<int>("slidingWindowSize", 5)),
    patchRadius(cf.get<int>("patchRadius", 2)),
    maskBlockRadius(cf.get<int>("maskBlockRadius", 1)),
    maxFrameDistance(cf.get<int>("maxFrameDistance", 1)),
    numThreads(cf.get<int>("numThreads", -1)),
    doGaussianWeighting((bool) cf.get<int>("doGaussianWeighting", 0)),
    verbose((bool) cf.get<int>("verbose", 1)),
    minScore(cf.get<double>("minScore", 0.75)),
    robustThreshold(cf.get<double>("robustThreshold", 0.05)),
    minValidDepth(cf.get<double>("minValidDepth", 0.01)),
    maxValidDepth(cf.get<double>("maxValidDepth", 1000.0)),
    nonMaxSuppRadius(cf.get<int>("nonMaxSuppRadius", 1)),
    descriptorType(DescriptorTypeFromString(cf.get<std::string>("descriptorType", "Intensity")))
{}

/**
 * simple class to store the image gradient
 */
class ImageGradient
{
 public:
  typedef Image_<float> ImageT;

 public:
  ImageGradient() = default;
  ImageGradient(const ImageT& Ix, const ImageT& Iy)
      : _Ix(Ix), _Iy(Iy) {}

  inline const ImageT& Ix() const { return _Ix; }
  inline const ImageT& Iy() const { return _Iy; }

  inline ImageT absGradientMag() const
  {
    return _Ix.array().abs() + _Iy.array().abs();
  }

  template <class InputImage>
  inline void compute(const InputImage& I)
  {
    static_assert(std::is_same<typename InputImage::Scalar, uint8_t>::value ||
                  std::is_same<typename InputImage::Scalar, float>::value,
                  "type mismatch, input image must be uint8_t or float");

    optResize(I.rows(), I.cols());
    imgradient(I.data(), ImageSize(I.rows(), I.cols()), _Ix.data(), _Iy.data());
  }

 private:
  ImageT _Ix;
  ImageT _Iy;

  void optResize(int rows, int cols)
  {
    if(_Ix.rows() != rows || _Ix.cols() != cols) {
      _Ix.resize(rows, cols);
      _Iy.resize(rows, cols);
    }
  }
}; // ImageGradient

class PhotometricBundleAdjustment::DescriptorFrame
{
 public:
  typedef EigenAlignedContainer_<Image_<float>> Channels;
  typedef EigenAlignedContainer_<ImageGradient> ImageGradientList;

 public:
  /**
   * \param frame_id the frame number (unique per image)
   * \param I        grayscale input image
   * \param gx       list of x-gradients per channel
   * \param gy       list of y-gradients per channel
   */
  inline DescriptorFrame(uint32_t frame_id, const Channels& channels)
      : _frame_id(frame_id), _channels(channels)
  {
    assert( !_channels.empty() );

    _max_rows = _channels[0].rows() - 1;
    _max_cols = _channels[0].cols() - 1;

    _gradients.resize( _channels.size() );
    for(size_t i = 0; i < _channels.size(); ++i) {
      _gradients[i].compute(_channels[i]);
    }
  }

  DescriptorFrame(const DescriptorFrame&) = delete;
  DescriptorFrame& operator=(const DescriptorFrame&) = delete;

  inline uint32_t id() const { return _frame_id; }

  inline bool operator<(const DescriptorFrame& other) const
  {
    return _frame_id < other._frame_id;
  }

  inline size_t numChannels() const { return _channels.size(); }

  inline const Image_<float>& getChannel(size_t i) const
  {
    assert(i < _channels.size());
    return _channels[i];
  }

  inline const ImageGradient& getChannelGradient(size_t i) const
  {
    assert(i < _gradients.size());
    return _gradients[i];
  }

  /**
   * \return true of point projects to the image
   */
  template <class ProjType> inline
  bool isProjectionValid(const ProjType& x) const
  {
    return x[0] >= 0.0 && x[0] < _max_cols &&
           x[1] >= 0.0 && x[1] < _max_rows;
  }

  void computeSaliencyMap(Image_<float>& smap) const
  {
    assert( !_gradients.empty() );

    smap.array() = _gradients[0].absGradientMag();
    for(size_t i = 1; i < _gradients.size(); ++i) {
      smap.array() += _gradients[i].absGradientMag().array();
    }
  }

 public:
  static inline DescriptorFrame* Create(uint32_t id, const Image_<uint8_t>& image,
                                        PhotometricBundleAdjustment::Options::DescriptorType type)
  {
    Channels channels;
    switch(type) {
      case PhotometricBundleAdjustment::Options::DescriptorType::Intensity:
        channels.push_back( image.cast<Channels::value_type::Scalar>() );
        break;
      case PhotometricBundleAdjustment::Options::DescriptorType::IntensityAndGradient: {
        channels.push_back( image.cast<Channels::value_type::Scalar>() );
        channels.push_back( Image_<float>(image.rows(), image.cols()) );
        channels.push_back( Image_<float>(image.rows(), image.cols()) );

        imgradient(image.data(), ImageSize(image.rows(), image.cols()),
                   channels[1].data(), channels[2].data());

      } break;
      case PhotometricBundleAdjustment::Options::DescriptorType::BitPlanes: {
        computeBitPlanes(image.data(), ImageSize(image.rows(), image.cols()), channels);
      } break;
    }

    return new DescriptorFrame(id, channels);
  }

 private:
  uint32_t _frame_id;
  uint32_t _max_rows;
  uint32_t _max_cols;

  Channels _channels;
  ImageGradientList _gradients;
}; // DescriptorFrame

/**
 * \return bilinearly interpolated pixel value at subpixel location (xf,yf)
 */
template <class Image, class T> inline
T interp2(const Image& I, T xf, T yf, T fillval = 0.0, T offset = 0.0)
{
  const int max_cols = I.cols() - 1;
  const int max_rows = I.rows() - 1;

  xf += offset;
  yf += offset;

  int xi = (int) std::floor(xf);
  int yi = (int) std::floor(yf);

  xf -= xi;
  yf -= yi;

  if( xi >= 0 && xi < max_cols && yi >= 0 && yi < max_rows )
  {
    const T wx = 1.0 - xf;
    return (1.0 - yf) * ( I(yi,   xi)*wx + I(yi,   xi+1)*xf )
               +  yf  * ( I(yi+1, xi)*wx + I(yi+1, xi+1)*xf );
  } else
  {
    if( xi == max_cols && yi < max_rows )
      return ( xf > 0 ) ? fillval : (1.0-yf)*I(yi,xi) + yf*I(yi+1, xi);
    else if( yi == max_rows && xi < max_cols )
      return ( yf > 0 ) ? fillval : (1.0-xf)*I(yi,xi) + xf*I(yi, xi+1);
    else if( xi == max_cols && yi == max_rows )
      return ( xf > 0 || yf > 0 ) ? fillval : I(yi, xi);
    else
      return fillval;
  }

}

template <int N> constexpr int square() { return N*N; }

template <int R, class ImageType, class ProjType, typename T = double>
void interpolateFixedPatch(Vec_<T, square<2*R+1>()>& dst,
                           const ImageType& I, const ProjType& p,
                           const T& fillval = T(0), const T& offset = T(0))
{
  const T x = static_cast<T>( p[0] + offset );
  const T y = static_cast<T>( p[1] + offset );

  auto d_ptr = dst.data();
  for(int r = -R; r <= R; ++r) {
    for(int c = -R; c <= R; ++c) {
      *d_ptr++ = interp2(I, c + x, r + y, fillval);
    }
  }
}

template <int R, typename T = float>
class ZnccPatch_
{
  static_assert(std::is_floating_point<T>::value, "T must be floating point");

 public:
  static constexpr int Radius    = R;
  static constexpr int Dimension = (2*R+1) * (2*R+1);

 public:
  inline ZnccPatch_() {}

  template <class ImageType, class ProjType> inline
  ZnccPatch_(const ImageType& image, const ProjType& uv) { set(image, uv); }

  template <class ImageType, class ProjType> inline
  const ZnccPatch_& set(const ImageType& I, const ProjType& uv)
  {
    interpolateFixedPatch<R>(_data, I, uv, T(0.0), T(0.0));
    T mean = _data.array().sum() / (T) _data.size();
    _data.array() -= mean;
    _norm = _data.norm();

    return *this;
  }

  template <class ImageType, class ProjType>
  inline static ZnccPatch_ FromImage(const ImageType& I, const ProjType& p)
  {
    ZnccPatch_ ret;
    ret.set(I, p);
    return ret;
  }

  inline T score(const ZnccPatch_& other) const
  {
    T d = _norm * other._norm;
    return d > 1e-6 ? _data.dot(other._data) / d : -1.0;
  }

 private:
  Vec_<T, Dimension> _data;
  T _norm;

 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
}; // ZnccPatch

/**
 */
struct PhotometricBundleAdjustment::ScenePoint
{
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

  typedef std::vector<uint32_t>        VisibilityList;
  typedef EigenAlignedContainer_<Vec2> ProjectionList;
  typedef ZnccPatch_<2, float>         ZnccPatchType;

  /**
   * Create a scene point with position 'X' and reference frame number 'f_id'
   *
   * We also store the original point for later comparision
   */
  inline ScenePoint(const Vec3& X, uint32_t f_id)
      : _X(X), _X_original(X)
  {
    _f.reserve(8);
    _f.push_back(f_id);
  }

  /**
   * \return true if the scene point has 'f_id' it is visibility list
   */
  inline bool hasFrame(uint32_t f_id) const {
    return std::find(_f.begin(), _f.end(), f_id) != _f.end();
  }

  /**
   * \return the visibility list
   */
  inline const VisibilityList& visibilityList() const { return _f; }

  /** \return the reference frame number (also the first in the list) */
  inline const uint32_t& refFrameId() const { return _f.front(); }

  /** \return the last frame number, most recent observation */
  inline const uint32_t& lastFrameId() const { return _f.back(); }

  /** \return the 3D point associated with the ScenePoint */
  inline const Vec3& X() const { return _X; }
  inline       Vec3& X()       { return _X; }

  /** \return the original 3D point */
  inline const Vec3& getOriginalPoint() const { return _X_original; }

  /** \return the associated patch */
  inline const ZnccPatchType& patch() const { return _patch; }

  inline void addFrame(uint32_t f) { _f.push_back(f); }

  template <class ImageType, class ProjType> inline
  void setZnccPach(const ImageType& I, const ProjType& x)
  {
    _patch.set(I, x);
  }

  inline const std::vector<double>& descriptor() const { return _descriptor; }
  inline       std::vector<double>& descriptor()       { return _descriptor; }

  inline void setSaliency(double v) { _saliency = v; }
  inline const double& getSaliency() const { return _saliency; }

  inline void setRefined(bool v) { _was_refined = v; }
  inline const bool& wasRefined() const { return _was_refined; }

  inline size_t numFrames() const { return _f.size(); }

  inline void setFirstProjection(const Vec_<int,2>& x) { _x = x; }
  inline const Vec_<int,2>& getFirstProjection() const { return _x; }

  Vec3 _X;
  Vec3 _X_original;
  VisibilityList _f;
  ZnccPatchType _patch;
  std::vector<double> _descriptor;

  double _saliency  = 0.0;
  bool _was_refined = false;

  Vec_<int,2> _x;
}; // ScenePoint

PhotometricBundleAdjustment::PhotometricBundleAdjustment(
    const Calibration& calib, const ImageSize& image_size, const Options& options)
  : _calib(calib), _image_size(image_size), _options(options),
    _frame_buffer(options.slidingWindowSize)
{
  _mask.resize(_image_size.rows, _image_size.cols);
  _saliency_map.resize(_image_size.rows, _image_size.cols);

  _K_inv = calib.K().inverse();
}

PhotometricBundleAdjustment::~PhotometricBundleAdjustment() {}

static inline int PatchSizeFromRadius(int r) { return (2*r+1) * (2*r+1);  }

static inline int PatchRadiusFromLength(int l) { return std::sqrt(l)/2; }

template <typename T, class Image> static inline
void ExtractPatch(T* dst, const Image& I, const Vec_<int,2>& uv, int radius)
{
  int max_cols = I.cols() - radius - 1,
      max_rows = I.rows() - radius - 1;

  for(int r = -radius, i=0; r <= radius; ++r) {
    int r_i = std::max(radius, std::min(uv[1] + r, max_rows));
    for(int c = -radius; c <= radius; ++c, ++i) {
      int c_i = std::max(radius, std::min(uv[0] + c, max_cols));
      dst[i] = static_cast<T>( I(r_i, c_i) );
    }
  }
}

void PhotometricBundleAdjustment::
addFrame(const uint8_t* I_ptr, const float* Z_ptr, const Mat44& T, Result* result)
{
  _trajectory.push_back(T, _frame_id);
  const Eigen::Isometry3d T_w(_trajectory.back());
  const Eigen::Isometry3d T_c(T_w.inverse());

  typedef Eigen::Map<const Image_<uint8_t>, Eigen::Aligned> SrcMap;
  auto I = SrcMap(I_ptr, _image_size.rows, _image_size.cols);

  typedef Eigen::Map<const Image_<float>, Eigen::Aligned> SrcDepthMap;
  auto Z = SrcDepthMap(Z_ptr, _image_size.rows, _image_size.cols);

  auto frame = DescriptorFrame::Create(_frame_id, I, _options.descriptorType);

  auto B = std::max(_options.maskBlockRadius, std::max(2, _options.patchRadius));
  auto max_rows = (int) I.rows() - B - 1,
       max_cols = (int) I.cols() - B - 1,
       radius = _options.patchRadius,
       patch_length = PatchSizeFromRadius(radius),
       descriptor_dim = (int) frame->numChannels() * patch_length,
       mask_radius = _options.maskBlockRadius;

  //
  // Establish "correspondences" with the old data. This is the visibility list
  // computation
  //
  _mask.setOnes();

  int num_updated = 0, max_num_to_update = 0;
  for(size_t i = 0; i < _scene_points.size(); ++i) {
    const auto& pt = _scene_points[i];
    int f_dist = _frame_id - pt->lastFrameId();
    if(f_dist <= _options.maxFrameDistance) { // do not go too far back
      //
      // If the point projects to the current frame and it zncc score is
      // sufficiently highly, we'll add the current image to its visibility list
      Vec2 uv = _calib.project(T_c * pt->X());
      ++max_num_to_update;

      int r = std::round(uv[1]), c = std::round(uv[0]);
      if(r >= B && r < max_rows && c >= B && c <= max_cols) {
        typename ScenePoint::ZnccPatchType other_patch(I, uv);
        auto score = pt->patch().score( other_patch );
        if(score > _options.minScore) {
          num_updated++;

          // TODO update the patch for the new frame data
          pt->addFrame(_frame_id);

          //
          // block an area in the mask to prevent initializing redandant new
          // scene points
          //
          for(int r_i = -mask_radius; r_i <= mask_radius; ++r_i)
            for(int c_i = -mask_radius; c_i <= mask_radius; ++c_i)
              _mask(r+r_i, c+c_i) = 0;
        }
      }
    }
  }

  //
  // Add new scene points
  //
  decltype(_scene_points) new_scene_points;
  new_scene_points.reserve( max_rows * max_cols * 0.5 );
  frame->computeSaliencyMap(_saliency_map);

  typedef IsLocalMax_<decltype(_saliency_map), decltype(_mask)> IsLocalMax;
  const IsLocalMax is_local_max(_saliency_map, _mask, _options.nonMaxSuppRadius);

  for(int y = B; y < max_rows; ++y) {
    for(int x = B; x < max_cols; ++x) {
      auto z = Z(y,x);
      if(z >= _options.minValidDepth && z <= _options.maxValidDepth) {
        if(is_local_max(y, x)) {
          Vec3 X = T_w * (z * _K_inv * Vec3(x, y, 1.0));

          auto p = make_unique<ScenePoint>(X, _frame_id);
          Vec_<int,2> xy(x, y);
          p->setZnccPach( I, xy );
          p->descriptor().resize(descriptor_dim);
          p->setSaliency( _saliency_map(y,x) );
          p->setFirstProjection(xy);

          new_scene_points.push_back(std::move(p));
        }
      }
    }
  }

  //
  // keep the best N points
  //
  if(new_scene_points.size() > (size_t) _options.maxNumPoints) {
    auto nth = new_scene_points.begin() + _options.maxNumPoints;
    std::nth_element(new_scene_points.begin(), nth, new_scene_points.end(),
                     [&](const ScenePointPointer& a, const ScenePointPointer& b) {
                      return a->getSaliency() > b->getSaliency();
                     });
    new_scene_points.erase(nth, new_scene_points.end());
  }

  //
  // extract the descriptors
  //
  const int num_channels = frame->numChannels(),
        num_new_points = (int) new_scene_points.size();

  Info("updated %d [%0.2f%%] max %d new %d\n",
       num_updated, 100.0 * num_updated / _scene_points.size(),
       max_num_to_update, num_new_points);

  for(int k = 0; k < num_channels; ++k) {
    const auto& channel = frame->getChannel(k);
    for(int i = 0; i < num_new_points; ++i) {
      auto ptr = new_scene_points[i]->descriptor().data() + k*patch_length;
      ExtractPatch(ptr, channel, new_scene_points[i]->getFirstProjection(), radius);
    }
  }

  _scene_points.reserve(_scene_points.size() + new_scene_points.size());
  std::move(new_scene_points.begin(), new_scene_points.end(), std::back_inserter(_scene_points));

  _frame_buffer.push_back(DescriptorFramePointer(frame));

  if(_frame_buffer.full()) {
    optimize(result);
  }

  ++_frame_id;
}

static inline std::vector<double>
MakePatchWeights(int radius, bool do_gaussian, double s_x = 1.0,
                 double s_y = 1.0, double a = 1.0)
{
  int n = (2*radius + 1) * (2*radius + 1);

  if(do_gaussian) {
    std::vector<double> ret(n);
    double sum = 0.0;
    for(int r = -radius, i = 0; r <= radius; ++r) {
      const double d_r = (r*r) / s_x;
      for(int c = -radius; c <= radius; ++c, ++i) {
        const double d_c = (c*c) / s_y;
        const double w = a * std::exp( -0.5 * (d_r + d_c) );
        ret[i] = w;
        sum += w;
      }
    }

    for(int i = 0; i < n; ++i) {
      ret[i] /= sum;
    }

    return ret;
  } else {
    return std::vector<double>(n, 1.0);
  }
}

static Vec_<double,6> PoseToParams(const Mat44& T)
{
  Vec_<double,6> ret;
  const Mat_<double,3,3> R = T.block<3,3>(0,0);
  ceres::RotationMatrixToAngleAxis(ceres::ColumnMajorAdapter3x3(R.data()), ret.data());

  ret[3] = T(0,3);
  ret[4] = T(1,3);
  ret[5] = T(2,3);
  return ret;
}

static Mat_<double,4,4> ParamsToPose(const double* p)
{
  Mat_<double,3,3> R;
  ceres::AngleAxisToRotationMatrix(p, ceres::ColumnMajorAdapter3x3(R.data()));

  Mat_<double,4,4> ret(Mat_<double,4,4>::Identity());
  ret.block<3,3>(0,0) = R;
  ret.block<3,1>(0,3) = Vec_<double,3>(p[3], p[4], p[5]);
  return ret;
}

class PhotometricBundleAdjustment::DescriptorError
{
 public:
  /**
   * \param radius  the patch radius
   * \param calib   the camera calibration
   * \param p0      the reference frame descriptor must have size (2*radius+1)^2
   * \param frame   descriptor data of the image we are matching against
   */
  DescriptorError(const Calibration& calib, const std::vector<double>& p0,
                  const DescriptorFrame* frame, const std::vector<double>& w)
      : _radius(PatchRadiusFromLength(p0.size() / frame->numChannels())),
      _calib(calib), _p0(p0.data()), _frame(frame), _patch_weights(w.data())
  {
    // TODO should just pass the config to get the radius value
    assert( p0.size() == w.size() );
  }

  static ceres::CostFunction* Create(const Calibration& calib,
                                     const std::vector<double>& p0,
                                     const DescriptorFrame* f,
                                     const std::vector<double>& w)
  {
    return new ceres::AutoDiffCostFunction<DescriptorError, ceres::DYNAMIC, 6, 3>(
        new DescriptorError(calib, p0, f, w), p0.size());
  }

  template <class T> inline
  bool operator()(const T* const camera, const T* const point, T* residuals) const
  {
    T xw[3];
    ceres::AngleAxisRotatePoint(camera, point, xw);
    xw[0] += camera[3];
    xw[1] += camera[4];
    xw[2] += camera[5];

    T u_w, v_w;
    _calib.project(xw, u_w, v_w);

    for(size_t k = 0, i=0; k < _frame->numChannels(); ++k) {
      const auto& I = _frame->getChannel(k);
      const auto& G = _frame->getChannelGradient(k);
      const auto& Gx = G.Ix();
      const auto& Gy = G.Iy();

      for(int y = -_radius, j = 0; y <= _radius; ++y) {
        const T v = v_w + T(y);
        for(int x = -_radius; x <= _radius; ++x, ++i, ++j) {
          const T u = u_w + T(x);
          const T i0 = T(_p0[i]);
          const T i1 = SampleWithDerivative(I, Gx, Gy, u, v);
          residuals[i] = _patch_weights[j] * (i0 - i1);
        }
      }
    }

    // maybe we should return false if the point goes out of the image!
    return true;
  }

 private:
  const int _radius;
  const Calibration& _calib;
  const double* const _p0;
  const DescriptorFrame* _frame;
  const double* const _patch_weights;
}; // DescriptorError

static inline ceres::Solver::Options
GetSolverOptions(int num_threads, bool verbose = false, double tol = 1e-6)
{
  ceres::Solver::Options options;

  options.linear_solver_type            = ceres::SPARSE_SCHUR;

  options.minimizer_type                = ceres::TRUST_REGION;
  options.trust_region_strategy_type    = ceres::LEVENBERG_MARQUARDT;

  options.preconditioner_type           = ceres::CLUSTER_JACOBI;
  options.visibility_clustering_type    = ceres::SINGLE_LINKAGE;
  options.minimizer_progress_to_stdout  = verbose;
  options.max_num_iterations            = 500;

  options.num_threads = num_threads;
  options.num_linear_solver_threads = options.num_threads;

  options.function_tolerance  = tol;
  options.gradient_tolerance  = tol;
  options.parameter_tolerance = tol;

  return options;
}

void PhotometricBundleAdjustment::optimize(Result* result)
{
  auto frame_id_start = _frame_buffer.front()->id(),
       frame_id_end   = _frame_buffer.back()->id();

  auto patch_weights = MakePatchWeights(_options.patchRadius, _options.doGaussianWeighting);

  //
  // collect the camera poses in a single map for easy access
  //
  std::map<uint32_t, Vec_<double,6>> camera_params;
  for(auto id = frame_id_start; id <= frame_id_end; ++id) {
    // NOTE camera parameters are inverted
    camera_params[id] = PoseToParams(Eigen::Isometry3d(_trajectory.atId(id)).inverse().matrix());
  }

  //
  // get the points that we *should* optimize. They must have a large enough
  // visibility list
  //
  ceres::Problem problem;
  int num_selected_points = 0;
  for(auto& pt : _scene_points) {
    // it is enough to check the visibility list length, because we will remove
    // points as soon as they leave the optimization window
    if(pt->numFrames() >= 3 && pt->refFrameId() >= frame_id_start) {
      num_selected_points++;
      for(auto id : pt->visibilityList()) {
        if(id >= frame_id_start && id <= frame_id_end) {
          pt->setRefined(true);
          auto* camera_ptr = camera_params[id].data();
          auto* xyz = pt->X().data();

          const auto huber_t = _options.robustThreshold;
          auto* loss = huber_t > 0.0 ? new ceres::HuberLoss(huber_t) : nullptr;

          ceres::CostFunction* cost = nullptr;
          cost = DescriptorError::Create(_calib, pt->descriptor(), getFrameAtId(id), patch_weights);
          problem.AddResidualBlock(cost, loss, camera_ptr, xyz);
        }
      }
    }
  }

  // set the first camera cosntant
  {
    auto p = camera_params[frame_id_start].data();
    if(problem.HasParameterBlock(p)) {
      problem.SetParameterBlockConstant(p);
    } else {
      Warn("first camera is not in bundle\n");
    }
  }

  Info("Using %d points (%d residual blocks) [id start %d]\n",
       num_selected_points, problem.NumResidualBlocks(), frame_id_start);

  ceres::Solver::Summary summary;

#if HAS_OPENMP
  int num_threads = _options.numThreads > 0 ? _options.numThreads : std::min(omp_get_max_threads(), 4);
#else
  int num_threads = 4;
#endif

  ceres::Solve(GetSolverOptions(num_threads, _options.verbose), &problem, &summary);
  if(_options.verbose)
    std::cout << summary.FullReport() << std::endl;

  //
  // TODO: run another optimization pass over residuals with small error
  // (eliminate the outliers)
  //

  //
  // put back the refined camera poses
  //
  for(auto& it : camera_params) {
    _trajectory.atId(it.first) = Eigen::Isometry3d(
        ParamsToPose(it.second.data())).inverse().matrix();
  }

  //
  // set a side the old points. Since we are doing a sliding window, all points
  // at frame_id_start should go out
  //
  auto points_to_remove = removePointsAtFrame(frame_id_start);
  printf("removing %zu old points\n", points_to_remove.size());

  //
  // check if we should return a result to the user
  //
  if(result) {
    result->poses = _trajectory.poses();
    const auto npts = points_to_remove.size();
    result->refinedPoints.resize(npts);
    result->originalPoints.resize(npts);
    for(size_t i = 0; i < npts; ++i) {
      result->refinedPoints[i] = points_to_remove[i]->X();
      result->originalPoints[i] = points_to_remove[i]->getOriginalPoint();
    }

    result->initialCost = summary.initial_cost;
    result->finalCost   = summary.final_cost;
    result->fixedCost   = summary.fixed_cost;
    result->numSuccessfulStep = summary.num_successful_steps;
    result->totalTime = summary.total_time_in_seconds;
    result->numResiduals = summary.num_residuals;
    result->message = std::string(summary.message);
    result->iterationSummary = summary.iterations;
  }
}

auto PhotometricBundleAdjustment::getFrameAtId(uint32_t id) const -> const DescriptorFrame*
{
  for(const auto& f : _frame_buffer)
    if(f->id() == id) {
      return f.get();
    }

  throw std::runtime_error("could not find frame id!");
}

auto PhotometricBundleAdjustment::removePointsAtFrame(uint32_t id) -> ScenePointPointerList
{
  using namespace std;

  decltype(_scene_points) points_to_keep, points_to_remove;

  points_to_keep.reserve(_scene_points.size());
  points_to_remove.reserve( 0.5 * _scene_points.size() );

  partition_copy(make_move_iterator(begin(_scene_points)),
                 make_move_iterator(end(_scene_points)),
                 back_inserter(points_to_remove),
                 back_inserter(points_to_keep),
                 [&](const ScenePointPointer& p) { return p->refFrameId() <= id; });

  _scene_points.swap(points_to_keep);
  return points_to_remove;
}