/*
 ewald.cpp

 Copyright (c) 2015 Tatsuro Nishimoto
 Copyright (c) 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 "mpi_common.h"
#include "ewald.h"
#include "constants.h"
#include "dynamical.h"
#include "error.h"
#include "kpoint.h"
#include "mathfunctions.h"
#include "memory.h"
#include "parsephon.h"
#include "system.h"
#include <iostream>
#include <fstream>
#include <vector>
#include <iomanip>
#include <complex>
#include <boost/math/special_functions/erf.hpp>

using namespace PHON_NS;

Ewald::Ewald(PHON *phon) : Pointers(phon)
{
    set_default_variables();
}

Ewald::~Ewald()
{
    deallocate_variables();
}

void Ewald::set_default_variables()
{
    is_longrange = false;
    print_fc2_ewald = false;
    file_longrange = "";
    prec_ewald = 1.0e-12;
    rate_ab = 1.0;
    multiplicity = nullptr;
    Born_charge = nullptr;
    distall_ewald = nullptr;
    force_permutation_sym = true;
}

void Ewald::deallocate_variables()
{
    if (multiplicity) {
        deallocate(multiplicity);
    }
    if (Born_charge) {
        deallocate(Born_charge);
    }
    if (distall_ewald) {
        deallocate(distall_ewald);
    }
}

void Ewald::init()
{
    MPI_Bcast(&is_longrange, 1, MPI_CXX_BOOL, 0, MPI_COMM_WORLD);
    MPI_Bcast(&prec_ewald, 1, MPI_DOUBLE, 0, MPI_COMM_WORLD);
    MPI_Bcast(&rate_ab, 1, MPI_DOUBLE, 0, MPI_COMM_WORLD);

    if (is_longrange) {
        int nsize[3] = {1, 1, 1};

        allocate(multiplicity, system->nat, system->nat);
        allocate(Born_charge, system->natmin, 3, 3);
        allocate(distall_ewald, system->nat, system->nat);

        get_pairs_of_minimum_distance(system->nat, nsize, system->xr_s);

        for (int i = 0; i < system->natmin; ++i) {
            for (int j = 0; j < 3; ++j) {
                for (int k = 0; k < 3; ++k) {
                    Born_charge[i][j][k] = dynamical->borncharge[i][j][k];
                }
            }
        }

        prepare_Ewald(dynamical->dielec);
        prepare_G();
        compute_ewald_fcs();
    }
}

void Ewald::prepare_Ewald(const double dielectric[3][3])
{
    int icrd;

    double lavec_norm[3], rlavec_norm[3];
    double e_lavec[3], e_rlavec[3];
    double lavec_enorm, rlavec_enorm, lavec_min[2], rlavec_min[2];

    if (mympi->my_rank == 0) {
        std::cout << std::endl;
        std::cout << "  Preparing for the Ewald summation ..." << std::endl << std::endl;
    }

    double p = -std::log(prec_ewald);

    for (icrd = 0; icrd < 3; ++icrd) {
        for (int jcrd = 0; jcrd < 3; ++jcrd) {
            epsilon[icrd][jcrd] = dielectric[icrd][jcrd];
        }
    }

    // Calculating convergence parameters
    invmat3(epsilon_inv, epsilon);

    // For calculating Coulombic (dipole-dipole) FCs
    for (icrd = 0; icrd < 3; ++icrd) {
        lavec_norm[icrd] = std::sqrt(std::pow(system->lavec_s[icrd][0], 2.0)
                                     + std::pow(system->lavec_s[icrd][1], 2.0)
                                     + std::pow(system->lavec_s[icrd][2], 2.0));

        rlavec_norm[icrd] = std::sqrt(std::pow(system->rlavec_s[icrd][0], 2.0)
                                      + std::pow(system->rlavec_s[icrd][1], 2.0)
                                      + std::pow(system->rlavec_s[icrd][2], 2.0));

        rotvec(e_lavec, system->lavec_s[icrd], epsilon_inv);
        rotvec(e_rlavec, system->rlavec_s[icrd], epsilon);

        lavec_enorm = std::sqrt(system->lavec_s[icrd][0] * e_lavec[0]
                                + system->lavec_s[icrd][1] * e_lavec[1]
                                + system->lavec_s[icrd][2] * e_lavec[2]);

        rlavec_enorm = std::sqrt(system->rlavec_s[icrd][0] * e_rlavec[0]
                                 + system->rlavec_s[icrd][1] * e_rlavec[1]
                                 + system->rlavec_s[icrd][2] * e_rlavec[2]);

        if (lavec_enorm < lavec_min[0] || icrd == 0) {
            lavec_min[0] = lavec_enorm;
            lavec_min[1] = lavec_norm[icrd];
        }
        if (rlavec_enorm < rlavec_min[0] || icrd == 0) {
            rlavec_min[0] = rlavec_enorm;
            rlavec_min[1] = rlavec_norm[icrd];
        }
    }

    Lmax_sub = std::sqrt(2.0 * p / (lavec_min[0] * rlavec_min[0] * std::pow(rate_ab, 1.0 / 3.0))) * lavec_min[1];
    Gmax_sub = std::sqrt(2.0 * p / (lavec_min[0] * rlavec_min[0]) * std::pow(rate_ab, 1.0 / 3.0)) * rlavec_min[1];
    lambda_sub = std::sqrt(rlavec_min[0] / (2.0 * lavec_min[0]) * std::pow(rate_ab, 1.0 / 3.0));

    for (icrd = 0; icrd < 3; ++icrd) {
        nl_sub[icrd] = static_cast<int>(Lmax_sub / lavec_norm[icrd]) + 1;
        ng_sub[icrd] = static_cast<int>(Gmax_sub / rlavec_norm[icrd]) + 1;
    }
    num_l_sub = (2 * nl_sub[0] + 1) * (2 * nl_sub[1] + 1) * (2 * nl_sub[2] + 1);
    num_g_sub = (2 * ng_sub[0] + 1) * (2 * ng_sub[1] + 1) * (2 * ng_sub[2] + 1);

    // For calculating Coulombic (dipole-dipole) dynamical matrix
    for (icrd = 0; icrd < 3; ++icrd) {
        lavec_norm[icrd] = std::sqrt(std::pow(system->lavec_p[icrd][0], 2.0)
                                     + std::pow(system->lavec_p[icrd][1], 2.0)
                                     + std::pow(system->lavec_p[icrd][2], 2.0));

        rlavec_norm[icrd] = std::sqrt(std::pow(system->rlavec_p[icrd][0], 2.0)
                                      + std::pow(system->rlavec_p[icrd][1], 2.0)
                                      + std::pow(system->rlavec_p[icrd][2], 2.0));

        rotvec(e_lavec, system->lavec_p[icrd], epsilon_inv);
        lavec_enorm = std::sqrt(system->lavec_p[icrd][0] * e_lavec[0]
                                + system->lavec_p[icrd][1] * e_lavec[1]
                                + system->lavec_p[icrd][2] * e_lavec[2]);

        rotvec(e_rlavec, system->rlavec_p[icrd], epsilon);
        rlavec_enorm = std::sqrt(system->rlavec_p[icrd][0] * e_rlavec[0]
                                 + system->rlavec_p[icrd][1] * e_rlavec[1]
                                 + system->rlavec_p[icrd][2] * e_rlavec[2]);

        if (lavec_enorm < lavec_min[0] || icrd == 0) {
            lavec_min[0] = lavec_enorm;
            lavec_min[1] = lavec_norm[icrd];
        }
        if (rlavec_enorm < rlavec_min[0] || icrd == 0) {
            rlavec_min[0] = rlavec_enorm;
            rlavec_min[1] = rlavec_norm[icrd];
        }
    }

    Lmax = std::sqrt(2.0 * p / (lavec_min[0] * rlavec_min[0] * std::pow(rate_ab, 1.0 / 3.0))) * lavec_min[1];
    Gmax = std::sqrt(2.0 * p / (lavec_min[0] * rlavec_min[0]) * std::pow(rate_ab, 1.0 / 3.0)) * rlavec_min[1];
    lambda = std::sqrt(rlavec_min[0] / (2.0 * lavec_min[0]) * std::pow(rate_ab, 1.0 / 3.0));

    for (icrd = 0; icrd < 3; ++icrd) {
        nl[icrd] = static_cast<int>(Lmax / lavec_norm[icrd]) + 1;
        ng[icrd] = static_cast<int>(Gmax / rlavec_norm[icrd]) + 1;
    }
    num_l = (2 * nl[0] + 1) * (2 * nl[1] + 1) * (2 * nl[2] + 1);
    num_g = (2 * ng[0] + 1) * (2 * ng[1] + 1) * (2 * ng[2] + 1);

    det_epsilon = epsilon[0][0] * (epsilon[1][1] * epsilon[2][2] - epsilon[1][2] * epsilon[2][1])
                  - epsilon[0][1] * (epsilon[1][0] * epsilon[2][2] - epsilon[1][2] * epsilon[2][0])
                  + epsilon[0][2] * (epsilon[1][0] * epsilon[2][1] - epsilon[1][1] * epsilon[2][0]);

    if (mympi->my_rank == 0) {

        std::cout << "  Inverse dielectric tensor : " << std::endl;
        for (int i = 0; i < 3; ++i) {
            for (int j = 0; j < 3; ++j) {
                std::cout << std::setw(15) << epsilon_inv[i][j];
            }
            std::cout << std::endl;
        }
        std::cout << std::endl;

        std::cout << "  Determinant of epsilon: " << std::setw(15) << det_epsilon
                  << std::endl << std::endl;

        std::cout << "  Parameters for the Ewald summation :" << std::endl;
        std::cout << "  - Force constant" << std::endl;
        std::cout << "    Lambda : " << std::setw(15) << lambda_sub << std::endl;
        std::cout << "    Lmax   : " << std::setw(15) << Lmax_sub << std::endl;
        std::cout << "    Gmax   : " << std::setw(15) << Gmax_sub << std::endl;
        std::cout << "    Maximum number of real-space cells : "
                  << std::setw(3) << nl_sub[0] << "x" << std::setw(3) << nl_sub[1] << "x" << std::setw(3) << nl_sub[2]
                  << std
                  ::endl;
        std::cout << "    Maximum number of reciprocal cells : "
                  << std::setw(3) << ng_sub[0] << "x" << std::setw(3) << ng_sub[1] << "x" << std::setw(3) << ng_sub[2]
                  << std
                  ::endl;
        std::cout << std::endl;
        std::cout << "  - Dynamical matrix" << std::endl;
        std::cout << "    Lambda : " << std::setw(15) << lambda << std::endl;
        std::cout << "    Lmax   : " << std::setw(15) << Lmax << std::endl;
        std::cout << "    Gmax   : " << std::setw(15) << Gmax << std::endl;
        std::cout << "    Maximum number of real-space cells : "
                  << std::setw(3) << nl[0] << "x" << std::setw(3) << nl[1] << "x" << std::setw(3) << nl[2] << std::endl;
        std::cout << "    Maximum number of reciprocal cells : "
                  << std::setw(3) << ng[0] << "x" << std::setw(3) << ng[1] << "x" << std::setw(3) << ng[2] << std::endl;
        std::cout << std::endl;
    }
}

void Ewald::prepare_G()
{
    // Accumulate reciprocal lattice vectors

    int ix, iy, iz;
    double g_tmp[3], gnorm;

    G_vector_sub.clear();
    G_vector.clear();

    for (ix = -ng_sub[0]; ix <= ng_sub[0]; ++ix) {
        for (iy = -ng_sub[1]; iy <= ng_sub[1]; ++iy) {
            for (iz = -ng_sub[2]; iz <= ng_sub[2]; ++iz) {
                if (ix == 0 && iy == 0 && iz == 0) continue;
                g_tmp[0] = static_cast<double>(ix);
                g_tmp[1] = static_cast<double>(iy);
                g_tmp[2] = static_cast<double>(iz);
                rotvec(g_tmp, g_tmp, system->rlavec_s, 'T');
                gnorm = std::sqrt(g_tmp[0] * g_tmp[0] + g_tmp[1] * g_tmp[1] + g_tmp[2] * g_tmp[2]);
                if (gnorm <= Gmax_sub) {
                    G_vector_sub.emplace_back(g_tmp);
                }
            }
        }
    }

    for (ix = -ng[0]; ix <= ng[0]; ++ix) {
        for (iy = -ng[1]; iy <= ng[1]; ++iy) {
            for (iz = -ng[2]; iz <= ng[2]; ++iz) {
                if (ix == 0 && iy == 0 && iz == 0) continue;
                g_tmp[0] = static_cast<double>(ix);
                g_tmp[1] = static_cast<double>(iy);
                g_tmp[2] = static_cast<double>(iz);
                rotvec(g_tmp, g_tmp, system->rlavec_p, 'T');
                gnorm = std::sqrt(g_tmp[0] * g_tmp[0] + g_tmp[1] * g_tmp[1] + g_tmp[2] * g_tmp[2]);
                if (gnorm <= Gmax) {
                    G_vector.emplace_back(g_tmp);
                }
            }
        }
    }
}

void Ewald::get_pairs_of_minimum_distance(const int nat,
                                          const int nsize[3],
                                          double **xf) const
{
    // Get pairs and multiplicities

    int icell = 0;
    int iat, jat;
    double dist_tmp;
    double ***xcrd; // fractional coordinate

    int ncell = (2 * nsize[0] + 1) * (2 * nsize[1] + 1) * (2 * nsize[2] + 1);

    allocate(xcrd, ncell, nat, 3);

    for (iat = 0; iat < nat; ++iat) {
        for (int icrd = 0; icrd < 3; ++icrd) {
            xcrd[0][iat][icrd] = xf[iat][icrd];
        }
        rotvec(xcrd[0][iat], xcrd[0][iat], system->lavec_s);
    }

    for (int isize = -nsize[0]; isize <= nsize[0]; ++isize) {
        for (int jsize = -nsize[1]; jsize <= nsize[1]; ++jsize) {
            for (int ksize = -nsize[2]; ksize <= nsize[2]; ++ksize) {
                if (isize == 0 && jsize == 0 && ksize == 0) continue;
                ++icell;
                for (iat = 0; iat < nat; ++iat) {
                    xcrd[icell][iat][0] = xf[iat][0] + static_cast<double>(isize);
                    xcrd[icell][iat][1] = xf[iat][1] + static_cast<double>(jsize);
                    xcrd[icell][iat][2] = xf[iat][2] + static_cast<double>(ksize);
                    rotvec(xcrd[icell][iat], xcrd[icell][iat], system->lavec_s);
                }

            }
        }
    }

    for (iat = 0; iat < nat; ++iat) {
        for (jat = 0; jat < nat; ++jat) {
            for (icell = 0; icell < ncell; ++icell) {
                dist_tmp = std::sqrt(std::pow(xcrd[0][iat][0] - xcrd[icell][jat][0], 2.0)
                                     + std::pow(xcrd[0][iat][1] - xcrd[icell][jat][1], 2.0)
                                     + std::pow(xcrd[0][iat][2] - xcrd[icell][jat][2], 2.0));

                distall_ewald[iat][jat].emplace_back(icell, dist_tmp);
            }
            std::sort(distall_ewald[iat][jat].begin(), distall_ewald[iat][jat].end());
        }
    }
    double dist_hold = -1.0;
    for (iat = 0; iat < nat; ++iat) {
        for (jat = 0; jat < nat; ++jat) {
            multiplicity[iat][jat] = 0;

            for (auto it = distall_ewald[iat][jat].begin();
                 it != distall_ewald[iat][jat].end(); ++it) {
                if (it == distall_ewald[iat][jat].begin()) dist_hold = (*it).dist;
                dist_tmp = (*it).dist;
                if (std::abs(dist_tmp - dist_hold) < 1.0e-3) multiplicity[iat][jat] += 1;
            }

        }
    }
    deallocate(xcrd);
}

void Ewald::compute_ewald_fcs()
{
    int i, j;
    int iat, jat;
    int icrd, jcrd;
    int atm_s;
    int nat = system->nat;
    int natmin = system->natmin;
    double **fcs_ewald;
    double **fc_ewald_short, **fc_ewald_long;
    double **fcs_total, **fcs_other;
    std::string file_fcs_ewald = input->job_title + ".fc2_ewald";

    if (mympi->my_rank == 0) {
        std::cout << " Calculating long-range (dipole-dipole) FCs in the supercell ...";
    }

    allocate(fcs_ewald, 3 * natmin, 3 * nat);
    allocate(fc_ewald_short, 3, 3);
    allocate(fc_ewald_long, 3, 3);

    for (iat = 0; iat < natmin; ++iat) {
        atm_s = system->map_p2s[iat][0];
        for (jat = 0; jat < nat; ++jat) {
            calc_short_term_ewald_fcs(atm_s, jat, fc_ewald_short);
            calc_long_term_ewald_fcs(atm_s, jat, fc_ewald_long);

            for (icrd = 0; icrd < 3; ++icrd) {
                for (jcrd = 0; jcrd < 3; ++jcrd) {
                    fcs_ewald[3 * iat + icrd][3 * jat + jcrd]
                            = fc_ewald_short[icrd][jcrd] + fc_ewald_long[icrd][jcrd];
                }
            }
        }
    }

    deallocate(fc_ewald_short);
    deallocate(fc_ewald_long);

    allocate(fcs_total, 3 * natmin, 3 * nat);
    allocate(fcs_other, 3 * natmin, 3 * nat);

    for (i = 0; i < 3 * natmin; ++i) {
        for (j = 0; j < 3 * nat; ++j) {
            fcs_total[i][j] = 0.0;
        }
    }

    for (const auto &it: fcs_phonon->fc2_ext) {
        fcs_total[3 * it.atm1 + it.xyz1][3 * it.atm2 + it.xyz2] += it.fcs_val;
    }

    for (i = 0; i < 3 * natmin; ++i) {
        for (j = 0; j < 3 * nat; ++j) {
            fcs_other[i][j] = fcs_total[i][j] - fcs_ewald[i][j];
        }
    }
    FcsClassExtent fcext_tmp;

    for (iat = 0; iat < natmin; ++iat) {
        atm_s = system->map_p2s[iat][0];

        for (icrd = 0; icrd < 3; ++icrd) {
            for (jat = 0; jat < nat; ++jat) {
                for (jcrd = 0; jcrd < 3; ++jcrd) {

                    fcext_tmp.atm1 = iat;
                    fcext_tmp.xyz1 = icrd;
                    fcext_tmp.atm2 = jat;
                    fcext_tmp.xyz2 = jcrd;

                    int nmulti = multiplicity[atm_s][jat];
                    fcext_tmp.fcs_val = fcs_other[3 * iat + icrd][3 * jat + jcrd] / static_cast<double>(nmulti);

                    if (std::abs(fcext_tmp.fcs_val) > eps15) {

                        for (int icell = 0; icell < nmulti; ++icell) {
                            fcext_tmp.cell_s = distall_ewald[atm_s][jat][icell].cell;
                            fc2_without_dipole.push_back(fcext_tmp);
                        }
                    }
                }
            }
        }
    }

    if (mympi->my_rank == 0) {
        if (print_fc2_ewald) {

            std::ofstream ofs_fcs_ewald;

            ofs_fcs_ewald.open(file_fcs_ewald.c_str(), std::ios::out);
            if (!ofs_fcs_ewald) exit("compute_ewald_fcs", "cannot open file PREFIX.fcs_ewald");

            ofs_fcs_ewald << "# Harmonic force constants" << std::endl;
            ofs_fcs_ewald << "# atom1, xyz1, atom2, xyz2, fc2 original, fc2 dipole-dipole, fc2_orig - fc2_dipole" << std
            ::endl;

            for (iat = 0; iat < natmin; ++iat) {
                for (icrd = 0; icrd < 3; ++icrd) {
                    for (jat = 0; jat < nat; ++jat) {
                        for (jcrd = 0; jcrd < 3; ++jcrd) {
                            ofs_fcs_ewald << std::setw(5) << iat + 1;
                            ofs_fcs_ewald << std::setw(5) << icrd + 1;
                            ofs_fcs_ewald << std::setw(5) << jat + 1;
                            ofs_fcs_ewald << std::setw(5) << jcrd + 1;
                            ofs_fcs_ewald << std::setw(15) << fcs_total[3 * iat + icrd][3 * jat + jcrd];
                            ofs_fcs_ewald << std::setw(15) << fcs_ewald[3 * iat + icrd][3 * jat + jcrd];
                            ofs_fcs_ewald << std::setw(15) << fcs_other[3 * iat + icrd][3 * jat + jcrd];
                            ofs_fcs_ewald << std::endl;
                        }
                    }
                }
            }
            ofs_fcs_ewald.close();
        }
    }

    deallocate(fcs_ewald);
    deallocate(fcs_total);
    deallocate(fcs_other);

    if (mympi->my_rank == 0) {
        std::cout << " done." << std::endl;
        if (print_fc2_ewald) {
            std::cout << std::endl;
            std::cout << " FC2_EWALD = 1: Dipole-dipole and short-ranged components of harmonic " << std::endl;
            std::cout << "                force constants are printed in " << file_fcs_ewald << std::endl;
        }
    }
}

void Ewald::compute_ewald_fcs2()
{
    int i, j;
    int iat, jat;
    int icrd, jcrd;
    int atm_s;
    int nat = system->nat;
    int natmin = system->natmin;
    double **fcs_ewald;
    double **fc_ewald_short, **fc_ewald_long;
    double **fcs_total, **fcs_other;
    std::string file_fcs_ewald = input->job_title + ".fc2_ewald";

    if (mympi->my_rank == 0) {
        std::cout << " Calculating long-range (dipole-dipole) FCs in the supercell ...";
    }

    std::vector<std::vector<double>> k_commensurate;
    kpoint->get_commensurate_kpoints(system->lavec_s,
                                     system->lavec_p,
                                     k_commensurate);

    allocate(fcs_ewald, 3 * natmin, 3 * nat);
    allocate(fc_ewald_short, 3, 3);
    allocate(fc_ewald_long, 3, 3);

    for (iat = 0; iat < natmin; ++iat) {
        atm_s = system->map_p2s[iat][0];
        for (jat = 0; jat < nat; ++jat) {
            calc_short_term_ewald_fcs(atm_s, jat, fc_ewald_short);
            calc_long_term_ewald_fcs(atm_s, jat, fc_ewald_long);

            for (icrd = 0; icrd < 3; ++icrd) {
                for (jcrd = 0; jcrd < 3; ++jcrd) {
                    fcs_ewald[3 * iat + icrd][3 * jat + jcrd]
                            = fc_ewald_short[icrd][jcrd] + fc_ewald_long[icrd][jcrd];
                }
            }
        }
    }

    deallocate(fc_ewald_short);
    deallocate(fc_ewald_long);

    allocate(fcs_total, 3 * natmin, 3 * nat);
    allocate(fcs_other, 3 * natmin, 3 * nat);

    for (i = 0; i < 3 * natmin; ++i) {
        for (j = 0; j < 3 * nat; ++j) {
            fcs_total[i][j] = 0.0;
        }
    }

    for (const auto &it: fcs_phonon->fc2_ext) {
        fcs_total[3 * it.atm1 + it.xyz1][3 * it.atm2 + it.xyz2] += it.fcs_val;
    }

    for (i = 0; i < 3 * natmin; ++i) {
        for (j = 0; j < 3 * nat; ++j) {
            fcs_other[i][j] = fcs_total[i][j] - fcs_ewald[i][j];
        }
    }
    FcsClassExtent fcext_tmp;

    for (iat = 0; iat < natmin; ++iat) {
        atm_s = system->map_p2s[iat][0];

        for (icrd = 0; icrd < 3; ++icrd) {
            for (jat = 0; jat < nat; ++jat) {
                for (jcrd = 0; jcrd < 3; ++jcrd) {

                    fcext_tmp.atm1 = iat;
                    fcext_tmp.xyz1 = icrd;
                    fcext_tmp.atm2 = jat;
                    fcext_tmp.xyz2 = jcrd;

                    int nmulti = multiplicity[atm_s][jat];
                    fcext_tmp.fcs_val = fcs_other[3 * iat + icrd][3 * jat + jcrd] / static_cast<double>(nmulti);

                    if (std::abs(fcext_tmp.fcs_val) > eps15) {

                        for (int icell = 0; icell < nmulti; ++icell) {
                            fcext_tmp.cell_s = distall_ewald[atm_s][jat][icell].cell;
                            fc2_without_dipole.push_back(fcext_tmp);
                        }
                    }
                }
            }
        }
    }

    if (mympi->my_rank == 0) {
        if (print_fc2_ewald) {

            std::ofstream ofs_fcs_ewald;

            ofs_fcs_ewald.open(file_fcs_ewald.c_str(), std::ios::out);
            if (!ofs_fcs_ewald) exit("compute_ewald_fcs", "cannot open file PREFIX.fcs_ewald");

            ofs_fcs_ewald << "# Harmonic force constants" << std::endl;
            ofs_fcs_ewald << "# atom1, xyz1, atom2, xyz2, fc2 original, fc2 dipole-dipole, fc2_orig - fc2_dipole" << std
            ::endl;

            for (iat = 0; iat < natmin; ++iat) {
                for (icrd = 0; icrd < 3; ++icrd) {
                    for (jat = 0; jat < nat; ++jat) {
                        for (jcrd = 0; jcrd < 3; ++jcrd) {
                            ofs_fcs_ewald << std::setw(5) << iat + 1;
                            ofs_fcs_ewald << std::setw(5) << icrd + 1;
                            ofs_fcs_ewald << std::setw(5) << jat + 1;
                            ofs_fcs_ewald << std::setw(5) << jcrd + 1;
                            ofs_fcs_ewald << std::setw(15) << fcs_total[3 * iat + icrd][3 * jat + jcrd];
                            ofs_fcs_ewald << std::setw(15) << fcs_ewald[3 * iat + icrd][3 * jat + jcrd];
                            ofs_fcs_ewald << std::setw(15) << fcs_other[3 * iat + icrd][3 * jat + jcrd];
                            ofs_fcs_ewald << std::endl;
                        }
                    }
                }
            }
            ofs_fcs_ewald.close();
        }
    }

    deallocate(fcs_ewald);
    deallocate(fcs_total);
    deallocate(fcs_other);

    if (mympi->my_rank == 0) {
        std::cout << " done." << std::endl;
        if (print_fc2_ewald) {
            std::cout << std::endl;
            std::cout << " FC2_EWALD = 1: Dipole-dipole and short-ranged components of harmonic " << std::endl;
            std::cout << "                force constants are printed in " << file_fcs_ewald << std::endl;
        }
    }
}

void Ewald::calc_short_term_ewald_fcs(const int iat,
                                      const int jat,
                                      double **fc_l_out)
{
    // Real lattice sum part for FCs
    // iat : atom index in the supercell (should be in the center primitive cell)
    // jat : atom index in the supercell

    int i;
    int icrd, jcrd;
    int icell, jcell, kcell;
    int kat, kkd;
    double xnorm;
    double x_tmp[3], trans[3];
    std::vector<std::vector<double>> func_L(3, std::vector<double>(3, 0.0));

    for (icrd = 0; icrd < 3; ++icrd) {
        for (jcrd = 0; jcrd < 3; ++jcrd) {
            fc_l_out[icrd][jcrd] = 0.0;
        }
    }

    if (iat == jat) {
        // (k,l) = (k',l')

        for (icell = -nl_sub[0]; icell <= nl_sub[0]; ++icell) {
            for (jcell = -nl_sub[1]; jcell <= nl_sub[1]; ++jcell) {
                for (kcell = -nl_sub[2]; kcell <= nl_sub[2]; ++kcell) {

                    if (icell == 0 && jcell == 0 && kcell == 0) {
                        // l'' = l = 0

                        for (kat = 0; kat < system->nat; ++kat) {

                            if (kat == iat) continue; // k'' = k

                            kkd = system->map_s2p[kat].atom_num;
                            for (i = 0; i < 3; ++i) {
                                x_tmp[i] = system->xr_s[iat][i] - system->xr_s[kat][i];
                            }
                            rotvec(x_tmp, x_tmp, system->lavec_s);
                            xnorm = std::sqrt(x_tmp[0] * x_tmp[0]
                                              + x_tmp[1] * x_tmp[1]
                                              + x_tmp[2] * x_tmp[2]);

                            if (xnorm < Lmax_sub) {

                                calc_realspace_sum(iat, kat, x_tmp, lambda_sub, func_L);

                                if (force_permutation_sym) {
                                    for (icrd = 0; icrd < 3; ++icrd) {
                                        for (jcrd = 0; jcrd < 3; ++jcrd) {
                                            fc_l_out[icrd][jcrd] += 0.5 * (func_L[icrd][jcrd]
                                                                           + func_L[jcrd][icrd]);
                                        }
                                    }
                                } else {
                                    for (icrd = 0; icrd < 3; ++icrd) {
                                        for (jcrd = 0; jcrd < 3; ++jcrd) {
                                            fc_l_out[icrd][jcrd] += func_L[icrd][jcrd];
                                        }
                                    }
                                }
                            }
                        }

                    } else {
                        // l'' != 0

                        trans[0] = static_cast<double>(icell);
                        trans[1] = static_cast<double>(jcell);
                        trans[2] = static_cast<double>(kcell);
                        rotvec(trans, trans, system->lavec_s);

                        for (kat = 0; kat < system->nat; ++kat) {
                            kkd = system->map_s2p[kat].atom_num;
                            for (i = 0; i < 3; ++i) {
                                x_tmp[i] = system->xr_s[iat][i] - system->xr_s[kat][i];
                            }
                            rotvec(x_tmp, x_tmp, system->lavec_s);
                            for (i = 0; i < 3; ++i) {
                                x_tmp[i] -= trans[i];
                            }
                            xnorm = std::sqrt(x_tmp[0] * x_tmp[0]
                                              + x_tmp[1] * x_tmp[1]
                                              + x_tmp[2] * x_tmp[2]);

                            if (xnorm < Lmax_sub) {
                                calc_realspace_sum(iat, kat, x_tmp, lambda_sub, func_L);

                                if (force_permutation_sym) {
                                    for (icrd = 0; icrd < 3; ++icrd) {
                                        for (jcrd = 0; jcrd < 3; ++jcrd) {
                                            fc_l_out[icrd][jcrd] += 0.5 * (func_L[icrd][jcrd]
                                                                           + func_L[jcrd][icrd]);
                                        }
                                    }
                                } else {
                                    for (icrd = 0; icrd < 3; ++icrd) {
                                        for (jcrd = 0; jcrd < 3; ++jcrd) {
                                            fc_l_out[icrd][jcrd] += func_L[icrd][jcrd];
                                        }
                                    }
                                }
                            }
                        }

                        for (i = 0; i < 3; ++i) {
                            x_tmp[i] = system->xr_s[iat][i] - system->xr_s[jat][i];
                        }
                        rotvec(x_tmp, x_tmp, system->lavec_s);
                        for (i = 0; i < 3; ++i) {
                            x_tmp[i] -= trans[i];
                        }
                        xnorm = std::sqrt(x_tmp[0] * x_tmp[0]
                                          + x_tmp[1] * x_tmp[1]
                                          + x_tmp[2] * x_tmp[2]);

                        if (xnorm < Lmax_sub) {
                            calc_realspace_sum(iat, jat, x_tmp, lambda_sub, func_L);

                            for (icrd = 0; icrd < 3; ++icrd) {
                                for (jcrd = 0; jcrd < 3; ++jcrd) {
                                    fc_l_out[icrd][jcrd] -= func_L[icrd][jcrd];
                                }
                            }
                        }
                    }
                }
            }
        }

    } else {

        // case of i != j
        // (k,l) != (k',l')

        for (icell = -nl_sub[0]; icell <= nl_sub[0]; ++icell) {
            for (jcell = -nl_sub[1]; jcell <= nl_sub[1]; ++jcell) {
                for (kcell = -nl_sub[2]; kcell <= nl_sub[2]; ++kcell) {

                    trans[0] = static_cast<double>(icell);
                    trans[1] = static_cast<double>(jcell);
                    trans[2] = static_cast<double>(kcell);

                    for (i = 0; i < 3; ++i) {
                        x_tmp[i] = system->xr_s[iat][i] - system->xr_s[jat][i] - trans[i];
                    }
                    rotvec(x_tmp, x_tmp, system->lavec_s);
                    xnorm = std::sqrt(x_tmp[0] * x_tmp[0]
                                      + x_tmp[1] * x_tmp[1]
                                      + x_tmp[2] * x_tmp[2]);

                    if (xnorm < Lmax_sub) {
                        calc_realspace_sum(iat, jat, x_tmp, lambda_sub, func_L);

                        for (icrd = 0; icrd < 3; ++icrd) {
                            for (jcrd = 0; jcrd < 3; ++jcrd) {
                                fc_l_out[icrd][jcrd] -= func_L[icrd][jcrd];
                            }
                        }
                    }

                }
            }
        }

    }
}

void Ewald::calc_long_term_ewald_fcs(const int iat,
                                     const int jat,
                                     double **fc_g_out)
{
    // Reciprocal lattice sum part for FCs

    int i;
    int icrd, jcrd;
    int acrd, bcrd;
    double gnorm2;
    double x_tmp[3], g_tmp[3], epsilon_gvector[3];
    double common_tmp;

    for (icrd = 0; icrd < 3; ++icrd) {
        for (jcrd = 0; jcrd < 3; ++jcrd) {
            fc_g_out[icrd][jcrd] = 0.0;
        }
    }
    double volume = system->volume(system->lavec_s[0],
                                   system->lavec_s[1],
                                   system->lavec_s[2]);

    int ikd = system->map_s2p[iat].atom_num;
    int jkd = system->map_s2p[jat].atom_num;

    double factor = 4.0 * pi / volume;

    if (iat == jat) {

        for (const auto &it: G_vector_sub) {
            for (i = 0; i < 3; ++i) g_tmp[i] = it.vec[i];
            rotvec(epsilon_gvector, g_tmp, epsilon);
            gnorm2 = g_tmp[0] * epsilon_gvector[0]
                     + g_tmp[1] * epsilon_gvector[1]
                     + g_tmp[2] * epsilon_gvector[2];

            for (int kat = 0; kat < system->nat; ++kat) {
                int kkd = system->map_s2p[kat].atom_num;

                for (i = 0; i < 3; ++i) {
                    x_tmp[i] = system->xr_s[iat][i] - system->xr_s[kat][i];
                }
                rotvec(x_tmp, x_tmp, system->lavec_s);

                common_tmp = factor * std::exp(-0.25 * gnorm2 / std::pow(lambda_sub, 2.0)) / gnorm2
                             * std::cos(g_tmp[0] * x_tmp[0]
                                        + g_tmp[1] * x_tmp[1]
                                        + g_tmp[2] * x_tmp[2]);

                for (icrd = 0; icrd < 3; ++icrd) {
                    for (jcrd = 0; jcrd < 3; ++jcrd) {
                        for (acrd = 0; acrd < 3; ++acrd) {
                            for (bcrd = 0; bcrd < 3; ++bcrd) {
                                if (force_permutation_sym) {
                                    fc_g_out[icrd][jcrd] -= g_tmp[acrd] * g_tmp[bcrd] * common_tmp
                                                            *
                                                            (Born_charge[ikd][acrd][icrd] * Born_charge[kkd][bcrd][jcrd]
                                                             +
                                                             Born_charge[jkd][acrd][jcrd] *
                                                             Born_charge[kkd][bcrd][icrd]);
                                } else {
                                    fc_g_out[icrd][jcrd] -= g_tmp[acrd] * g_tmp[bcrd] * common_tmp * 2.0
                                                            * (Born_charge[ikd][acrd][icrd] *
                                                               Born_charge[kkd][bcrd][jcrd]);
                                }
                            }
                        }
                    }
                }
            }
        }
    }

    for (const auto &it: G_vector_sub) {
        for (i = 0; i < 3; ++i) g_tmp[i] = it.vec[i];
        rotvec(epsilon_gvector, g_tmp, epsilon);
        gnorm2 = g_tmp[0] * epsilon_gvector[0]
                 + g_tmp[1] * epsilon_gvector[1]
                 + g_tmp[2] * epsilon_gvector[2];

        for (i = 0; i < 3; ++i) {
            x_tmp[i] = system->xr_s[iat][i] - system->xr_s[jat][i];
        }
        rotvec(x_tmp, x_tmp, system->lavec_s);

        common_tmp = 2.0 * factor * std::exp(-0.25 * gnorm2 / std::pow(lambda_sub, 2.0)) / gnorm2
                     * std::cos(g_tmp[0] * x_tmp[0] + g_tmp[1] * x_tmp[1] + g_tmp[2] * x_tmp[2]);

        for (icrd = 0; icrd < 3; ++icrd) {
            for (jcrd = 0; jcrd < 3; ++jcrd) {
                for (acrd = 0; acrd < 3; ++acrd) {
                    for (bcrd = 0; bcrd < 3; ++bcrd) {
                        fc_g_out[icrd][jcrd] += g_tmp[acrd] * g_tmp[bcrd] * common_tmp
                                                * Born_charge[ikd][acrd][icrd] * Born_charge[jkd][bcrd][jcrd];
                    }
                }
            }
        }
    }
}

void Ewald::add_longrange_matrix(const double *xk_in,
                                 const double *kvec_in,
                                 std::complex<double> **dymat_k_out)
{
    int icrd, jcrd, iat, jat;
    int natmin = system->natmin;
    int neval = 3 * system->natmin;
    double xk[3];
    std::complex<double> **dymat_tmp_l, **dymat_tmp_g;

    allocate(dymat_tmp_l, 3, 3);
    allocate(dymat_tmp_g, 3, 3);

    rotvec(xk, xk_in, system->rlavec_p, 'T');

    for (int i = 0; i < neval; ++i) {
        for (int j = 0; j < neval; ++j) {
            dymat_k_out[i][j] = std::complex<double>(0.0, 0.0);
        }
    }

    for (iat = 0; iat < natmin; ++iat) {
        for (jat = 0; jat < natmin; ++jat) {
            calc_short_term_dynamical_matrix(iat, jat, xk, dymat_tmp_l);
            calc_long_term_dynamical_matrix(iat, jat, xk, kvec_in, dymat_tmp_g);
            for (icrd = 0; icrd < 3; ++icrd) {
                for (jcrd = 0; jcrd < 3; ++jcrd) {
                    dymat_k_out[3 * iat + icrd][3 * jat + jcrd] = dymat_tmp_l[icrd][jcrd]
                                                                  + dymat_tmp_g[icrd][jcrd];
                }
            }

        }
    }
    deallocate(dymat_tmp_l);
    deallocate(dymat_tmp_g);

//    for (iat = 0; iat < natmin; ++iat) {
//        for (icrd = 0; icrd < 3; ++icrd) {
//            for (jat = 0; jat < natmin; ++jat) {
//                for (jcrd = 0; jcrd < 3; ++jcrd) {
//
//                    // Hermiticity
//                    std::complex<double> check = dymat_k_out[3 * iat + icrd][3 * jat + jcrd]
//                                                 - std::conj(dymat_k_out[3 * jat + jcrd][3 * iat + icrd]);
//                    if (std::abs(check) > eps10) {
//                        std::cout << std::endl;
//                        exit("add_longrange_matrix",
//                                    "Hermiticity of Dynamical matrix is broken.");
//                    }
//                }
//            }
//        }
//    }
}

void Ewald::calc_short_term_dynamical_matrix(const int iat,
                                             const int jat,
                                             double *xk_in,
                                             std::complex<double> **mat_out)
{
    // Real lattice sum part for a dynamical matrix
    // iat : atom index in the primitive cell
    // jat : atom index in the primitive cell

    int i;
    int icrd, jcrd, kat;
    int atm_s3;
    int icell, jcell, kcell;
    double xnorm, phase;
    double x_tmp[3], trans[3];
    std::complex<double> im(0.0, 1.0);
    std::vector<std::vector<double>> func_L(3, std::vector<double>(3, 0.0));

    // Substitute quantities into variables
    int atm_s1 = system->map_p2s[iat][0];
    int atm_s2 = system->map_p2s[jat][0];

    for (i = 0; i < 3; ++i) {
        for (int j = 0; j < 3; ++j) {
            mat_out[i][j] = std::complex<double>(0.0, 0.0);
        }
    }

    if (iat == jat) {
        // k = k'

        for (icell = -nl[0]; icell <= nl[0]; ++icell) {
            for (jcell = -nl[1]; jcell <= nl[1]; ++jcell) {
                for (kcell = -nl[2]; kcell <= nl[2]; ++kcell) {

                    trans[0] = static_cast<double>(icell);
                    trans[1] = static_cast<double>(jcell);
                    trans[2] = static_cast<double>(kcell);
                    rotvec(trans, trans, system->lavec_p);

                    // Lattice vector = 0
                    if (icell == 0 && jcell == 0 && kcell == 0) {

                        for (kat = 0; kat < system->natmin; ++kat) {
                            if (kat == iat) continue;

                            atm_s3 = system->map_p2s[kat][0];
                            for (i = 0; i < 3; ++i) {
                                x_tmp[i] = system->xr_s[atm_s1][i] - system->xr_s[atm_s3][i];
                            }
                            rotvec(x_tmp, x_tmp, system->lavec_s);
                            xnorm = std::sqrt(x_tmp[0] * x_tmp[0]
                                              + x_tmp[1] * x_tmp[1]
                                              + x_tmp[2] * x_tmp[2]);

                            if (xnorm < Lmax) {
                                calc_realspace_sum(atm_s1, atm_s3, x_tmp, lambda, func_L);

                                if (force_permutation_sym) {
                                    for (icrd = 0; icrd < 3; ++icrd) {
                                        for (jcrd = 0; jcrd < 3; ++jcrd) {
                                            mat_out[icrd][jcrd] += 0.5 * (func_L[icrd][jcrd]
                                                                          + func_L[jcrd][icrd]);
                                        }
                                    }
                                } else {
                                    for (icrd = 0; icrd < 3; ++icrd) {
                                        for (jcrd = 0; jcrd < 3; ++jcrd) {
                                            mat_out[icrd][jcrd] += func_L[icrd][jcrd];
                                        }
                                    }
                                }
                            }
                        }

                    } else {

                        for (kat = 0; kat < system->natmin; ++kat) {
                            atm_s3 = system->map_p2s[kat][0];
                            for (i = 0; i < 3; ++i) {
                                x_tmp[i] = system->xr_s[atm_s1][i] - system->xr_s[atm_s3][i];
                            }
                            rotvec(x_tmp, x_tmp, system->lavec_s);
                            for (i = 0; i < 3; ++i) {
                                x_tmp[i] -= trans[i];
                            }
                            xnorm = std::sqrt(x_tmp[0] * x_tmp[0]
                                              + x_tmp[1] * x_tmp[1]
                                              + x_tmp[2] * x_tmp[2]);

                            if (xnorm < Lmax) {

                                calc_realspace_sum(atm_s1, atm_s3, x_tmp, lambda, func_L);

                                if (force_permutation_sym) {
                                    for (icrd = 0; icrd < 3; ++icrd) {
                                        for (jcrd = 0; jcrd < 3; ++jcrd) {
                                            mat_out[icrd][jcrd] += 0.5 * (func_L[icrd][jcrd]
                                                                          + func_L[jcrd][icrd]);
                                        }
                                    }
                                } else {
                                    for (icrd = 0; icrd < 3; ++icrd) {
                                        for (jcrd = 0; jcrd < 3; ++jcrd) {
                                            mat_out[icrd][jcrd] += func_L[icrd][jcrd];
                                        }
                                    }
                                }
                            }
                        }

                        for (i = 0; i < 3; ++i) {
                            x_tmp[i] = system->xr_s[atm_s1][i] - system->xr_s[atm_s2][i];
                        }
                        rotvec(x_tmp, x_tmp, system->lavec_s);
                        for (i = 0; i < 3; ++i) {
                            x_tmp[i] -= trans[i];
                        }
                        xnorm = std::sqrt(x_tmp[0] * x_tmp[0]
                                          + x_tmp[1] * x_tmp[1]
                                          + x_tmp[2] * x_tmp[2]);

                        if (xnorm < Lmax) {
                            calc_realspace_sum(atm_s1, atm_s2, x_tmp, lambda, func_L);
                            phase = xk_in[0] * trans[0] + xk_in[1] * trans[1] + xk_in[2] * trans[2];

                            for (icrd = 0; icrd < 3; ++icrd) {
                                for (jcrd = 0; jcrd < 3; ++jcrd) {
                                    mat_out[icrd][jcrd] -= func_L[icrd][jcrd] * std::exp(im * phase);
                                }
                            }
                        }
                    }
                }
            }
        }

    } else {

        for (icell = -nl[0]; icell <= nl[0]; ++icell) {
            for (jcell = -nl[1]; jcell <= nl[1]; ++jcell) {
                for (kcell = -nl[2]; kcell <= nl[2]; ++kcell) {

                    trans[0] = static_cast<double>(icell);
                    trans[1] = static_cast<double>(jcell);
                    trans[2] = static_cast<double>(kcell);
                    rotvec(trans, trans, system->lavec_p);

                    for (i = 0; i < 3; ++i) {
                        x_tmp[i] = system->xr_s[atm_s1][i] - system->xr_s[atm_s2][i];
                    }
                    rotvec(x_tmp, x_tmp, system->lavec_s);
                    for (i = 0; i < 3; ++i) {
                        x_tmp[i] -= trans[i];
                    }
                    xnorm = std::sqrt(x_tmp[0] * x_tmp[0]
                                      + x_tmp[1] * x_tmp[1]
                                      + x_tmp[2] * x_tmp[2]);

                    if (xnorm < Lmax) {
                        calc_realspace_sum(atm_s1, atm_s2, x_tmp, lambda, func_L);
                        phase = xk_in[0] * trans[0] + xk_in[1] * trans[1] + xk_in[2] * trans[2];

                        for (icrd = 0; icrd < 3; ++icrd) {
                            for (jcrd = 0; jcrd < 3; ++jcrd) {
                                mat_out[icrd][jcrd] -= func_L[icrd][jcrd] * std::exp(im * phase);
                            }
                        }
                    }
                }
            }
        }

    }

    const auto mi = system->mass[atm_s1];
    const auto mj = system->mass[atm_s2];
    for (icrd = 0; icrd < 3; ++icrd) {
        for (jcrd = 0; jcrd < 3; ++jcrd) {
            mat_out[icrd][jcrd] /= std::sqrt(mi * mj);
        }
    }
}

void Ewald::calc_long_term_dynamical_matrix(const int iat,
                                            const int jat,
                                            const double *xk_in,
                                            const double *kvec_in,
                                            std::complex<double> **mat_out)
{
    // Real lattice sum part for a dynamical matrix

    int i, j;
    int icrd, jcrd, acrd, bcrd;
    double vec[3], e_kvec[3];
    std::complex<double> im(0.0, 1.0);
    double tmp;

    int atm_s1 = system->map_p2s[iat][0];
    int atm_s2 = system->map_p2s[jat][0];
    double mi = system->mass[atm_s1];
    double mj = system->mass[atm_s2];
    double vol_p = system->volume_p;
    for (i = 0; i < 3; ++i) {
        vec[i] = system->xr_s[atm_s1][i] - system->xr_s[atm_s2][i];
    }
    rotvec(vec, vec, system->lavec_s);
    double phase = xk_in[0] * vec[0] + xk_in[1] * vec[1] + xk_in[2] * vec[2];
    rotvec(e_kvec, xk_in, epsilon);

    for (i = 0; i < 3; ++i) {
        for (j = 0; j < 3; ++j) {
            mat_out[i][j] = std::complex<double>(0.0, 0.0);
        }
    }
    double kd = xk_in[0] * e_kvec[0] + xk_in[1] * e_kvec[1] + xk_in[2] * e_kvec[2];

    std::complex<double> exp_phase = std::exp(im * phase);

    if (std::sqrt(kd) > eps10) {

        // constant analytic term

        for (icrd = 0; icrd < 3; ++icrd) {
            for (jcrd = 0; jcrd < 3; ++jcrd) {
                tmp = 0.0;

                for (acrd = 0; acrd < 3; ++acrd) {
                    for (bcrd = 0; bcrd < 3; ++bcrd) {
                        tmp += xk_in[acrd] * xk_in[bcrd]
                               * Born_charge[iat][acrd][icrd] * Born_charge[jat][bcrd][jcrd];
                    }
                }
                mat_out[icrd][jcrd] += 2.0 * tmp / kd * exp_phase
                                       * std::exp(-0.25 * kd / std::pow(lambda, 2.0));
            }
        }

    } else {

        // Treat non-analytic term

        double kdirec[3], e_kdirec[3];
        for (i = 0; i < 3; ++i) kdirec[i] = kvec_in[i];
        rotvec(e_kdirec, kdirec, epsilon);
        double norm = kdirec[0] * e_kdirec[0] + kdirec[1] * e_kdirec[1] + kdirec[2] * e_kdirec[2];

        if (norm > eps) {
            for (icrd = 0; icrd < 3; ++icrd) {
                for (jcrd = 0; jcrd < 3; ++jcrd) {
                    tmp = 0.0;

                    for (acrd = 0; acrd < 3; ++acrd) {
                        for (bcrd = 0; bcrd < 3; ++bcrd) {
                            tmp += kdirec[acrd] * kdirec[bcrd]
                                   * Born_charge[iat][acrd][icrd] * Born_charge[jat][bcrd][jcrd];
                        }
                    }
                    mat_out[icrd][jcrd] += 2.0 * tmp / norm * exp_phase;
                }
            }
        }
    }

    double g[3], gk[3], vecl[3], g_tmp[3], gk_tmp[3];
    double common;

    for (auto &it: G_vector) {
        for (int l = 0; l < 3; ++l) {
            g[l] = it.vec[l];
            gk[l] = g[l] + xk_in[l];
        }

        if (iat == jat) {

            rotvec(g_tmp, g, epsilon);
            double gd = g[0] * g_tmp[0] + g[1] * g_tmp[1] + g[2] * g_tmp[2];
            common = std::exp(-0.25 * gd / std::pow(lambda, 2.0)) / gd;

            for (int kat = 0; kat < system->natmin; ++kat) {
                int atm_s3 = system->map_p2s[kat][0];

                for (i = 0; i < 3; ++i) {
                    vecl[i] = system->xr_s[atm_s1][i] - system->xr_s[atm_s3][i];
                }
                rotvec(vecl, vecl, system->lavec_s);
                double phase_g1 = g[0] * vecl[0] + g[1] * vecl[1] + g[2] * vecl[2];
                exp_phase = std::exp(im * phase_g1);

                for (icrd = 0; icrd < 3; ++icrd) {
                    for (jcrd = 0; jcrd < 3; ++jcrd) {
                        tmp = 0.0;

                        for (acrd = 0; acrd < 3; ++acrd) {
                            for (bcrd = 0; bcrd < 3; ++bcrd) {
                                if (force_permutation_sym) {
                                    tmp += g[acrd] * g[bcrd]
                                           * (Born_charge[iat][acrd][icrd] * Born_charge[kat][bcrd][jcrd]
                                              + Born_charge[jat][acrd][jcrd] * Born_charge[kat][bcrd][icrd]);
                                } else {
                                    tmp += g[acrd] * g[bcrd] * 2.0
                                           * (Born_charge[iat][acrd][icrd] * Born_charge[kat][bcrd][jcrd]);
                                }
                            }
                        }
                        mat_out[icrd][jcrd] -= tmp * common * exp_phase;
                    }
                }
            }
        }

        rotvec(gk_tmp, gk, epsilon);
        double gkd = gk[0] * gk_tmp[0] + gk[1] * gk_tmp[1] + gk[2] * gk_tmp[2];
        double phase_g2 = gk[0] * vec[0] + gk[1] * vec[1] + gk[2] * vec[2];

        common = 2.0 * std::exp(-0.25 * gkd / std::pow(lambda, 2.0)) / gkd;
        exp_phase = std::exp(im * phase_g2);

        for (icrd = 0; icrd < 3; ++icrd) {
            for (jcrd = 0; jcrd < 3; ++jcrd) {
                tmp = 0.0;

                for (acrd = 0; acrd < 3; ++acrd) {
                    for (bcrd = 0; bcrd < 3; ++bcrd) {
                        tmp += gk[acrd] * gk[bcrd]
                               * Born_charge[iat][acrd][icrd] * Born_charge[jat][bcrd][jcrd];
                    }
                }
                mat_out[icrd][jcrd] += tmp * common * exp_phase;
            }
        }
    }

    for (i = 0; i < 3; ++i) {
        for (j = 0; j < 3; ++j) {
            mat_out[i][j] *= 4.0 * pi / (vol_p * std::sqrt(mi * mj));
        }
    }
}

void Ewald::calc_realspace_sum(const int iat,
                               const int jat,
                               const double xdist[3],
                               const double lambda_in,
                               std::vector<std::vector<double>> &ret)
{
    // iat : atom index in the supercell
    // jat : atom index in the supercell
    // xdist: distance between the two atomic sites
    unsigned int icrd, jcrd, acrd, bcrd;
    double tmp;
    double **hmat_tmp;
    const double lambda3 = std::pow(lambda_in, 3.0);
    allocate(hmat_tmp, 3, 3);

    calc_anisotropic_hmat(lambda_in, xdist, hmat_tmp);

    const auto ikd = system->map_s2p[iat].atom_num;
    const auto jkd = system->map_s2p[jat].atom_num;

    for (icrd = 0; icrd < 3; ++icrd) {
        for (jcrd = 0; jcrd < 3; ++jcrd) {
            ret[icrd][jcrd] = 0.0;
        }
    }

    for (icrd = 0; icrd < 3; ++icrd) {
        for (jcrd = 0; jcrd < 3; ++jcrd) {
            tmp = 0.0;
            for (acrd = 0; acrd < 3; ++acrd) {
                for (bcrd = 0; bcrd < 3; ++bcrd) {
                    tmp += hmat_tmp[acrd][bcrd]
                           * Born_charge[ikd][acrd][icrd]
                           * Born_charge[jkd][bcrd][jcrd];
                }
            }
            ret[icrd][jcrd] = 2.0 * tmp * lambda3;
        }
    }
    deallocate(hmat_tmp);
}

void Ewald::calc_anisotropic_hmat(const double lambda_in,
                                  const double *x,
                                  double **hmat_out)
{
    // Compute H_ab(0\kappa;\ell'\kappa')
    int icrd, jcrd;
    double common_tmp[2];
    double x_tmp[3], y_tmp[3];

    for (icrd = 0; icrd < 3; ++icrd) {
        for (jcrd = 0; jcrd < 3; ++jcrd) {
            hmat_out[icrd][jcrd] = 0.0;
        }
    }

    for (int i = 0; i < 3; ++i) {
        y_tmp[i] = x[i] * lambda_in;
    }

    rotvec(x_tmp, y_tmp, epsilon_inv);

    double yd = std::sqrt(x_tmp[0] * y_tmp[0] + x_tmp[1] * y_tmp[1] + x_tmp[2] * y_tmp[2]);
    if (yd == 0.0) {
        exit("ewald->calc_anisotropic_hmat", "components of hmat diverge.");
    }
    double yd_inv = 1.0 / yd;
    double yd2 = std::pow(yd, 2.0);
    double yd2_inv = yd_inv * yd_inv;
    double erfc_y = boost::math::erfc(yd);
    double exp_y2 = std::exp(-yd2);
    double two_over_sqrtpi = 2.0 / std::sqrt(pi);

    common_tmp[0] = (3.0 * yd_inv * yd2_inv * erfc_y + two_over_sqrtpi * (3.0 * yd2_inv + 2.0) * exp_y2)
                    * yd2_inv / std::sqrt(det_epsilon);
    common_tmp[1] = (yd_inv * yd2_inv * erfc_y + two_over_sqrtpi * yd2_inv * exp_y2) / std::sqrt(det_epsilon);

    for (icrd = 0; icrd < 3; ++icrd) {
        for (jcrd = 0; jcrd < 3; ++jcrd) {
            hmat_out[icrd][jcrd] = x_tmp[icrd] * x_tmp[jcrd] * common_tmp[0]
                                   - epsilon_inv[icrd][jcrd] * common_tmp[1];
        }
    }
}


/* Copyright (c) 2015 USC, IRIS, Computer vision Lab */
#include "FaceServices2.h"
#include <fstream>
//#include "opencv2/contrib/contrib.hpp"
#include <opencv2/calib3d.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/highgui.hpp>
#include <Eigen/SparseLU>
//#include <Eigen/SPQRSupport>
#include <omp.h>

