#include "CNUMPY.h"

///////////////////////////////////////////////////////////
// ------------------ array helpers -----------------------
///////////////////////////////////////////////////////////
std::vector<double> linspace(const double& a, const double& b, const int& N)
{
    assert(N >= 2);
    std::vector<double> spaced(N);
    for(int i = 0; i < N; i++)
    {
        const double t = (double)i/(N - 1);
        spaced[i] = (1 - t) * a + b * (t);
    }
    return spaced;
}

std::vector<double> distanceAccumulate(const std::vector<VEC3>& vs)
{
    std::vector<double> ds(vs.size());
    ds[0] = 0;
    for(unsigned int i = 1; i < vs.size(); i++)
    {
        ds[i] = ds[i-1] + (vs[i] - vs[i-1]).norm();
    }
    return ds;
}

std::vector<double> interp(const std::vector<double>& xs, const std::vector<double>& x0, const std::vector<double>& y0)
{
    assert(x0.size() == y0.size());
    std::vector<double> samps(xs.size());
    for(unsigned int i = 0; i < xs.size(); i++)
    {
        const double ts = xs[i];
        bool foundTopper = false;
        for(unsigned int j = 0; j < x0.size(); j++)
        {
            if(ts < x0[j])
            {
                foundTopper = true;
                if(j == 0) 
                {
                    samps[i] = y0[0];
                    break;
                }
                const double s = (ts - x0[j - 1])/(x0[j] - x0[j - 1]);
                samps[i] = y0[j - 1] * (1 - s) + y0[j] * s;
                break;
            }
        }
        if(!foundTopper) samps[i] = y0[x0.size() - 1];
    }
    return samps;
}

double cNPSum(const std::vector<double>& vals)
{
    double sum = 0;
    for(unsigned int i = 0 ; i < vals.size(); i++)
    {
        sum += vals[i];
    }
    return sum;
}
VEC3 cNPSum(const std::vector<VEC3>& vals)
{
    VEC3 sum = VEC3::Zero();
    for(unsigned int i = 0 ; i < vals.size(); i++)
    {
        sum += vals[i];
    }
    return sum;
}