#include <iostream>

#include "LOCALSUPERHELIX.h"
#include "CNUMPY.h"

VEC3 darbEqn(const VEC3& globalDarb, const VEC3& base, const VEC3& t0, const double& s)
{
    VEC3 hVal = VEC3::Zero();

    if(globalDarb == VEC3::Zero())
    {
        hVal = base + t0 * s;
        return hVal;
    }

    const double om = globalDarb.norm();
    const double tauBar = t0.dot(globalDarb)/om;
    const VEC3 nDarb = globalDarb.normalized();

    const VEC3 tnDarb = tauBar * nDarb;
    const double omS = om * s;

    hVal += base;
    hVal += tnDarb * s;
    hVal += sin(omS) / om * (t0 - tnDarb);
    hVal -= (1 - cos(omS)) / om * t0.cross(nDarb);
    return hVal;
}

VEC3 darbCenterEqn(const VEC3& globalDarb, const VEC3& base, const VEC3& t0, const double& s)
{
    VEC3 hVal = VEC3::Zero();

    if(globalDarb == VEC3::Zero())
    {
        hVal = base + t0 * s;
        return hVal;
    }

    const double om = globalDarb.norm();
    const double tauBar = t0.dot(globalDarb)/om;
    const VEC3 nDarb = globalDarb.normalized();

    const VEC3 tnDarb = tauBar * nDarb;
    const double omS = om * s;

    hVal += base;
    hVal += tnDarb * s;
    hVal -= (1.0) / om * t0.cross(nDarb);
    return hVal;
}

VEC3 FDarbEqn(const VEC3& globalDarb, const VEC3& n0, const double& s)
{
    VEC3 tVal = VEC3::Zero();

    if(globalDarb == VEC3::Zero())
    {
        tVal = n0;
        return tVal;
    }

    const double om = globalDarb.norm();
    const double tauBar = n0.dot(globalDarb)/om;
    const VEC3 nDarb = globalDarb.normalized();

    const VEC3 tnDarb = tauBar * nDarb;
    const double omS = om * s;

    tVal += tnDarb;
    tVal += cos(omS) * (n0 - tnDarb) + sin(omS) * (nDarb.cross(n0));
    return tVal.normalized();
}

VEC3 sampleLocalSupHelix(const superHelix& h, const VEC3& base, const MATRIX3& F, const double& s)
{
    const VEC3 globalDarb = F * h.darboux;
    return darbEqn(globalDarb, base, F.col(0), s);
}

VEC3 sampleLocalSupHelixCenter(const superHelix& h, const VEC3& base, const MATRIX3& F, const double& s)
{
    const VEC3 globalDarb = F * h.darboux;
    return darbCenterEqn(globalDarb, base, F.col(0), s);
}

MATRIX3 FsampleLocalSupHelix(const superHelix& h, const MATRIX3& F, const double& s)
{
    const VEC3 globalDarb = F * h.darboux;
    MATRIX3 F1;
    F1.col(0) = FDarbEqn(globalDarb, F.col(0), s);
    F1.col(1) = FDarbEqn(globalDarb, F.col(1), s);
    F1.col(2) = FDarbEqn(globalDarb, F.col(2), s);
    return F1;
}

