// Copyright (C) 2017 EDF
// All Rights Reserved
// This code is published under the GNU Lesser General Public License (GNU LGPL)
#include <vector>
#include <iomanip>
#include <memory>
#include <Eigen/Dense>
#include <iostream>

using namespace Eigen;
using namespace std;


namespace StOpt
{
inline double round(double d)
{
    return floor(d + 0.5);
}


void dimgrid(const int &p_nbSimul, const ArrayXd &p_Sing,
             const double &p_prop, const double   &p_q, int &p_dEff,
             ArrayXi   &p_aK, ArrayXi &p_aN)
{
    p_dEff =  p_Sing.size();
    double singProd =   p_Sing.prod();
    ArrayXd aN = pow(p_nbSimul * p_q / singProd, 1. / p_Sing.size()) * p_Sing;
    while (floor(aN(p_dEff - 1) + 0.5) < 2)
    {
        p_dEff -= 1;
        singProd = p_Sing.head(p_dEff).prod();
        aN.head(p_dEff) = pow(p_nbSimul * p_q / singProd, 1.0 / p_dEff) * p_Sing.head(p_dEff);
    }
    int p_dEff_p = p_dEff;
    ArrayXd aP = pow(p_prop * singProd, 1.0 / p_dEff) / p_Sing.head(p_dEff);
    while (aP(p_dEff_p - 1) > 1)
    {
        aP(p_dEff_p - 1) = 1.0;
        p_dEff_p -= 1;
        aP.head(p_dEff_p) = pow(p_prop * p_Sing.head(p_dEff_p).prod(), 1.0 / p_dEff_p) / p_Sing.head(p_dEff_p);
        if (p_dEff_p == 0)
            break;
    }
    p_aK = ArrayXi::Zero(p_dEff);
    for (int i = 0; i < p_dEff; ++i)
    {
        if (fabs(aP(i) - 1.0) < 1e-8)
            p_aK(i) = p_nbSimul;
        else
        {
            p_aK(i) = floor(p_nbSimul * aP(i));
            if ((p_aK(i) % 2) == 0)
                p_aK(i) += 1;
        }
    }
    p_aN = (aN.round()).cast<int>();
}

void   preprocessData(ArrayXXd &p_X,  const ArrayXd &p_sing, const double &p_prop,
                      const double &p_q, ArrayXi &p_aN, ArrayXi &p_aK, int &p_dEff)
{
    dimgrid(p_X.cols(), p_sing, p_prop, p_q, p_dEff, p_aK, p_aN);
    p_X.conservativeResize(p_dEff, p_X.cols());
}



ArrayXd  KNearestBandwithKPt1D(const ArrayXd &p_sx, const int &p_k)
{
    int n = p_sx.size();
    ArrayXd h = ArrayXd::Zero(n);
    double dxmax = p_sx(n - 1) - p_sx(0);
    if (p_k >= n)
    {
        h.setConstant(dxmax);
        return h;
    }
    int il = 0;
    int ir = p_k - 1;
    double cm = 0.5 * (p_sx(il) + p_sx(ir + 1));
    double dxl = dxmax;
    double dxr = p_sx(ir + 1) - p_sx(ir);
    for (int i = 0; i < n ; ++i)
    {
        while ((ir + 1 < n - 1) && (p_sx(i) > cm))
        {
            il += 1;
            ir += 1;
            cm = 0.5 * (p_sx(il) + p_sx(ir + 1));
            dxl = p_sx(il) - p_sx(il - 1);
            dxr = p_sx(ir + 1) - p_sx(ir);
        }
        if ((ir + 1 == n - 1) && (p_sx(i) > cm))
        {
            il += 1;
            ir += 1;
            dxl = p_sx(il) - p_sx(il - 1);
            dxr = dxmax;
        }
        double hmin = max(p_sx(i) - p_sx(il), p_sx(ir) - p_sx(i));
        double hmax = min(p_sx(i) - (p_sx(il) - dxl), (p_sx(ir) + dxr) - p_sx(i));
        h(i) = 0.5 * (hmin + hmax);
    }
    return h    ;
}



void   dataPartition1D(const ArrayXd &p_sx, const ArrayXd &p_z,
                       const ArrayXd &p_h,
                       ArrayXd &p_g, ArrayXi &p_zl, ArrayXi &p_zr)
{
    ArrayXd zMh = p_z - p_h;
    ArrayXd zPh = p_z + p_h;
    p_g.resize(2 * p_z.size());
    p_zl.resize(p_z.size());
    p_zr.resize(p_z.size());
    int iposL = 1;
    int iposR = 0;
    int iposG = 0;
    double gL = zMh(0);
    double gR = zMh(0); //zPh(0);
    int iposSx = 0;
    int iposSxPrev = 0;
    double gPrev = gL;
    ArrayXi iZL(p_z.size());
    ArrayXi iZR(p_z.size());
    ArrayXi iG(2 * p_z.size());
    int igPos = 0;
    iZL(0) = 0;
    int  iGUnion = 1;
    while ((iposR < p_z.size())) //&&(iposSx<p_sx.size()))
    {
        if (iposL < p_z.size())
        {
            if (zMh(iposL) < zPh(iposR))
            {
                iZL(iposL) = iGUnion++;
                gR = zMh(iposL++);

            }
            else
            {
                iZR(iposR) = iGUnion++;
                gR = zPh(iposR++);
            }
        }
        else
        {
            iZR(iposR) = iGUnion++;
            gR = zPh(iposR++);
        }
        iG(igPos++) = iposG;
        // check if admissible
        iposSxPrev = iposSx;
        if (iposSx < p_sx.size())
        {
            while (p_sx(iposSx) < gR)
            {
                iposSx++;
                if (iposSx == p_sx.size())
                    break ;
            }
            if (iposSx > iposSxPrev)
            {
                p_g(iposG++) = 0.5 * (gL + gPrev);
                gL = gR;
            }
        }

        gPrev = gR;
    }
    p_g(iposG) =  0.5 * (gL + gPrev);
    iG(igPos) = iposG;
    p_g.conservativeResize(iposG + 1);
    for (int i = 0; i < p_z.size(); ++i)
    {
        p_zl(i) = iG(iZL(i));
        p_zr(i) = iG(iZR(i));
    }

}


void adaptiveBandwithNd(const ArrayXXd  &p_sx, const ArrayXXi &p_iSort,
                        const ArrayXi &p_aN, const ArrayXi &p_aK,
                        vector< shared_ptr<ArrayXd> > &p_h,
                        vector< shared_ptr<ArrayXd> > &p_z,
                        vector< shared_ptr<ArrayXi> > &p_zl,
                        vector< shared_ptr<ArrayXi> > &p_zr,
                        vector<  shared_ptr<ArrayXd> > &p_g,
                        ArrayXXi &p_xG)
{
    p_h.resize(p_sx.rows());
    p_z.resize(p_sx.rows());
    p_zl.resize(p_sx.rows());
    p_zr.resize(p_sx.rows());
    p_g.resize(p_sx.rows());
    for (int id = 0; id < p_sx.rows(); ++id)
    {
        ArrayXd hFull = KNearestBandwithKPt1D(p_sx.row(id), p_aK(id));
        shared_ptr<ArrayXd> z = make_shared<ArrayXd>(p_aN(id));
        shared_ptr<ArrayXd> h = make_shared<ArrayXd>(p_aN(id));
        shared_ptr<ArrayXd> g = make_shared<ArrayXd>();
        shared_ptr<ArrayXi> zl = make_shared<ArrayXi>();
        shared_ptr<ArrayXi> zr = make_shared<ArrayXi>();
        for (int i = 0; i <  p_aN(id); ++i)
        {
            int idec = static_cast<int>(round(i * (p_sx.cols() - 1) / (p_aN(id) - 1)));
            (*z)(i) = p_sx(id, idec);
            (*h)(i) = hFull(idec);
        }
        dataPartition1D(p_sx.row(id), *z, *h, *g, *zl, *zr);
        int p_nG = g->size();
        // each particle to a mesh in G
        int ig = 0;
        for (int is = 0; is < p_sx.cols(); ++is)
        {
            while (((ig + 1) < p_nG) && ((*g)(ig + 1) < p_sx(id, is))) ig++;
            p_xG(id, p_iSort(id, is)) = ig;
        }
        p_h[id] = h;
        p_z[id] = z;
        p_zl[id] = zl;
        p_zr[id] = zr;
        p_g[id] = g;
    }

}

}
