#include "fitfunc.h"
#include "IndependentAlg.h"
#include "matlab_func.h"
#include "genenalfunc.h"
using namespace arma;

namespace Fcn {

mat l1interpolate(mat x, colvec para)
{
    mat y = zeros(size(x));     // y = zeros(size(x));
    int pl = para.size()-1;
    colvec xdata = MySpan(para, 0, pl, 2);  // xdata = para(1:2:end);
    colvec ydata = MySpan(para, 1, pl, 2);  // ydata = para(2:2:end);

    int l = xdata.size();               // l = length(xdata);

    uvec lowidx = find(x < xdata(0));       // lowidx = find(x < xdata(1));
    if(lowidx.size() > 0)                   // if any(lowidx)
    {
        // y(lowidx) = lExtrapolate(xdata(1:2), ydata(1:2), x(lowidx));
        y(lowidx) = lExtrapolate(xdata.rows(0, 1), ydata.rows(0, 1), x(lowidx));
    }

    uvec highidx = find(x > xdata(l-1));    // highidx = find(x > xdata(l));
    if(highidx.size() > 0)                  // if any(highidx)
    {
        // y(highidx) = lExtrapolate(xdata(l-1:l), ydata(l-1:l), x(highidx));
        y(highidx) = lExtrapolate(xdata.rows(l-2,l-1), ydata.rows(l-2,l-1), x(highidx));
    }

    uvec mididx = find(x >= xdata(0) && x <= xdata(l-1));
    if(mididx.size() > 0)                   // if any(mididx)
    {
        // y(mididx) = interp1(xdata, ydata, x(mididx));
        mat YI;
        interp1(xdata, ydata, x(mididx), YI);
        y(mididx) = YI;
    }
    return y;
}

mat l2polynomial(mat x, colvec para)
{
    // y = polyval(para(end:-1:1), x);
    return Matlab::polyval(MySpan(para, para.size()-1, 0), x);
}

mat l3sqrtpoly(mat x, colvec para)
{
    uvec k = find(x < 0);
    if(!k.is_empty())
    {
       MatAssign(x, k, 0);
    }
    //y = polyval(para(end:-1:1), sqrt(x));
    return Matlab::polyval(MySpan(para, para.size()-1, 0), sqrt(x));
}

mat l4sqrtofpoly(mat x, colvec para)
{
    mat y2 = Matlab::polyval(MySpan(para, para.size()-1, 0), x); // y2 = polyval(para(end:-1:1), x);
    uvec k = find(y2 < 0);  // k = find(y2 < 0);
    if (any(k))             // if any(k)
    {
       MatAssign(y2, k, 0); // y2(k) = 0;
    }
    return sqrt(y2);     // y = sqrt(y2);
}

mat l5hteff(mat x, colvec para)
{
    double A = para(0);
    double E1 = para(1);
    double E2 = para(2);
    double k = para(3);
    double n = para(4);

    //y = A*exp(-(E1./x).^k).*(1-exp(-(E2./x).^n));
    return A * exp(-pow(E1/x, k)) % (1-exp(-pow(E2/x,n)));
}

mat l6loglogpoly(mat x, colvec para)
{
    //y = exp(polyval(para(end:-1:1), log(x)));
    return exp(Matlab::polyval(MySpan(para, para.size()-1, 0), log(x)));
}

mat l7logpoly(mat x, colvec para)
{
    //y = exp(x.*polyval(para(end:-1:1), 1./x));
    return exp(x % Matlab::polyval(MySpan(para, para.size()-1, 0), 1.0/x));
}

mat l8loginvlog(mat x, colvec para)
{
    uword l = para.size();
    double c = para(l-1u);
    para.shed_row(l-1u);
    //y = exp(polyval(para(end:-1:1), log(c./x)));
    return exp(Matlab::polyval(MySpan(para, para.size()-1, 0), log(c/x)));
}

mat l9invexp(mat x, colvec para)
{
    double a = para(0);
    double b = para(1);
    double k = -para(2);
    double n = -para(3);

    //y = 1 ./ (a*x.^k + b*x.^n);
    return 1 / (a * pow(x, k) + b * pow(x, n));
}

mat l93ht2fix(mat x, colvec para)
{
    double EMax = max(vectorise(x)); //EMax = max(x);
    vec pnew = zeros(8u); //pnew = zeros(8, 1);

    //pnew([1,2,3,7,8]) = reshape(para, [5,1]);
    urowvec idx("0 1 2 6 7");
    pnew(idx) = reshape(para, 5, 1);

    //pnew([4,5,6]) = [EMax; EMax; 1];
    pnew(3) = EMax;
    pnew(4) = EMax;
    pnew(5) = 1;

    //y = l95ht3eff(x, pnew);
    return l95ht3eff(x, pnew);
}

mat l94ht2free(mat x, colvec para)
{
    double EMax = max(vectorise(x)); // EMax = max(x);
    colvec pnew = zeros<colvec>(8u); // pnew = zeros(8, 1);
    uvec idx;
    idx << 0 << 1 << 2 << 3 << 4 << 5;
    pnew(idx) = para; // pnew([1,2,3,4,5,6]) = reshape(para, [6,1]);
    pnew(6) = EMax; // pnew([7,8]) = [EMax; 1];
    pnew(7) = 1;
    return l95ht3eff(x, pnew); // y = l95ht3eff(x, pnew);
}

mat l95ht3eff(mat x, colvec para)
{
    double S = para(0);
    double E1 = para(1);
    double k = para(2);
    double b = para(3);
    double E2 = para(4);
    double m = para(5);
    double E3 = para(6);
    double n = para(7);

    mat f1 = exp(- pow((E1 / x), k));// f1 = exp(- (E1./x).^k );
    mat f2 = ones<mat>(x.n_rows, x.n_cols); // f2 = ones(size(x));

    // f2(x>E2) = 1 - exp(- b* ( 1 ./ (x(x>E2) - E2) ).^m);
    uvec idx = find(x > E2);
    f2(idx) = 1 - exp(- b * pow(1 / (x(idx) - E2), m));

    mat f3 = ones<mat>(x.n_rows, x.n_cols); // f3 = ones(size(x));

    // f3(x>E3) = 1  - exp(- ( 2*E3 ./ (x(x>E3) - E3) ).^n);
    idx = find(x > E3);
    f3(idx) = 1 - exp( - pow( 2*E3 / (x(idx) - E3), n) );

    return S * f1 % f2 % f3; // y = S * f1 .* f2 .* f3;
}

mat l96invpow(mat x, colvec para)
{
    //y = para(1) + (x/para(2)).^(-para(3));
    return para(0) + pow(x/para(1), -para(2));
}

mat l97expsum(mat x, colvec para)
{
    mat y = para(0) * ones(size(x)); //y = para(1) * ones(size(x));
    int n = (para.size()-1) / 2; //n = (length(para)-1) / 2;
    for(int i=1; i<=n; ++i) //for i = 1:n
    {
        //y = y + para(2*i) * exp(-para(2*i+1)*x);
        y = y + para(2*i-1) * exp(-para(2*i)*x);
    }
    return y;
}

mat l98constant(mat x, colvec para)
{
    //y = para(1) * ones(size(x));
    return para(0) * ones(size(x));
}

mat l99cutlin(mat x, colvec para)
{
    double ECut = para(0);
    double t0 = para(1);
    double k = para(2);

    mat y = zeros(size(x));
    uvec idx = find(x >= ECut);
    if(!idx.is_empty()) //if any(idx)
    {
        y(idx) = t0 +k * x(idx);
    }
    return y;
}

mat lExtrapolate(mat xd, colvec yd, mat x)
{
    // y = yd(1) + (yd(2)-yd(1))*(x-xd(1))/(xd(2) - xd(1));
    return yd(0) + (yd(1)-yd(0))*(x-xd(0))/(xd(1) - xd(0));
}

}