using namespace std;
using namespace cv;

FaceServices2::FaceServices2(void)
{
	//omp_set_num_threads(8);
	prevEF = 1000;
	mstep = 0.0001;
	countFail = 0;
	maxVal = 4;
	mlambda = 0.005;
	PREV_USE_THRESH = 3.141592/9;
}

void FaceServices2::setUp(int w, int h, float f){
	memset(_k,0,9*sizeof(float));
	_k[8] = 1;
	_k[0] = -f;
	_k[4] = f;
	_k[2] = w/2.0f;
	_k[5] = h/2.0f;
	faces = festimator.getFaces() - 1;
	cv::Mat shape = festimator.getShape(cv::Mat(99,1,CV_32F));
	tex = shape*0 + 128;
	im_render = new FImRenderer(cv::Mat::zeros(h,w,CV_8UC3));
	im_render->loadMesh(shape,shape*0,faces);
}

// Yuval
void FaceServices2::init(int w, int h, float f)
{
    // Initialize camera matrix
    memset(_k, 0, 9 * sizeof(float));
    _k[8] = 1;
    _k[0] = -f;
    _k[4] = f;
    _k[2] = w / 2.0f;
    _k[5] = h / 2.0f;

    // Initialize shape and texture
    if (faces.empty())
        faces = festimator.getFaces() - 1;
    cv::Mat shape = festimator.getShape(cv::Mat(99, 1, CV_32F));
    tex = shape * 0 + 128;

    // Initialize image renderer
    if (im_render == nullptr)
    {
        im_render = new FImRenderer(cv::Mat::zeros(h, w, CV_8UC3));
        im_render->loadMesh(shape, shape * 0, faces);
    }
    else im_render->init(cv::Mat::zeros(h, w, CV_8UC3));
}