std::vector<VEC3> sampleLocalSupHelixes(const std::vector<superHelix>& hs, const VEC3& pB, const MATRIX3& F0, const std::vector<double>& sampleLens)
{
    std::vector<VEC3> endPts;
    std::vector<MATRIX3> endFrames;

    endPts.push_back(pB);
    endFrames.push_back(F0);

    for(unsigned int i = 0 ; i < hs.size(); i++)
    {
        endPts.push_back(sampleLocalSupHelix(hs[i], endPts[i], endFrames[i], hs[i].sQ));
        endFrames.push_back(FsampleLocalSupHelix(hs[i], endFrames[i], hs[i].sQ));
    } // endPts and endTans have size hs.size() + 1  

    std::vector<double> nearLens;
    std::vector<double> partLens;
    double tolLen = 0;
    for(unsigned int i = 0; i < hs.size(); i++)
    {
        double partLen = hs[i].sQ;
        partLens.push_back(partLen);
        tolLen += partLen;
        nearLens.push_back(tolLen);
    }

    std::vector<VEC3> sampledPoints;

    // total lengths and individual lengths acquired
    for(unsigned int i = 0; i < sampleLens.size(); i++)
    {
        const double sLen = sampleLens[i] * tolLen;
        if(sLen < 0)
        {
            continue;
        } 
        else if (sLen > tolLen)
        {
            continue;
        }
        
        int nearestGroundInd = 0;
        for(unsigned int j = 0; j < nearLens.size(); j++)
        {
            if(nearLens[j] < sLen) 
            {
                nearestGroundInd += 1;
            }
        }

        // split to proper distance with phiStar proportioning, 
        double leftoverLen = nearestGroundInd > 0 ? sLen - nearLens[nearestGroundInd - 1] : sLen;
        // then sample and add
        sampledPoints.push_back(sampleLocalSupHelix(hs[nearestGroundInd], endPts[nearestGroundInd], endFrames[nearestGroundInd], leftoverLen));
    }

    return sampledPoints;
}

std::vector<VEC3> sampleLocalSupHelixes(const std::vector<superHelix>& hs, const VEC3& pB, const MATRIX3& F0, int numEvenSamples)
{
    std::vector<double> evenSamples = linspace(0, 1, numEvenSamples);
    return sampleLocalSupHelixes(hs, pB, F0, evenSamples);
}

std::vector<std::vector<VEC3>> sampleLocalSupHelixGroup(const std::vector<std::vector<superHelix>>& hs, const std::vector<VEC3>& pBs, const std::vector<MATRIX3>& FBs, int numEvenSamples)
{
    std::vector<std::vector<VEC3>> strands(hs.size());
    std::vector<double> evenSamples = linspace(0, 1, numEvenSamples);
    for(unsigned int i = 0 ; i < strands.size(); i++)
    {
        strands[i] = sampleLocalSupHelixes(hs[i], pBs[i], FBs[i], evenSamples);
    }
    return strands;
}

VEC3 sampleLSHSinglePoint(const std::vector<superHelix>& hs, const VEC3& pB, const MATRIX3& F0, const double& t)
{
    std::vector<VEC3> endPts;
    std::vector<MATRIX3> endFrames;

    endPts.push_back(pB);
    endFrames.push_back(F0);

    for(unsigned int i = 0 ; i < hs.size(); i++)
    {
        endPts.push_back(sampleLocalSupHelix(hs[i], endPts[i], endFrames[i], hs[i].sQ));
        endFrames.push_back(FsampleLocalSupHelix(hs[i], endFrames[i], hs[i].sQ));
    } // endPts and endTans have size hs.size() + 1  

    std::vector<double> nearLens;
    std::vector<double> partLens;
    double tolLen = 0;
    for(unsigned int i = 0; i < hs.size(); i++)
    {
        double partLen = hs[i].sQ;
        partLens.push_back(partLen);
        tolLen += partLen;
        nearLens.push_back(tolLen);
    }

    VEC3 sampledPoint;

    // total lengths and individual lengths acquired
    const double sLen = t * tolLen;
    assert(sLen >= 0);
    assert(sLen <= tolLen);
        
    int nearestGroundInd = 0;
    for(unsigned int j = 0; j < nearLens.size(); j++)
    {
        if(nearLens[j] < sLen) 
        {
            nearestGroundInd += 1;
        }
    }

        // split to proper distance with phiStar proportioning, 
    double leftoverLen = nearestGroundInd > 0 ? sLen - nearLens[nearestGroundInd - 1] : sLen;
        // then sample and add
    sampledPoint = sampleLocalSupHelix(hs[nearestGroundInd], endPts[nearestGroundInd], endFrames[nearestGroundInd], leftoverLen);

    return sampledPoint;
}