namespace Deriv {

mat l1interpolate(mat x, colvec para)
{
    // unpack x and y data points
    mat dy = zeros(size(x));
    colvec xdata = MySpan(para, 0, para.size()-1, 2); //xdata = para(1:2:end);
    colvec ydata = MySpan(para, 1, para.size()-1, 2); //ydata = para(2:2:end);
    // slopes are a step function
    colvec slopeData = diff(ydata) / diff(xdata);

    // up to second data point return slope between first and second data point
    uword l = xdata.size();
    uvec idx = find(x <= xdata(1)); //idx = find(x <= xdata(2));
    if (!idx.is_empty())
    {
        MatAssign(dy, idx, slopeData(0)); //dy(idx) = slopeData(1);
    }
    // between second and one but last data point: slope between nearest data points
    for(uword i=1; i<l-2; ++i) //for i = 2 : l-2
    {
        idx = find(x > xdata(i) && x <= xdata(i+1) );
        if (!idx.is_empty())
        {
            MatAssign(dy, idx, slopeData(i));
        }
    }
    // from last but one data point onwards slope between last two data points
    idx = find(x > xdata(l - 2)); //idx = find(x > xdata(l - 1));
    if (!idx.is_empty())
    {
        MatAssign(dy, idx, slopeData(l-2)); //dy(idx) = slopeData(l - 1);
    }
    return dy;
}

mat l2polynomial(mat x, colvec para)
{
    // dy = polyval(polyder(para(end:-1:1)), x);
    mat a, b;
    Matlab::polyder(a, b, 1, MySpan(para, para.size()-1, 0));

    return Matlab::polyval(a, x);
}

mat l3sqrtpoly(mat x, colvec para)
{
    uvec k = find(x < 0);
    if (!k.is_empty())
    {
        x(k) = zeros(size(k));
    }

    // create polynomial p for dy = p(x)/sqrt(x)
    colvec dpara;
    for(uword i=1; i<para.size(); ++i) //for i = 2 : length(para)
    {
        dpara(i-1) = para(i)*(i-1)/2; //dpara(i - 1) = para(i)*(i-1)/2;
    }

    //dy = Matlab::polyval(dpara(end:-1:1), sqrt(x));
    mat dy = Matlab::polyval(MySpan(dpara, 0, dpara.size()-1), sqrt(x));
    dy = dy / sqrt(x);
    return dy;
}

mat l96invpow(mat x, colvec para)
{
    double E1 = para(1);
    double n = para(2);
    mat dy = -n/E1 * pow(E1/x, n+1); //dy = -n/E1 * (E1./x).^(n+1);
    return dy;
}

mat l97expsum(mat x, colvec para)
{
    mat dy = zeros(size(x));
    uword s = (para.size() - 1) / 2;
    for(uword i=1; i<=s; ++i) //for i = 1 : (length(para) - 1)/2
    {
        //dy = dy - para(2*i)*para(2*i+1)*exp(-para(2*i+1)*x);
        dy = dy - para(2*i-1)*para(2*i)*exp(-para(2*i)*x);
    }
    return dy;
}

mat l99cutlin(mat x, colvec para)
{
    double ECut = para(0);
    double E0 = para(1);
    double k = para(2);

    mat dy = zeros(size(x));
    uvec idx = find(x >= ECut);
    if (!idx.is_empty())
    {
        MatAssign(dy, idx, k);
    }
    return dy;
}

mat lApproxDeriv(mat x, colvec para)
{
    double seps = sqrt(eps(1.0));
    mat delta = seps*x;
    uvec idx = find(abs(x) < seps);
    MatAssign(delta, idx, seps);
    mat yplus = Independ::calFcnEval(x+delta, para);
    mat y0 = Independ::calFcnEval(x, para);
    mat dy = (yplus - y0) / delta;

    return dy;
}

}