bool FaceServices2::projectCheckVis(FImRenderer* im_render, cv::Mat shape, float* r, float *t, cv::Mat refDepth, bool* &visible){
	float zNear_ = im_render->zNear;
	float zFar_ = im_render->zFar;

	cv::Mat k_m( 3, 3, CV_32F, _k );

	int nV = shape.rows;
	if (visible == 0) visible = new bool[shape.rows];
	cv::Mat rVec( 3, 1, CV_32F, r );
	cv::Mat tVec( 3, 1, CV_32F, t );
	cv::Mat rMat;
	cv::Rodrigues(rVec, rMat);
	cv::Mat new3D = rMat * shape.t() + cv::repeat(tVec,1,nV);

	for (int i=0;i<nV;i++){
		visible[i] = false;
		float Z = new3D.at<float>(2,i);
		float x = -new3D.at<float>(0,i)/Z*_k[4] + _k[2];
		float y = new3D.at<float>(1,i)/Z*_k[4] + _k[5];
		if (x > 0 && y > 0 & x < refDepth.cols-1 && y <refDepth.rows-1) {
			for (int dx =-1;dx<2;dx++){
				for (int dy =-1;dy<2;dy++){
					float dd = refDepth.at<float>(y+dy,x+dx);
					dd = - zNear_*zFar_   / ( zFar_ - dd * ( zFar_ - zNear_ ));
					if (fabs(Z - dd) < 5){
						visible[i] = true;
					}
				}
			}
		}
	}

	return true;
}

std::vector<cv::Point2f> FaceServices2::projectCheckVis2(FImRenderer* im_render, cv::Mat shape, float* r, float *t, cv::Mat refDepth, bool* &visible){
	float zNear_ = im_render->zNear;
	float zFar_ = im_render->zFar;

	cv::Mat k_m( 3, 3, CV_32F, _k );
	std::vector<cv::Point2f> out;
	int nV = shape.rows;
	if (visible == 0) visible = new bool[shape.rows];
	cv::Mat rVec( 3, 1, CV_32F, r );
	cv::Mat tVec( 3, 1, CV_32F, t );
	cv::Mat rMat;
	cv::Rodrigues(rVec, rMat);
	cv::Mat new3D = rMat * shape.t() + cv::repeat(tVec,1,nV);

	for (int i=0;i<nV;i++){
		visible[i] = false;
		float Z = new3D.at<float>(2,i);
		float x = -new3D.at<float>(0,i)/Z*_k[4] + _k[2];
		float y = new3D.at<float>(1,i)/Z*_k[4] + _k[5];
		out.push_back(cv::Point2f(x,y));
		if (x > 0 && y > 0 & x < refDepth.cols-1 && y <refDepth.rows-1) {
			for (int dx =-1;dx<2;dx++){
				for (int dy =-1;dy<2;dy++){
					float dd = refDepth.at<float>(y+dy,x+dx);
					dd = - zNear_*zFar_   / ( zFar_ - dd * ( zFar_ - zNear_ ));
					if (fabs(Z - dd) < 2.5){
						visible[i] = true;
					}
				}
			}
		}
	}

	return out;
}

float FaceServices2::updateHessianMatrix(bool part, cv::Mat alpha, float* renderParams, cv::Mat faces, cv::Mat colorIm,std::vector<int> lmInds, cv::Mat landIm, BFMParams &params, cv::Mat &prevR, cv::Mat &prevT, cv::Mat exprW ){
	int M = alpha.rows;
	int EM = exprW.rows;
	float step;
	cv::Mat k_m( 3, 3, CV_32F, _k );
	cv::Mat distCoef = cv::Mat::zeros( 1, 4, CV_32F );
	params.hessDiag.release();
	params.hessDiag = cv::Mat::zeros(2*M+EM+RENDER_PARAMS_COUNT,1,CV_32F);
	//printf("sno_step --------\n");
	//std::cout << "alpha " << alpha.t() <<std::endl;
	//std::cout << "beta " << beta.t() <<std::endl;
	//printf("renderParams ");
	//for (int i=0;i<RENDER_PARAMS_COUNT;i++)
	//	printf("(%d) %f, ",i,renderParams[i]);
	//printf("\n");
	cv::Mat alpha2, expr2;
	float renderParams2[RENDER_PARAMS_COUNT];

	cv::Mat rVec(3,1,CV_32F,renderParams+RENDER_PARAMS_R);
	cv::Mat tVec(3,1,CV_32F,renderParams+RENDER_PARAMS_T);
	cv::Mat rVec2(3,1,CV_32F,renderParams2+RENDER_PARAMS_R);
	cv::Mat tVec2(3,1,CV_32F,renderParams2+RENDER_PARAMS_T);

	float currEF = eF(part, alpha, lmInds, landIm, renderParams, exprW);
	//printf("currF: %f\n",currEF);
	cEF = currEF;

	// expr
	step = mstep*5;
	if (params.optimizeExpr) {
		for (int i=0;i<EM; i++){
			expr2.release(); expr2 = exprW.clone();
			expr2.at<float>(i,0) += step;
			float tmpEF1 = eF(part, alpha, lmInds, landIm, renderParams,expr2);
			expr2.at<float>(i,0) -= 2*step;
			float tmpEF2 = eF(part, alpha, lmInds, landIm, renderParams,expr2);
			params.hessDiag.at<float>(2*M+i,0) = params.sF[FEATURES_LANDMARK] * (tmpEF1 - 2*currEF + tmpEF2)/(step*step)
				+ params.sExpr * 2/(0.25f*29) ;
		}
	}
	// r
	//step = 0.05;
	step = mstep*2;
	//step = 0.02;
	//step = 0.01;
	if (params.doOptimize[RENDER_PARAMS_R]) {
		for (int i=0;i<3; i++){
			memcpy(renderParams2,renderParams,RENDER_PARAMS_COUNT*sizeof(float));
			renderParams2[RENDER_PARAMS_R+i] += step;
			float tmpEF1 = eF(part, alpha, lmInds, landIm, renderParams2,exprW);

			renderParams2[RENDER_PARAMS_R+i] -= 2*step;
			float tmpEF2 = eF(part, alpha, lmInds, landIm, renderParams2,exprW);
			params.hessDiag.at<float>(2*M+EM+i,0) = params.sF[FEATURES_LANDMARK] * (tmpEF1 - 2*currEF + tmpEF2)/(step*step) + 2.0f/params.sR[RENDER_PARAMS_R+i];

		}
	}
	// t
	step = mstep*10;
	//step = 0.05;
	//step = 0.1;
	if (params.doOptimize[RENDER_PARAMS_T]) {
		for (int i=0;i<3; i++){
			memcpy(renderParams2,renderParams,RENDER_PARAMS_COUNT*sizeof(float));
			renderParams2[RENDER_PARAMS_T+i] += step;
			float tmpEF1 = eF(part, alpha, lmInds, landIm, renderParams2,exprW);
			renderParams2[RENDER_PARAMS_T+i] -= 2*step;
			float tmpEF2 = eF(part, alpha, lmInds, landIm, renderParams2,exprW);
			params.hessDiag.at<float>(2*M+EM+RENDER_PARAMS_T+i,0) = params.sF[FEATURES_LANDMARK] * (tmpEF1 - 2*currEF + tmpEF2)/(step*step)
				+ 2.0f/params.sR[RENDER_PARAMS_T+i];
		}
	}
	return 0;
}

FaceServices2::~FaceServices2(void)
{
}

cv::Mat FaceServices2::computeGradient(bool part, cv::Mat alpha, float* renderParams, cv::Mat faces,cv::Mat colorIm, std::vector<int> lmInds, cv::Mat landIm, BFMParams &params, std::vector<int> &inds, cv::Mat exprW, cv::Mat &prevR, cv::Mat &prevT){
	int M = alpha.rows;
	int EM = exprW.rows;
	int nTri = 40;
	float step;
	cv::Mat k_m( 3, 3, CV_32F, _k );
	cv::Mat distCoef = cv::Mat::zeros( 1, 4, CV_32F );
	cv::Mat out(2*M+EM+RENDER_PARAMS_COUNT,1,CV_32F);

	cv::Mat alpha2, expr2;
	float renderParams2[RENDER_PARAMS_COUNT];
	cv::Mat rVec(3,1,CV_32F,renderParams+RENDER_PARAMS_R);
	cv::Mat tVec(3,1,CV_32F,renderParams+RENDER_PARAMS_T);
	cv::Mat rVec2(3,1,CV_32F,renderParams2+RENDER_PARAMS_R);
	cv::Mat tVec2(3,1,CV_32F,renderParams2+RENDER_PARAMS_T);

	float currEF = eF(part, alpha, lmInds, landIm, renderParams,exprW);
	cEF = currEF;

	#pragma omp parallel for
	for (int target=0;target<EM+6; target++){
	  if (target < EM) {
		// expr
		float step = mstep*5;
		if (params.optimizeExpr) {
				int i = target;
				std::vector<cv::Point2f> pPoints;
				cv::Mat expr2 = exprW.clone();
				expr2.at<float>(i,0) += step;
				float tmpEF = eF(part, alpha, lmInds, landIm, renderParams,expr2);
				out.at<float>(2*M+i,0) = params.sF[FEATURES_LANDMARK] * (tmpEF - currEF)/step
					+ params.sExpr * 2*exprW.at<float>(i,0)/(0.25f*29);
				//if (cEF > prevEF) printf("%f, ",tmpEF);
		}
	   }
	   else if (target < EM+3) {
		// r
		//step = 0.05;
		//step = 0.01;
		float step = mstep*2;
		//step = 0.01;
		if (params.doOptimize[RENDER_PARAMS_R]) {
			int i = target-EM;
				float renderParams2[RENDER_PARAMS_COUNT];
				memcpy(renderParams2,renderParams,RENDER_PARAMS_COUNT*sizeof(float));
				renderParams2[RENDER_PARAMS_R+i] += step;
				float tmpEF = eF(part, alpha, lmInds, landIm, renderParams2,exprW);
				out.at<float>(2*M+EM+i,0) = params.sF[FEATURES_LANDMARK] * (tmpEF - currEF)/step;
				if (prevR.rows == 0)
					out.at<float>(2*M+EM+i,0) += 2*(renderParams[RENDER_PARAMS_R+i] - params.initR[RENDER_PARAMS_R+i])/params.sR[RENDER_PARAMS_R+i];
				else {
					float val = renderParams[RENDER_PARAMS_R+i] - prevR.at<float>(i,0);
					if (val > PREV_USE_THRESH)
						out.at<float>(2*M+EM+i,0) += 2*(renderParams[RENDER_PARAMS_R+i] - params.initR[RENDER_PARAMS_R+i])/params.sR[RENDER_PARAMS_R+i];
					else {
						//printf("usePrev %f (%d %d)\n",prevR.at<float>(i,0),REG_FROM_CURR,REG_FROM_PREV);
						out.at<float>(2*M+EM+i,0) += 2*REG_FROM_CURR*(renderParams[RENDER_PARAMS_R+i] - params.initR[RENDER_PARAMS_R+i])/params.sR[RENDER_PARAMS_R+i] + 2*REG_FROM_PREV*val/params.sR[RENDER_PARAMS_R+i];
					}
				}
				//if (cEF > prevEF) printf("%f, ",tmpEF);
		}
	  }
	  else {
		// t
		float step = mstep*10;
		//step = 1;
		if (params.doOptimize[RENDER_PARAMS_T]) {
			int i = target-EM-3;
				float renderParams2[RENDER_PARAMS_COUNT];
				memcpy(renderParams2,renderParams,RENDER_PARAMS_COUNT*sizeof(float));
				renderParams2[RENDER_PARAMS_T+i] += step;
				float tmpEF = eF(part, alpha, lmInds, landIm, renderParams2,exprW);
				out.at<float>(2*M+EM+RENDER_PARAMS_T+i,0) = params.sF[FEATURES_LANDMARK] * (tmpEF - currEF)/step
					+ 2*(renderParams[RENDER_PARAMS_T+i] - params.initR[RENDER_PARAMS_T+i])/params.sR[RENDER_PARAMS_T+i];
				//if (cEF > prevEF) printf("%f, ",tmpEF);
		}
	  }
	}
	return out;
}

float FaceServices2::eF(bool part, cv::Mat alpha, std::vector<int> inds, cv::Mat landIm, float* renderParams, cv::Mat exprW){
	Mat k_m(3,3,CV_32F,_k);
	//printf("%f\n",renderParams[RENDER_PARAMS_R+1]);
	cv::Mat mLM;
	if (!part)
		mLM = festimator.getLMByAlpha(alpha,-renderParams[RENDER_PARAMS_R+1], inds, exprW);
	else
		mLM = festimator.getLMByAlphaParts(alpha,-renderParams[RENDER_PARAMS_R+1], inds, exprW);
	//write_plyFloat("vismLM.ply",mLM.t());
	//printf("inds\n");
	//for (int i=0;i<inds.size(); i++)
//		printf("%d\n", inds[i]);
	//printf("renderParams %f %f %f   %f %f %f\n", renderParams[0],renderParams[1],renderParams[2],renderParams[3],renderParams[4],renderParams[5]);
	//std::cout << "landIm " << landIm << std::endl;
	//cv::Mat ads = cv::Mat::zeros(720,540,CV_8UC3);

	cv::Mat rVec(3,1,CV_32F, renderParams + RENDER_PARAMS_R);
	cv::Mat tVec(3,1,CV_32F, renderParams + RENDER_PARAMS_T);
	std::vector<cv::Point2f> allImgPts;
	cv::Mat distCoef = cv::Mat::zeros( 1, 4, CV_32F );

	cv::projectPoints( mLM, rVec, tVec, k_m, distCoef, allImgPts );
	float err = 0;
	for (int i=0;i<mLM.rows;i++){
		float val = landIm.at<float>(i,0) - allImgPts[i].x;
		err += val*val;
		val = landIm.at<float>(i,1) - allImgPts[i].y;
		err += val*val;
		//cv::circle(ads,cv::Point(landIm.at<float>(i,0),landIm.at<float>(i,1)),2,cv::Scalar(255,0,0));
		//cv::circle(ads,cv::Point(allImgPts[i].x,allImgPts[i].y),2,cv::Scalar(0,0,255));
	}
	//imwrite("sas.png",ads);
	//getchar();
	return sqrt(err/mLM.rows);
}

void FaceServices2::sno_step2(bool part, cv::Mat &alpha, float* renderParams, cv::Mat faces,cv::Mat colorIm, std::vector<int> lmInds, cv::Mat landIm, BFMParams &params, cv::Mat &exprW, cv::Mat &prevR, cv::Mat &prevT){
	//printf("sno_step --------\n");
	//std::cout << "alpha " << alpha.t() <<std::endl;
	//std::cout << "beta " << beta.t() <<std::endl;
	//printf("renderParams ");
	//for (int i=0;i<RENDER_PARAMS_COUNT;i++)
	//	printf("%f, ",renderParams[i]);
	//printf("\n");
	float lambda = 0.05;
	std::vector<int> inds;
	cv::Mat dE = computeGradient(part, alpha, renderParams, faces, colorIm, lmInds, landIm, params,inds, exprW, prevR, prevT);
	params.gradVec.release(); params.gradVec = dE.clone();
	cv::Mat dirMove = dE*0;

	int M = alpha.rows;
	int EM = exprW.rows;
	if (params.optimizeExpr){
		for (int i=0;i<EM;i++)
			if (abs(params.hessDiag.at<float>(2*M+i,0)) > 0.0000001) {
				dirMove.at<float>(2*M+i,0) = - lambda*dE.at<float>(2*M+i,0)/abs(params.hessDiag.at<float>(2*M+i,0));
			}
	}

	for (int i=0;i<RENDER_PARAMS_COUNT;i++) {
		if (params.doOptimize[i]){
			if (abs(params.hessDiag.at<float>(2*M+EM+i,0)) > 0.0000001) {
				dirMove.at<float>(2*M+EM+i,0) = - lambda*dE.at<float>(2*M+EM+i,0)/abs(params.hessDiag.at<float>(2*M+EM+i,0));
			}
		}
	}
	//float pc = 1;
	float pc = line_search(part, alpha, renderParams, dirMove,inds, faces, colorIm, lmInds, landIm, params, exprW, prevR, prevT, 10);
	//printf("pc = %f\n",pc);
	if (pc == 0) countFail++;
	else {
		if (params.optimizeExpr){
			for (int i=0;i<EM;i++) {
				exprW.at<float>(i,0) += pc*dirMove.at<float>(i+2*M,0);
				if (exprW.at<float>(i,0) > 3) exprW.at<float>(i,0) = 3;
				else if (exprW.at<float>(i,0) < -3) exprW.at<float>(i,0) = -3;
			}
		}

		for (int i=0;i<RENDER_PARAMS_COUNT;i++) {
			if (params.doOptimize[i]){
				renderParams[i] += pc*dirMove.at<float>(2*M+EM+i,0);
				if (i == RENDER_PARAMS_CONTRAST || (i>=RENDER_PARAMS_AMBIENT && i<RENDER_PARAMS_DIFFUSE+3) ) {
					if (renderParams[i] > 1.0) renderParams[i] = 1.0;
					if (renderParams[i] < 0) renderParams[i] = 0;

				}
				else if (i >= RENDER_PARAMS_GAIN && i<=RENDER_PARAMS_GAIN+3) {
					if (renderParams[i] > 3.0) renderParams[i]  = 3;
					if (renderParams[i] < 0.3) renderParams[i] = 0.3;
				}
			}
		}
	}
	prevEF = cEF;
}

float FaceServices2::line_search(bool part, cv::Mat &alpha, float* renderParams, cv::Mat &dirMove,std::vector<int> inds, cv::Mat faces,cv::Mat colorIm,std::vector<int> lmInds, cv::Mat landIm, BFMParams &params, cv::Mat &exprW, cv::Mat &prevR, cv::Mat &prevT, int maxIters){
	float step = 1.0f;
	float sstep = 2.0f;
	float minStep = 0.0001f;
	//float cCost = computeCost(cEF, cEI,cETE, cECE, alpha, beta, renderParams,params);
	cv::Mat alpha2, exprW2;
	float renderParams2[RENDER_PARAMS_COUNT];
	alpha2 = alpha.clone();
	exprW2 = exprW.clone();
	memcpy(renderParams2,renderParams,sizeof(float)*RENDER_PARAMS_COUNT);

	cv::Mat k_m( 3, 3, CV_32F, _k );
	cv::Mat distCoef = cv::Mat::zeros( 1, 4, CV_32F );
	std::vector<cv::Point2f> pPoints;
	cv::Mat rVec2(3,1,CV_32F,renderParams2+RENDER_PARAMS_R);
	cv::Mat tVec2(3,1,CV_32F,renderParams2+RENDER_PARAMS_T);

	int M = alpha.rows;
	int EM = exprW.rows;
	float ssize = 0;
	for (int i=0;i<dirMove.rows;i++) ssize += dirMove.at<float>(i,0)*dirMove.at<float>(i,0);
	ssize = sqrt(ssize);
	//printf("ssize: %f\n",ssize);
	if (ssize > (2*M+EM+RENDER_PARAMS_COUNT)/5.0f) {
		step = (2*M+EM+RENDER_PARAMS_COUNT)/(5.0f * ssize);
		ssize = (2*M+EM+RENDER_PARAMS_COUNT)/5.0f;
	}
	if (ssize < minStep){
		return 0;
	}
	int tstep = floor(log(ssize/minStep));
	if (tstep < maxIters) maxIters = tstep;

	float curCost = computeCost(cEF, alpha, renderParams, params, exprW, prevR, prevT );

	bool hasNoBound = false;
	int iter = 0;
	for (; iter<maxIters; iter++){
		if (params.optimizeExpr){
			for (int i=0;i<EM;i++) {
				float tmp = exprW.at<float>(i,0) + step*dirMove.at<float>(2*M+i,0);
				if (tmp >= 3) exprW2.at<float>(i,0) = 3;
				else if (tmp <= -3) exprW2.at<float>(i,0) = -3;
				else {
					exprW2.at<float>(i,0) = tmp;
					hasNoBound = true;
				}
			}
		}

		for (int i=0;i<RENDER_PARAMS_COUNT;i++) {
			if (params.doOptimize[i]){
				float tmp = renderParams[i] + step*dirMove.at<float>(2*M+EM+i,0);
				if (i == RENDER_PARAMS_CONTRAST || (i>=RENDER_PARAMS_AMBIENT && i<RENDER_PARAMS_DIFFUSE+3) ) {
					if (tmp > 1.0) renderParams2[i] = 1.0f;
					else if (tmp < -1.0) renderParams2[i] = -1.0f;
					else {
						renderParams2[i] = tmp;
						hasNoBound = true;
					}
				}
				else if (i >= RENDER_PARAMS_GAIN && i<=RENDER_PARAMS_GAIN+3) {
					if (tmp >= 3.0) renderParams2[i] = 3.0f;
					else if (tmp <= -3.0) renderParams2[i] = -3.0f;
					else {
						renderParams2[i] = tmp;
						hasNoBound = true;
					}
				}
				else renderParams2[i] = tmp;
			}
		}
		if (!hasNoBound) {
			iter = maxIters; break;
		}
		float tmpEF = cEF;
		if (params.sF[FEATURES_LANDMARK] > 0) tmpEF = eF(part,alpha2, lmInds,landIm,renderParams2, exprW2);
		float tmpCost = computeCost(tmpEF, alpha2, renderParams2, params,exprW2, prevR, prevT);
		//printf("tmpCost %f\n",tmpCost);
		if (tmpCost < curCost) {
			break;
		}
		else {
			step = step/sstep;
			//printf("step %f\n",step);
		}
	}
	//getchar();
	if (iter >= maxIters) return 0;
	else return step;
}

float FaceServices2::computeCost(float vEF, cv::Mat &alpha, float* renderParams, BFMParams &params, cv::Mat &exprW, cv::Mat &prevR, cv::Mat &prevT ){
	float val = params.sF[FEATURES_LANDMARK]*vEF;
	int M = alpha.rows;
	if (params.optimizeExpr){
		for (int i=0;i<exprW.rows;i++)
			val += params.sExpr * exprW.at<float>(i,0)*exprW.at<float>(i,0)/(0.5f*29);
	}

	for (int i=0;i<3;i++) {
		if (params.doOptimize[i]){
			if (prevR.rows == 0) val += (renderParams[i] - params.initR[i])*(renderParams[i] - params.initR[i])/params.sR[i];
			else {
				float vval = renderParams[i] - prevR.at<float>(i,0);
				if (vval > PREV_USE_THRESH)
					val += (renderParams[i] - params.initR[i])*(renderParams[i] - params.initR[i])/params.sR[i];
				else
					val += REG_FROM_CURR*(renderParams[i] - params.initR[i])*(renderParams[i] - params.initR[i])/params.sR[i] + REG_FROM_PREV*vval*vval/params.sR[i];
			}
		}
	}
	for (int i=3;i<RENDER_PARAMS_COUNT;i++) {
		if (params.doOptimize[i]){
			val += (renderParams[i] - params.initR[i])*(renderParams[i] - params.initR[i])/params.sR[i];
		}
	}
	return val;
}

bool FaceServices2::loadReference(string refDir, string model_file, cv::Mat &alpha, cv::Mat &beta, float* renderParams, int &M, cv::Mat &exprW, int &EM){
	string fname(model_file);
	size_t sep = model_file.find_last_of("\\/");
	if (sep != std::string::npos)
		fname = model_file.substr(sep + 1, model_file.size() - sep - 1);
	fname = refDir + fname;

	alpha = cv::Mat::zeros(M,1,CV_32F);
	beta = cv::Mat::zeros(M,1,CV_32F);
	char fpath[250];
	char text[250];
	sprintf(fpath,"%s.alpha",fname.c_str());
	//printf("fpath %s\n",fpath);
	FILE* f = fopen(fpath,"r");
	if (f == 0) return false;

	text[0] = '\0';
	for (int i=0;i<M;i++){
		fgets(text,250,f);
		int l = strlen(text);
		if (text[l-1] <'0' || text[l-1] > '9') text[l-1] = '\0';
		alpha.at<float>(i,0) = atof(text);
	}
	fclose(f);

	sprintf(fpath,"%s.beta",fname.c_str());
	f = fopen(fpath,"r");
	text[0] = '\0';
	for (int i=0;i<M;i++){
		fgets(text,250,f);
		int l = strlen(text);
		if (text[l-1] <'0' || text[l-1] > '9') text[l-1] = '\0';
		beta.at<float>(i,0) = atof(text);
	}
	fclose(f);

	sprintf(fpath,"%s.rend",fname.c_str());
	f = fopen(fpath,"r");
	text[0] = '\0';
	for (int i=0;i<RENDER_PARAMS_COUNT;i++){
		fgets(text,250,f);
		int l = strlen(text);
		if (text[l-1] <'0' || text[l-1] > '9') text[l-1] = '\0';
		renderParams[i] = atof(text);
	}
	fclose(f);

	sprintf(fpath,"%s.expr",fname.c_str());
	//printf("fpath %s\n",fpath);
	f = fopen(fpath,"r");
	if (f == 0) return false;

	text[0] = '\0';
	for (int i=0;i<EM;i++){
		fgets(text,250,f);
		int l = strlen(text);
		if (text[l-1] <'0' || text[l-1] > '9') text[l-1] = '\0';
		exprW.at<float>(i,0) = atof(text);
	}
	fclose(f);
	return true;
}

bool FaceServices2::loadReference2(string refDir, string model_file, cv::Mat &alpha, cv::Mat &beta, int &M){
	string fname(model_file);
	size_t sep = model_file.find_last_of("\\/");
	if (sep != std::string::npos)
		fname = model_file.substr(sep + 1, model_file.size() - sep - 1);
	fname = refDir + fname;

	alpha = cv::Mat::zeros(M,1,CV_32F);
	beta = cv::Mat::zeros(M,1,CV_32F);
	char fpath[250];
	char text[250];
	sprintf(fpath,"%s.alpha",fname.c_str());
	//printf("fpath %s\n",fpath);
	FILE* f = fopen(fpath,"r");
	if (f == 0) return false;

	text[0] = '\0';
	for (int i=0;i<M;i++){
		fgets(text,250,f);
		int l = strlen(text);
		if (text[l-1] <'0' || text[l-1] > '9') text[l-1] = '\0';
		alpha.at<float>(i,0) = atof(text);
	}
	fclose(f);

	sprintf(fpath,"%s.beta",fname.c_str());
	f = fopen(fpath,"r");
	text[0] = '\0';
	for (int i=0;i<M;i++){
		fgets(text,250,f);
		int l = strlen(text);
		if (text[l-1] <'0' || text[l-1] > '9') text[l-1] = '\0';
		beta.at<float>(i,0) = atof(text);
	}
	fclose(f);
	return true;
}
void FaceServices2::initRenderer(cv::Mat &colorIm){
    cv::Mat faces = festimator.getFaces() - 1;
    cv::Mat faces_fill = festimator.getFaces_fill() - 1;
    cv::Mat colors;
    cv::Mat shape = festimator.getShape(cv::Mat::zeros(1,1,CV_32F));
    cv::Mat tex = festimator.getTexture(cv::Mat::zeros(1,1,CV_32F));

    im_render = new FImRenderer(cv::Mat::zeros(colorIm.rows,colorIm.cols,CV_8UC3));
    im_render->loadMesh(shape,tex,faces_fill);
}

void FaceServices2::mergeIm(cv::Mat* output,cv::Mat bg,cv::Mat depth){
	for (int i=0;i<bg.rows;i++){
		for (int j=0;j<bg.cols;j++){
			if (depth.at<float>(i,j) >= 0.9999)
				output->at<Vec3b>(i, j) = bg.at<Vec3b>(i,j);
		}
	}
}

bool FaceServices2::estimatePoseExpr(cv::Mat colorIm, cv::Mat lms, cv::Mat alpha, cv::Mat &vecR, cv::Mat &vecT, cv::Mat& K, cv::Mat &exprW, const char* outputDir, bool with_expr){
	char text[200];
	float renderParams[RENDER_PARAMS_COUNT];
	float renderParams2[RENDER_PARAMS_COUNT];
	Mat k_m(3,3,CV_32F,_k);
    K = k_m.clone();    // Yuval
	BFMParams params;
	params.init();
	exprW = cv::Mat::zeros(29,1,CV_32F);
	cv::Mat prevR;
	cv::Mat prevT;

	int M = 99;
	cv::Mat shape = festimator.getShape(alpha);

	Mat landModel0 = festimator.getLM(shape,0);
	int nLM = landModel0.rows;
	Mat landIm = cv::Mat( 60,2,CV_32F);
	Mat landModel = cv::Mat( 60,3,CV_32F);
	for (int i=0;i<60;i++){
		landModel.at<float>(i,0) = landModel0.at<float>(i,0);
		landModel.at<float>(i,1) = landModel0.at<float>(i,1);
		landModel.at<float>(i,2) = landModel0.at<float>(i,2);
		landIm.at<float>(i,0) = lms.at<float>(i,0);
		landIm.at<float>(i,1) = lms.at<float>(i,1);
	}
	festimator.estimatePose3D0(landModel,landIm,k_m,vecR,vecT);
	float yaw = -vecR.at<float>(1,0);
	landModel0 = festimator.getLM(shape,yaw);
	std::vector<int> lmVisInd;
	for (int i=0;i<60;i++){
		if ((yaw > M_PI/10 && i > 7) || (yaw < -M_PI/10 && i < 9) || i > 16 || abs(yaw) <= M_PI/10)
			lmVisInd.push_back(i);
	}
	cv::Mat tmpIm = colorIm/5;
	landModel = cv::Mat( lmVisInd.size(),3,CV_32F);
	landIm = cv::Mat::zeros( lmVisInd.size(),2,CV_32F);
	for (int i=0;i<lmVisInd.size();i++){
		int ind = lmVisInd[i];
		landModel.at<float>(i,0) = landModel0.at<float>(ind,0);
		landModel.at<float>(i,1) = landModel0.at<float>(ind,1);
		landModel.at<float>(i,2) = landModel0.at<float>(ind,2);
		landIm.at<float>(i,0) = lms.at<float>(ind,0);
		landIm.at<float>(i,1) = lms.at<float>(ind,1);
		//cv::circle(tmpIm,Point(landIm.at<float>(i,0),landIm.at<float>(i,1)),2,Scalar(255,0,0),2);
	}
	//sprintf(text,"%s/withLM.png",outputDir);
	//imwrite(text,tmpIm);
	//sprintf(text,"%s/%s_withLM.ply",outputDir,filename.c_str());
	//write_plyFloat(text,landModel.t());
	//getchar();

	festimator.estimatePose3D0(landModel,landIm,k_m,vecR,vecT);

    // Yuval
    if (!with_expr) return true;

	for (int i=0;i<3;i++)
		params.initR[RENDER_PARAMS_R+i] = vecR.at<float>(i,0);
	for (int i=0;i<3;i++)
		params.initR[RENDER_PARAMS_T+i] = vecT.at<float>(i,0);
	memcpy(renderParams,params.initR,sizeof(float)*RENDER_PARAMS_COUNT);

	for (int i=60;i<68;i++) lmVisInd.push_back(i);
	landIm = cv::Mat::zeros( lmVisInd.size(),2,CV_32F);
	for (int i=0;i<lmVisInd.size();i++){
		int ind = lmVisInd[i];
		landIm.at<float>(i,0) = lms.at<float>(ind,0);
		landIm.at<float>(i,1) = lms.at<float>(ind,1);
	}

	float bCost, cCost, fCost;
	int bestIter = 0;
	bCost = 10000.0f;

	memset(params.sF,0,sizeof(float)*NUM_EXTRA_FEATURES);

	params.sI = 0.0;
	params.sF[FEATURES_LANDMARK] = 8.0f;
	Mat alpha0;
	int iter=0;
	int badCount = 0;
	memset(params.doOptimize,true,sizeof(bool)*6);

	int EM = 29;
	float renderParams_tmp[RENDER_PARAMS_COUNT];

	params.optimizeAB[0] = params.optimizeAB[1] = false;
	for (;iter<60;iter++) {
			if (iter%20 == 0) {
				cCost = updateHessianMatrix(false, alpha,renderParams,faces,colorIm,lmVisInd,landIm,params, prevR, prevT, exprW);
				if (countFail > 10) {
					countFail = 0;
					break;
				}
				prevEF = cEF;
			}
			sno_step2(false, alpha, renderParams, faces,colorIm,lmVisInd,landIm,params,exprW,prevR, prevT);
		}
	iter = 60;
	memset(params.doOptimize,false,sizeof(bool)*6);countFail = 0;
    //int total_iter = 200;
    int total_iter = 400;   // Yuval
	for (;iter<total_iter;iter++) {
			if (iter%60 == 0) {
				cCost = updateHessianMatrix(false, alpha,renderParams,faces,colorIm,lmVisInd,landIm,params, prevR, prevT, exprW);
				if (countFail > 10) {
					countFail = 0;
					break;
				}
				prevEF = cEF;
			}
			sno_step2(false, alpha, renderParams, faces,colorIm,lmVisInd,landIm,params,exprW,prevR, prevT);
		}

	///for (int i=0;i<3; i++) vecR.at<float>(i,0) = renderParams[i];
	///for (int i=0;i<3; i++) vecT.at<float>(i,0) = renderParams[i+3];
	//std::cout << "vecR " << vecR << std::endl;
	//std::cout << "vecT " << vecT << std::endl;
	//std::cout << "exprW " << exprW << std::endl;

    return true;
}

bool FaceServices2::updatePoseExpr(cv::Mat colorIm, cv::Mat lms, cv::Mat alpha, cv::Mat &vecR, cv::Mat &vecT, cv::Mat &exprW, char* outputDir, cv::Mat &prevR, cv::Mat &prevT )
{
	char text[200];
	float renderParams[RENDER_PARAMS_COUNT];
	float renderParams2[RENDER_PARAMS_COUNT];
	Mat k_m(3,3,CV_32F,_k);
	BFMParams params;
	params.init();

	int M = 99;
	float yaw = -vecR.at<float>(1,0);
	Mat landModel0 = festimator.getLM(shape,yaw);
	std::vector<int> lmVisInd;
	for (int i=0;i<60;i++){
		if (i > 16 || std::abs(yaw) <= M_PI/10 || (yaw > M_PI/10 && i > 7) || (yaw < -M_PI/10 && i < 9))
			lmVisInd.push_back(i);
	}
	cv::Mat landModel = cv::Mat( lmVisInd.size(),3,CV_32F);
	cv::Mat landIm = cv::Mat::zeros( lmVisInd.size(),2,CV_32F);
	for (int i=0;i<lmVisInd.size();i++){
		int ind = lmVisInd[i];
		landModel.at<float>(i,0) = landModel0.at<float>(ind,0);
		landModel.at<float>(i,1) = landModel0.at<float>(ind,1);
		landModel.at<float>(i,2) = landModel0.at<float>(ind,2);
		landIm.at<float>(i,0) = lms.at<float>(ind,0);
		landIm.at<float>(i,1) = lms.at<float>(ind,1);
		//cv::circle(tmpIm,Point(landIm.at<float>(i,0),landIm.at<float>(i,1)),2,Scalar(255,0,0),2);
	}
	//sprintf(text,"%s/withLM.png",outputDir);
	//imwrite(text,tmpIm);
	//sprintf(text,"%s/%s_withLM.ply",outputDir,filename.c_str());
	//write_plyFloat(text,landModel.t());
	//getchar();

	festimator.estimatePose3D0(landModel,landIm,k_m,vecR,vecT);
	for (int i=60;i<68;i++) lmVisInd.push_back(i);

	for (int i=0;i<3;i++)
		params.initR[RENDER_PARAMS_R+i] = vecR.at<float>(i,0);
	for (int i=0;i<3;i++)
		params.initR[RENDER_PARAMS_T+i] = vecT.at<float>(i,0);
	memcpy(renderParams,params.initR,sizeof(float)*RENDER_PARAMS_COUNT);

	landIm = cv::Mat::zeros( lmVisInd.size(),2,CV_32F);
	for (int i=0;i<lmVisInd.size();i++){
		int ind = lmVisInd[i];
		landIm.at<float>(i,0) = lms.at<float>(ind,0);
		landIm.at<float>(i,1) = lms.at<float>(ind,1);
	}

	float bCost, cCost, fCost;
	int bestIter = 0;
	bCost = 10000.0f;

	memset(params.sF,0,sizeof(float)*NUM_EXTRA_FEATURES);

	params.sI = 0.0;
	params.sF[FEATURES_LANDMARK] = 8.0f;
	int iter=0;
	int badCount = 0;
	memset(params.doOptimize,true,sizeof(bool)*6);

	int EM = 29;
	float renderParams_tmp[RENDER_PARAMS_COUNT];

	params.optimizeAB[0] = params.optimizeAB[1] = false;

    for (; iter < 30; iter++)
    {
        if (iter % 10 == 0) {
            cCost = updateHessianMatrix(false, alpha, renderParams, faces, colorIm, lmVisInd, landIm, params, prevR, prevT, exprW);
            if (countFail > 10) {
                countFail = 0;

                break;
            }
            prevEF = cEF;
        }
        sno_step2(false, alpha, renderParams, faces, colorIm, lmVisInd, landIm, params, exprW, prevR, prevT);

    }

    return true;
}
cv::Mat FaceServices2::testRender(cv::Mat colorIm, cv::Mat alpha,cv::Mat vecR,cv::Mat vecT,cv::Mat exprW, char* outDir, bool updateColor){
	char text[200];
	int M = 99;
	float renderParams[RENDER_PARAMS_COUNT];
	for (int i =0;i<3;i++)
		renderParams[i] = vecR.at<float>(i,0);
	for (int i =0;i<3;i++)
		renderParams[i+3] = vecT.at<float>(i,0);

	////cv::Mat shape = festimator.getShape(alpha,exprW);
	//cv::Mat faces_fill = festimator.getFaces_fill() - 1;
	// Ambient
	//renderParams[6] = 0.29225;
	//renderParams[7] = 0.29225;
	//renderParams[8] = 0.29225;
	renderParams[6] = 0.49225;
	renderParams[7] = 0.49225;
	renderParams[8] = 0.49225;
	// Diffuse
	renderParams[9] = 0.50754;
	renderParams[10] = 0.50754;
	renderParams[11] = 0.50754;
	// LIGHT
	renderParams[12] = 3.1415/4;
	renderParams[13] = 3.1415/4;
	// OTHERS
	renderParams[14] = 1;
	renderParams[15] = renderParams[16] = renderParams[17] = RENDER_PARAMS_GAIN_DEFAULT;
	renderParams[18] = renderParams[19] = renderParams[20] = RENDER_PARAMS_OFFSET_DEFAULT;

	Mat k_m(3,3,CV_32F,_k);
	//RenderServices rs;
	float* r = renderParams + RENDER_PARAMS_R;
	float* t = renderParams + RENDER_PARAMS_T;
	//printf("t %f %f %f\n",t[0],t[1],t[2]);

		shape = festimator.getShape2(alpha,exprW);
	//if (updateColor) {
		cv::Mat colors;
		tex = shape*0 + 128;

		im_render->copyShape(shape);
		bool* visible = new bool[im_render->face_->mesh_.nVertices_];
		bool* noShadow = new bool[im_render->face_->mesh_.nVertices_];
		memset(noShadow,true,im_render->face_->mesh_.nVertices_*sizeof(bool));
		memset(visible,true,im_render->face_->mesh_.nVertices_*sizeof(bool));

		rs.estimateColor(shape,tex,faces,visible,noShadow,renderParams,colors);
		im_render->copyColors(colors);
		delete visible; delete noShadow;
		im_render->loadModel();
	//}
	cv::Mat refRGB = cv::Mat::zeros(colorIm.rows,colorIm.cols,CV_8UC3);
	cv::Mat refDepth = cv::Mat::zeros(colorIm.rows,colorIm.cols,CV_32F);
	im_render->render(r,t,_k[4],refRGB,refDepth);
	return refRGB;
}

void FaceServices2::testRenderWDepth(cv::Mat colorIm, cv::Mat alpha,cv::Mat vecR,cv::Mat vecT,cv::Mat exprW, cv::Mat &refRGB, cv::Mat &refDepth){
	char text[200];
	int M = 99;
	float renderParams[RENDER_PARAMS_COUNT];
	for (int i =0;i<3;i++)
		renderParams[i] = vecR.at<float>(i,0);
	for (int i =0;i<3;i++)
		renderParams[i+3] = vecT.at<float>(i,0);

	////cv::Mat shape = festimator.getShape(alpha,exprW);
	//cv::Mat faces_fill = festimator.getFaces_fill() - 1;
	// Ambient
	//renderParams[6] = 0.29225;
	//renderParams[7] = 0.29225;
	//renderParams[8] = 0.29225;
	renderParams[6] = 0.49225;
	renderParams[7] = 0.49225;
	renderParams[8] = 0.49225;
	// Diffuse
	renderParams[9] = 0.50754;
	renderParams[10] = 0.50754;
	renderParams[11] = 0.50754;
	// LIGHT
	renderParams[12] = 3.1415/4;
	renderParams[13] = 3.1415/4;
	// OTHERS
	renderParams[14] = 1;
	renderParams[15] = renderParams[16] = renderParams[17] = RENDER_PARAMS_GAIN_DEFAULT;
	renderParams[18] = renderParams[19] = renderParams[20] = RENDER_PARAMS_OFFSET_DEFAULT;

	Mat k_m(3,3,CV_32F,_k);
	//RenderServices rs;
	float* r = renderParams + RENDER_PARAMS_R;
	float* t = renderParams + RENDER_PARAMS_T;
	//printf("t %f %f %f\n",t[0],t[1],t[2]);

		shape = festimator.getShape2(alpha,exprW);
	//if (updateColor) {
		cv::Mat colors;
		tex = shape*0 + 128;

		im_render->copyShape(shape);
		bool* visible = new bool[im_render->face_->mesh_.nVertices_];
		bool* noShadow = new bool[im_render->face_->mesh_.nVertices_];
		memset(noShadow,true,im_render->face_->mesh_.nVertices_*sizeof(bool));
		memset(visible,true,im_render->face_->mesh_.nVertices_*sizeof(bool));

		rs.estimateColor(shape,tex,faces,visible,noShadow,renderParams,colors);
		im_render->copyColors(colors);
		delete visible; delete noShadow;
		im_render->loadModel();
	//}
	refRGB = cv::Mat::zeros(colorIm.rows,colorIm.cols,CV_8UC3);
	refDepth = cv::Mat::zeros(colorIm.rows,colorIm.cols,CV_32F);
	im_render->render(r,t,_k[4],refRGB,refDepth);
}

void FaceServices2::nextMotion(int &currFrame, cv::Mat &vecR, cv::Mat &vecT, cv::Mat &exprWeights){
    float stepYaw = 1;
    float PPI = 3.141592;
    int maxYaw = 70/stepYaw;
    float stepPitch = 1;
    int maxPitch = 45/stepPitch;

    int totalFrames = 4 * (maxYaw + maxPitch);
    currFrame = (currFrame + 1) % totalFrames;
    vecR = vecR*0 + 0.00001;
    // Rotate left
    if (currFrame < maxYaw) vecR.at<float>(1,0) = -currFrame*stepYaw * PPI/180;
    else if (currFrame < 2*maxYaw) vecR.at<float>(1,0) = -stepYaw * (2*maxYaw - currFrame) * PPI/180;
    // Rotate right
    else if (currFrame < 3*maxYaw) vecR.at<float>(1,0) = (currFrame-2*maxYaw) * stepYaw * PPI/180;
    else if (currFrame < 4*maxYaw) vecR.at<float>(1,0) = stepYaw * (4*maxYaw - currFrame) * PPI/180;

    // Rotate up
    else if (currFrame < 4*maxYaw + maxPitch) vecR.at<float>(0,0) = -(currFrame-4*maxYaw)*stepPitch * PPI/180;
    else if (currFrame < 4*maxYaw + 2*maxPitch) vecR.at<float>(0,0) = -stepPitch * (4*maxYaw+2*maxPitch - currFrame) * PPI/180;
    // Rotate right
    else if (currFrame < 4*maxYaw + 3*maxPitch) vecR.at<float>(0,0) = (currFrame-4*maxYaw-2*maxPitch) * stepPitch * PPI/180;
    else if (currFrame < 4*maxYaw + 4*maxPitch) vecR.at<float>(0,0) = stepPitch * (4*maxPitch+4*maxYaw - currFrame) * PPI/180;
    //std::cout << "vecR " << vecR.t() << std::endl;
}


#include <boost/math/special_functions/sin_pi.hpp>


/*
 *  Contains the base class for IMU Plugins
 * imu_plugin.hpp
 *
 *  Created on: 24.07.2018
 *      Author: tomlucas
 */

#ifndef PLUGINS_IMU_PLUGIN_HPP_
#define PLUGINS_IMU_PLUGIN_HPP_
#define GRAVITY_CONSTANT 9.81282
#include <vector>
#include <chrono>
#include <thread>
#include <Eigen/Core>
#include <Eigen/Geometry>

#include "ZaVI_Utils.hpp"

#include "sensor_plugin.hpp"
namespace zavi
::plugin {

	/**
	 * Plugin base class for all plugins simulating or reading an IMU
	 */

	class IMU_Plugin: public SensorPlugin {
	public:
		static inline Eigen::Vector3d gravity=Eigen::Vector3d(0,0,GRAVITY_CONSTANT);     // Gravity vector
		/**
		 * Returns the acceleration in object frame
		 * @return  Vector with x,y,z acceleration
		 */
		virtual inline const Eigen::Vector3d & getAcceleration() const{
			return acceleration;
		}
		virtual bool isDataValid()const{
			return  data_valid;
		}

		/**
		 * Angular Velocity of Gyroscope
		 *
		 * @return  Vector with angular roll,pitch,yaw
		 */
		virtual inline const Eigen::Vector3d & getAngularVelocity() const{
			return angular_velocity;
		}

		/**
		 * Magnetometer values
		 *
		 * @return  Vector with magnetometer
		 */
		virtual inline const Eigen::Vector3d & getMagnetometer()const{
			return magnetometer;
		}

		/**
		 * Returns the complete rotation
		 *
		 * @return   imu_in_world as euler angles
		 */
		virtual inline const Eigen::Quaterniond & getOrientation() const{
			return imu_in_world;
		}

		/**
		 * Returns the covariance of acceleration
		 * @return Matrix covariance  of x,y,z acceleration
		 */
		virtual inline const Eigen::Matrix3d & getAccelerationCov() const{
			return acceleration_cov;
		}
		/**
		 *
		 * @return the covariances of the 6 biases for the acceleration
		 */
		virtual inline const Eigen::Matrix<double, 6, 6> & getAccelerationBiasCov()const {
			return acceleration_bias_cov;
		}

		/**
		 *
		 * @return the autocorrelation factors of the 6 biases
		 */
		virtual inline const Eigen::Matrix<double, 6, 1>& getAccelerationBiasCorr() const{
			return acceleration_bias_corr;
		}

		/**
		 * Returns the covariance of anuglar rate
		 * @return Matrix covariance  of x,y,z acceleration
		 */
		virtual inline const Eigen::Matrix3d  getAngularRateBiasCov() const{
			return angular_rate_bias_cov;
		}
		/**
		 *
		 * @return the correlation time off the angular rate biases
		 */
		virtual inline const Eigen::Vector3d & getAngularRateBiasCorr() const{
			return angular_rate_bias_corr;
		}

		/**
		 * Covariance of Angular Velocity of Gyroscope
		 *
		 * @return Matrix covariance  of angular roll,pitch,yaw
		 */
		virtual inline const Eigen::Matrix3d & getAngularVelocityCov() const{
			return angular_velocity_cov;
		}

		/**
		 * Covariance of Magnetometer values
		 *
		 * @return Matrix covariance  of magnetometer
		 */
		virtual inline const Eigen::Matrix3d & getMagnetometerCov() const{
			return magnetometer_cov;
		}

		/**
		 * Returns the covariance of complete rotation
		 *
		 * @return Matrix covariance  of imu_in_world
		 */
		virtual inline const Eigen::Matrix3d & getOrientationCov() const{
			return orientation_cov;
		}

		/**
		 * Virtual Desctructor for later use
		 */
		virtual ~IMU_Plugin() {

		}
		/**
		 * Does not need to construct anything
		 */
		IMU_Plugin():data_valid(true) {

		}

	protected:

		Eigen::Vector3d acceleration, 				//< acceleration in body frame
		angular_velocity,//< angular velocity
		magnetometer;//< magnetometer data

		Eigen::Matrix3d acceleration_cov,//< covariance of acceleration
		angular_velocity_cov,//< covariance of angular velocity
		magnetometer_cov,//< covariance of magnetometer
		orientation_cov;//< covariance of imu_in_world
		Eigen::Quaterniond imu_in_world;//<imu_in_world data
		Eigen::Matrix<double, 6, 6> acceleration_bias_cov;//covariance of the acceleration biases
		Eigen::Matrix<double, 6, 1> acceleration_bias_corr;// exponential correlation factors of biases
		Eigen::Matrix3d angular_rate_bias_cov; // covariance of the angular velocity biases
		Eigen::Vector3d angular_rate_bias_corr; // correlations of the angular rate biases
		bool data_valid;//!< this flag tells whether the current bit of data is valid or not  (outages for file_imu)
	};

}
//plugin::zavi

#endif /* PLUGINS_IMU_PLUGIN_HPP_ */


/*
 * Copyright (C) 2020 ZMP Inc info@zmp.co.jp
 *
 * Distributed under the MIT License (http://opensource.org/licenses/MIT)
 *
 * Written by Andrei Pak
 */
#include "rc110_behavior.hpp"

#include <ackermann_msgs/AckermannDriveStamped.h>
#include <sensor_msgs/point_cloud2_iterator.h>

#include <Eigen/Geometry>

namespace zmp
{
namespace
{
constexpr float DEG_TO_RAD = M_PI / 180;
constexpr float INF = std::numeric_limits<float>::infinity();

// coordinates relative to robot origin, i.e. center of rear wheels axis
const Eigen::AlignedBox2f nearBox = {Eigen::Vector2f(0.2, -0.15f), Eigen::Vector2f(0.5f, 0.15f)};
const Eigen::AlignedBox2f leftBox = {Eigen::Vector2f(0.5f, 0), Eigen::Vector2f(1.0f, 0.35f)};
const Eigen::AlignedBox2f rightBox = {Eigen::Vector2f(0.5f, -0.35f), Eigen::Vector2f(1.0f, 0)};
}  // namespace

Rc110Behavior::Rc110Behavior() :
        handle(),
        cloudSubscriber(handle.subscribe("points2", 1, &Rc110Behavior::onCloud, this)),
        drivePublisher(handle.advertise<ackermann_msgs::AckermannDriveStamped>("drive_ad", 1))
{
    ros::NodeHandle privateHandle("~");
    privateHandle.param("forward_command", forwardCommand, forwardCommand);
    privateHandle.param("left_command", leftCommand, leftCommand);
    privateHandle.param("right_command", rightCommand, rightCommand);
}

void Rc110Behavior::onCloud(const sensor_msgs::PointCloud2& cloud)
{
    float closestLeftX = INF;
    float closestRightX = INF;
    auto result = forwardCommand;

    using Iter = sensor_msgs::PointCloud2ConstIterator<float>;
    for (Iter it = {cloud, "x"}; it != it.end(); ++it) {
        float x = it[0], y = it[1];
        Eigen::Vector2f point = {x, y};

        if (nearBox.contains(point)) {
            result = stopCommand;
            break;
        } else if (leftBox.contains(point)) {
            closestLeftX = std::min(x, closestLeftX);
        } else if (rightBox.contains(point)) {
            closestRightX = std::min(x, closestRightX);
        }
    }

    if (result == forwardCommand) {
        if (closestLeftX != INF || closestRightX != INF) {
            result = closestLeftX < closestRightX ? rightCommand : leftCommand;
        }
    }
    if (result.size() != 2) {
        ROS_ERROR_STREAM("Command should have 2 values: speed and steering! Current number of values: " << result.size());
    }
    publishCommand(result[0], result[1]);
}

void Rc110Behavior::publishCommand(float speed, float steering)
{
    ackermann_msgs::AckermannDriveStamped message;
    message.drive.speed = speed;
    message.drive.steering_angle = steering * DEG_TO_RAD;

    drivePublisher.publish(message);
}

}  // namespace zmp