/*--
    Combiner.cpp

    This file is part of the Cornucopia curve sketching library.
    Copyright (C) 2010 Ilya Baran (baran37@gmail.com)

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/

#include "Combiner.h"
#include "GraphConstructor.h"
#include "PrimitiveFitter.h"
#include "PrimitiveSequence.h"
#include "PathFinder.h"
#include "Preprocessing.h"
#include "Resampler.h"
#include "Polyline.h"
#include "Solver.h"
#include "ErrorComputer.h"
#include "Fitter.h"
#include "Oversketcher.h"
#include "PiecewiseLinearUtils.h"

#include <iterator>
#include <cstdio>

#include <Eigen/LU>
#include <Eigen/Cholesky>

using namespace std;
using namespace Eigen;
NAMESPACE_Cornu

#define SPARSE 1

class MulticurveDenseEvalData : public LSEvalData
{
public:
    //overrides
    double error() const { return _con.squaredNorm(); }

    void solveForDelta(double damping, Eigen::VectorXd &out, std::set<LSBoxConstraint> &constraints)
    {
        size_t vars = _errDer.cols();

        size_t size = vars + _con.size() + constraints.size();
        MatrixXd lhs = MatrixXd::Zero(size, size);
        VectorXd rhs = VectorXd::Zero(size);

        lhs.block(0, 0, vars, vars) = _errDer.transpose() * _errDer;
        rhs.segment(0, vars) = -_errDer.transpose() * _err;

        lhs.block(vars, 0, _conDer.rows(), _conDer.cols()) = _conDer;
        lhs.block(0, vars, _conDer.cols(), _conDer.rows()) = _conDer.transpose();
        rhs.segment(vars, _con.size()) = -_con;

        int cnt = 0;
        for(set<LSBoxConstraint>::const_iterator it = constraints.begin(); it != constraints.end(); ++it, ++cnt)
            lhs(vars + _conDer.rows() + cnt, it->index) = lhs(it->index, vars + _conDer.rows() + cnt) = 1.;

        //lhs += damping * MatrixXd::Identity(size, size);
        lhs.block(0, 0, vars, vars) += damping * MatrixXd::Identity(vars, vars);
        VectorXd result = lhs.lu().solve(rhs);

        out = result.segment(0, vars);

#if 0
        printf("Solve err = %lf\n", (lhs * result - rhs).norm());
        if(_conDer.size() > 0)
            printf("Con Solve err = %lf\n", (_conDer * out + _con).norm());
#endif

        //check which constraints we don't need
        cnt = 0;
        for(set<LSBoxConstraint>::iterator it = constraints.begin(); it != constraints.end(); ++cnt)
        {
            set<LSBoxConstraint>::iterator next = it;
            ++next;
            if(result(vars + _conDer.rows() + cnt) * it->sign > 0)
            {
                //printf("Unsetting constraint on variable at index %d\n", it->index);
                constraints.erase(it);
            }
            it = next;
        }
    }

    //for derivative verification, combine error and constraints
    VectorXd errVec() const
    {
        VectorXd out(_err.size() + _con.size());
        out << _err, _con;
        return out;
    }

    MatrixXd errVecDer() const
    {
        MatrixXd out(_err.size() + _con.size(), _errDer.cols());
        out << _errDer, _conDer;
        return out;
    }

    VectorXd &errVectorRef() { return _err; }
    MatrixXd &errDerRef() { return _errDer; }

    VectorXd &conVectorRef() { return _con; }
    MatrixXd &conDerRef() { return _conDer; }

private:
    Eigen::VectorXd _err;
    Eigen::MatrixXd _errDer;
    Eigen::VectorXd _con;
    Eigen::MatrixXd _conDer;
};

class MulticurveSparseEvalData : public LSEvalData
{
public:
    typedef Matrix<double, Dynamic, Dynamic, 0, 6, 6> BlockType;
    typedef vector<BlockType, aligned_allocator<BlockType> > BlockVectorType;
    typedef LLT<BlockType> BlockCholType;
    typedef vector<BlockCholType, aligned_allocator<BlockCholType> > BlockCholVectorType;

    //overrides
    double error() const { return _con.squaredNorm(); }

    void solveForDelta(double damping, Eigen::VectorXd &out, std::set<LSBoxConstraint> &constraints)
    {
        _computeIndices();

        size_t vars = _blockIndices.back();
        size_t cons = _con.size() + constraints.size();

        size_t size = vars + cons;
        VectorXd rhs = VectorXd::Zero(size);
        rhs.segment(0, vars) = _err;
        rhs.segment(vars, _con.size()) = -_con;

        BlockCholVectorType cholBlocks(_errDerBlocks.size());
        for(size_t i = 0; i < cholBlocks.size(); ++i)
            cholBlocks[i] = BlockCholType(_errDerBlocks[i] + damping * MatrixXd::Identity(_blockSizes[i], _blockSizes[i]));

        MatrixXd C = MatrixXd::Zero(cons, vars);
        C.block(0, 0, _con.size(), vars) = _conDer;

        int cnt = 0;
        for(set<LSBoxConstraint>::const_iterator it = constraints.begin(); it != constraints.end(); ++it, ++cnt)
            C(_conDer.rows() + cnt, it->index) = 1.;

        MatrixXd K(C.cols(), C.rows());
        _solveCholL(K, cholBlocks, C.transpose());
        LLT<MatrixXd> CholK(K.transpose() * K);

        VectorXd mid(size), result(size);

        //solve for mid
        _solveCholL(mid.segment(0, vars), cholBlocks, rhs.segment(0, vars));
        mid.segment(vars, cons) = CholK.matrixL().solve(rhs.segment(vars, cons) - K.adjoint() * mid.segment(0, vars));

        //solve for result
        result.segment(vars, cons) = -CholK.matrixU().solve(mid.segment(vars, cons));
        _solveCholU(result.segment(0, vars), cholBlocks, mid.segment(0, vars) - K * result.segment(vars, cons));

        out = result.segment(0, vars);
#if 0
        if(_conDer.size() > 0)
            printf("Con Solve err = %lf\n", (_conDer * out + _con).norm());
#endif

        //check which constraints we don't need
        cnt = 0;
        for(set<LSBoxConstraint>::iterator it = constraints.begin(); it != constraints.end(); ++cnt)
        {
            set<LSBoxConstraint>::iterator next = it;
            ++next;
            if(result(vars + _conDer.rows() + cnt) * it->sign > 0)
            {
                //printf("Unsetting constraint on variable at index %d\n", it->index);
                constraints.erase(it);
            }
            it = next;
        }
    }

    VectorXd &errVectorRef() { return _err; }
    BlockVectorType &errDerBlocksRef() { return _errDerBlocks; }

    VectorXd &conVectorRef() { return _con; }
    MatrixXd &conDerRef() { return _conDer; }

private:
    void _computeIndices()
    {
        _blockIndices.resize(_errDerBlocks.size() + 1);
        _blockSizes.resize(_errDerBlocks.size());
        _blockIndices[0] = 0;

        for(int i = 0; i < (int)_errDerBlocks.size(); ++i)
        {
            _blockSizes[i] = _errDerBlocks[i].rows();
            _blockIndices[i + 1] = _blockIndices[i] + _blockSizes[i];
        }
    }

    MatrixXd _blocksToMatrix(const BlockVectorType &blocks)
    {
        size_t sz = _blockIndices.back();
        MatrixXd out = MatrixXd::Zero(sz, sz);

        for(int i = 0; i < (int)blocks.size(); ++i)
            out.block(_blockIndices[i], _blockIndices[i], _blockSizes[i], _blockSizes[i]) = blocks[i];

        return out;
    }

    template<class M1, class M2> void _solveCholL(const M1 &out, const BlockCholVectorType &chols, const M2 &rhs)
    {
        for(int i = 0; i < (int)chols.size(); ++i)
            const_cast<M1 &>(out).block(_blockIndices[i], 0, _blockSizes[i], out.cols()) =
                chols[i].matrixL().solve(rhs.block(_blockIndices[i], 0, _blockSizes[i], rhs.cols()));
    }

    template<class M1, class M2> void _solveCholU(const M1 &out, const BlockCholVectorType &chols, const M2 &rhs)
    {
        for(int i = 0; i < (int)chols.size(); ++i)
            const_cast<M1 &>(out).block(_blockIndices[i], 0, _blockSizes[i], out.cols()) =
                chols[i].matrixU().solve(rhs.block(_blockIndices[i], 0, _blockSizes[i], rhs.cols()));
    }

    vector<size_t> _blockIndices, _blockSizes;
    BlockVectorType _errDerBlocks;

    Eigen::VectorXd _err;
    Eigen::VectorXd _con;
    Eigen::MatrixXd _conDer;
};

class MulticurveProblem : public LSProblem
{
public:
    MulticurveProblem(const Fitter &fitter)
        : _primitives(fitter.output<PRIMITIVE_FITTING>()->primitives), _iter(0)
    {
        smart_ptr<const AlgorithmOutput<GRAPH_CONSTRUCTION> > graph = fitter.output<GRAPH_CONSTRUCTION>();
        const vector<int> &path = fitter.output<PATH_FINDING>()->path;
        _errorComputer = fitter.output<ERROR_COMPUTER>()->errorComputer;
        _closed = fitter.output<CURVE_CLOSING>()->closed;
        _inflectionAccounting = fitter.params().get(Parameters::INFLECTION_COST) > 0.;

        for(int i = 0; i < (int)path.size(); ++i)
        {
            _primIdcs.push_back(graph->edges[path[i]].startVtx);
            _continuities.push_back(graph->edges[path[i]].continuity);
        }
        if(!_closed)
            _primIdcs.push_back(graph->edges[path.back()].endVtx);

        _curves = VectorC<CurvePrimitivePtr>((int)_primIdcs.size(), _closed ? CIRCULAR : NOT_CIRCULAR);
        _curveRanges = VectorC<pair<int, int> >((int)_primIdcs.size(), _curves.circular());
        for(int i = 0; i < (int)_primIdcs.size(); ++i)
        {
            _curveRanges[i] = make_pair(_primitives[_primIdcs[i]].startIdx, _primitives[_primIdcs[i]].endIdx);
            _curves[i] = _primitives[_primIdcs[i]].curve->clone();
        }

        //trim curves and ranges
        const int sampledPts = fitter.output<RESAMPLING>()->output->pts().size();
        for(int i = 0; i < (int)_continuities.size(); ++i)
        {
            if(_continuities[i] == 0)
                continue;

            //trim
            Vector2d trimPt = 0.5 * (_curves[i]->endPos() + _curves[i + 1]->startPos());
            if(!_primitives[_primIdcs[i]].isFixed())
                _curves[i]->trim(0, _curves[i]->project(trimPt));
            if(_curves.circular() || !_primitives[_primIdcs[i + 1]].isFixed())
                _curves[i + 1]->trim(_curves[i + 1]->project(trimPt), _curves[i + 1]->length());

            //the ranges over which error is computed should not overlap
            if(_continuities[i] == 1)
                swap(_curveRanges[i].second, _curveRanges[i + 1].first); //they overlap by 1 originally
            if(_continuities[i] == 2) //they overlap by 2
            {
                _curveRanges[i + 1].first = (_curveRanges[i + 1].first + 2) % sampledPts;
                _curveRanges[i].second = (_curveRanges[i].second + sampledPts - 2) % sampledPts;
            }
        }
    }

    vector<LSBoxConstraint> getConstraints() const
    {
        vector<LSBoxConstraint> out;
        VectorXd curParams = params();

        int curVar = 0;
        for(int i = 0; i < _curves.size(); ++i)
        {
            int primIdx = _primIdcs[i];
            if(_primitives[primIdx].isFixed())
            {
                int n = _curves[i]->numParams();
                for(int j = 0; j < n; ++j, ++curVar)
                    out.push_back(LSBoxConstraint(curVar, curParams[curVar], 0));

                continue;
            }

            //length must be at least half initial
            out.push_back(LSBoxConstraint(curVar + CurvePrimitive::LENGTH, _curves[i]->length() * 0.5, 1));

            //inflections
            //Debugging::get()->printf("idx = %d Type = %d, startSign = %d, endSign = %d", i, _curves[i]->getType(), _primitives[primIdx].startCurvSign, _primitives[primIdx].endCurvSign);
            if(_inflectionAccounting && _curves[i]->getType() >= CurvePrimitive::ARC)
            {
                int pi = _curves.toLinearIdx(i - 1);
                int ni = _curves.toLinearIdx(i + 1);
                bool c2toPrev = (pi >= 0 && _continuities[pi] == 2);
                bool c2toNext = (ni < _curves.size() && _continuities[i] == 2);
                bool line2Prev = c2toPrev && _curves[pi]->getType() == CurvePrimitive::LINE;
                bool line2Next = c2toNext && _curves[ni]->getType() == CurvePrimitive::LINE;

                int startSign = _primitives[primIdx].startCurvSign;
                int endSign = _primitives[primIdx].endCurvSign;

                if(!c2toPrev || (endSign == _primitives[_primIdcs[pi]].startCurvSign && startSign == endSign && !line2Prev))
                    out.push_back(LSBoxConstraint(curVar + CurvePrimitive::CURVATURE, 0., startSign));

                if(c2toPrev && c2toNext && startSign != endSign) //constrain the "easier one" of the endpoints
                {
                    double startViolation = -_curves[i]->startCurvature() * startSign;
                    double endViolation = -_curves[i]->endCurvature() * endSign;

                    //if we're C2 with a line, don't constrain that curvature
                    if(line2Prev)
                        startViolation = Parameters::infinity;
                    if(line2Next)
                        endViolation = Parameters::infinity;

                    if(startViolation < endViolation)
                        out.push_back(LSBoxConstraint(curVar + CurvePrimitive::CURVATURE, 0., startSign));
                    else
                        out.push_back(LSBoxConstraint(curVar + CurvePrimitive::DCURVATURE, 0., endSign));
                }

                if(!c2toNext && _curves[i]->getType() == CurvePrimitive::CLOTHOID)
                    out.push_back(LSBoxConstraint(curVar + CurvePrimitive::DCURVATURE, 0., endSign));
            }

            curVar += _curves[i]->numParams();
        }

        return out;
    }

#if SPARSE
    typedef MulticurveSparseEvalData EvalDataType;
#else
    typedef MulticurveDenseEvalData EvalDataType;
#endif

    int _iter;
    LSEvalData *createEvalData() { return new EvalDataType(); }
    void eval(const Eigen::VectorXd &x, LSEvalData *data)
    {
        setParams(x);
        EvalDataType *evalData = static_cast<EvalDataType *>(data);
        _evalError(evalData);
        _evalConstraints(evalData);
        //printf("Err: obj = %lf con = %lf\n", evalData->errVectorRef().norm(), evalData->conVectorRef().norm());
    }

    void setParams(const Eigen::VectorXd &x)
    {
        int curIdx = 0;
        for(int i = 0; i < (int)_curves.size(); ++i)
        {
            if(_curves[i]->getType() != CurvePrimitive::CLOTHOID)
            {
                _curves[i]->setParams(x.segment(curIdx, _curves[i]->numParams()));
            }
            else
            {
                VectorXd xm = x.segment(curIdx, _curves[i]->numParams());
                xm(CurvePrimitive::DCURVATURE) = (xm(CurvePrimitive::DCURVATURE) - xm(CurvePrimitive::CURVATURE)) / xm(CurvePrimitive::LENGTH);
                _curves[i]->setParams(xm);
            }
            curIdx += _curves[i]->numParams();
        }

        if(false)
        {
            char name[100];
            sprintf(name, "Out%d", _iter);
            for(int i = 0; i < _curves.size(); ++i)
                Debugging::get()->drawPrimitive(_curves[i], name, i, 2.);
        }

        ++_iter;
    }

    VectorXd params() const
    {
        int totParams = 0;
        for(int i = 0; i < (int)_curves.size(); ++i)
            totParams += _curves[i]->numParams();

        VectorXd out(totParams);

        int curIdx = 0;
        for(int i = 0; i < (int)_curves.size(); ++i)
        {
            if(_curves[i]->getType() != CurvePrimitive::CLOTHOID)
            {
                out.segment(curIdx, _curves[i]->numParams()) = _curves[i]->params();
            }
            else
            {
                VectorXd xm = _curves[i]->params();
                xm(CurvePrimitive::DCURVATURE) = xm(CurvePrimitive::CURVATURE) + xm(CurvePrimitive::DCURVATURE) * xm(CurvePrimitive::LENGTH);
                out.segment(curIdx, _curves[i]->numParams()) = xm;
            }
            curIdx += _curves[i]->numParams();
        }

        return out;
    }

    VectorC<CurvePrimitiveConstPtr> curves() const
    {
        VectorC<CurvePrimitiveConstPtr> out;
        copy(_curves.begin(), _curves.end(), back_inserter(out));
        return out;
    }

    double objective() const
    {
        double out = 0;

        for(int i = 0; i < (int)_curves.size(); ++i)
        {
            int csz = (int)_continuities.size();
            bool firstCorner = (!_closed && i == 0) || (_continuities[(i + csz - 1) % csz] == 0);
            bool lastCorner = (!_closed && i + 1 == (int)_curves.size()) || (_continuities[i] == 0);

            out += _errorComputer->computeError(_curves[i], _curveRanges[i].first, _curveRanges[i].second, firstCorner, lastCorner);
        }

        return out;
    }

private:
    void _evalError(EvalDataType *evalData)
    {
        vector<VectorXd> errVecs(_curves.size());
        vector<MatrixXd> errVecDers(_curves.size());

        size_t numErr = 0, numVar = 0;
        for(int i = 0; i < (int)_curves.size(); ++i)
        {
            int csz = (int)_continuities.size();
            bool firstCorner = (!_closed && i == 0) || (_continuities[(i + csz - 1) % csz] == 0);
            bool lastCorner = (!_closed && i + 1 == (int)_curves.size()) || (_continuities[i] == 0);

            _errorComputer->computeErrorVector(_curves[i], _curveRanges[i].first, _curveRanges[i].second,
                errVecs[i], &(errVecDers[i]), firstCorner, lastCorner);

            _curves[i]->toEndCurvatureDerivative(errVecDers[i]);

            numErr += errVecs[i].size();
            numVar += _curves[i]->numParams();
        }

        VectorXd &outErr = evalData->errVectorRef();

#if SPARSE
        EvalDataType::BlockVectorType &outErrDerBlocks = evalData->errDerBlocksRef();
        outErrDerBlocks.resize(_curves.size());
        outErr = VectorXd::Zero(numVar);
#else
        outErr = VectorXd::Zero(numErr);
        MatrixXd &outErrDer = evalData->errDerRef();
        outErrDer = MatrixXd::Zero(numErr, numVar);
#endif

        size_t curErr = 0, curVar = 0;
        for(int i = 0; i < (int)_curves.size(); ++i)
        {
            size_t nErr = errVecs[i].size();
            size_t nVar = errVecDers[i].cols();
#if SPARSE
            outErrDerBlocks[i] = errVecDers[i].transpose() * errVecDers[i];
            outErr.segment(curVar, nVar) = -errVecDers[i].transpose() * errVecs[i];
#else
            outErrDer.block(curErr, curVar, nErr, nVar) = errVecDers[i];
            outErr.segment(curErr, nErr) = errVecs[i];
#endif
            curErr += nErr;
            curVar += nVar;
        }
    }

    void _evalConstraints(EvalDataType *evalData)
    {
        VectorXd &outCon = evalData->conVectorRef();
        MatrixXd &outConDer = evalData->conDerRef();

        vector<VectorXd> conVecs(_continuities.size());
        vector<MatrixXd> conVecDers(_continuities.size());

        CurvePrimitive::EndDer endDer;

        size_t numCon = 0, numVar = 0;
        for(int i = 0; i < (int)_continuities.size(); ++i)
        {
            conVecs[i].resize(2 + _continuities[i]);
            conVecs[i].head<2>() = _curves[i]->endPos() - _curves[i + 1]->startPos();
            if(_continuities[i] >= 1)
                conVecs[i][2] = AngleUtils::toRange(_curves[i]->endAngle() - _curves[i + 1]->startAngle(), -PI);
            if(_continuities[i] == 2)
                conVecs[i][3] = _curves[i]->endCurvature() - _curves[i + 1]->startCurvature();

            _curves[i]->derivativeAtEnd(_continuities[i], endDer);
            conVecDers[i] = endDer;
            _curves[i]->toEndCurvatureDerivative(conVecDers[i]);

            numCon += conVecs[i].size();
            numVar += _curves[i]->numParams();
        }

        if(!_closed)
            numVar += _curves.back()->numParams();

        outCon = VectorXd::Zero(numCon);
        outConDer = MatrixXd::Zero(numCon, numVar);

        size_t curCon = 0, curVar = 0;
        for(int i = 0; i < (int)_continuities.size(); ++i)
        {
            size_t nCon = conVecs[i].size();
            size_t nVar = conVecDers[i].cols();
            outCon.segment(curCon, nCon) = conVecs[i];
            outConDer.block(curCon, curVar, nCon, nVar) = conVecDers[i];

            //now the derivatives for the second curve
            outConDer(curCon + 0, (curVar + nVar + CurvePrimitive::X) % numVar) = -1.;
            outConDer(curCon + 1, (curVar + nVar + CurvePrimitive::Y) % numVar) = -1.;
            if(nCon > 2)
                outConDer(curCon + 2, (curVar + nVar + CurvePrimitive::ANGLE) % numVar) = -1.;
            if(nCon > 3 && _curves[i + 1]->getType() != CurvePrimitive::LINE)
                outConDer(curCon + 3, (curVar + nVar + CurvePrimitive::CURVATURE) % numVar) = -1.;

            curCon += nCon;
            curVar += nVar;
        }
    }

    VectorC<CurvePrimitivePtr> _curves;
    const vector<FitPrimitive> &_primitives;
    vector<int> _primIdcs;
    vector<int> _continuities; //continuity[i] is between curves i and i + 1
    VectorC<pair<int, int> > _curveRanges;
    bool _closed;
    ErrorComputerConstPtr _errorComputer;
    bool _inflectionAccounting;
};

class DefaultCombiner : public Algorithm<COMBINING>
{
public:
    string name() const { return "Default"; }

protected:
    void _run(const Fitter &fitter, AlgorithmOutput<COMBINING> &out)
    {
        smart_ptr<const AlgorithmOutput<GRAPH_CONSTRUCTION> > graph = fitter.output<GRAPH_CONSTRUCTION>();
        const vector<FitPrimitive> &primitives = fitter.output<PRIMITIVE_FITTING>()->primitives;
        const vector<int> &path = fitter.output<PATH_FINDING>()->path;
        bool closed = fitter.output<CURVE_CLOSING>()->closed;

        if(path.empty())
            return; //no path

        VectorC<CurvePrimitiveConstPtr> outV;

        //if a single primitive
        if(graph->edges[path[0]].continuity == -1)
        {
            outV = VectorC<CurvePrimitiveConstPtr>(1, NOT_CIRCULAR);
            outV[0] = primitives[graph->edges[path[0]].startVtx].curve;
        }
        else //solve the nonlinear problem
        {
            MulticurveProblem problem(fitter);
            vector<LSBoxConstraint> constraints = problem.getConstraints();
            LSSolver solver(&problem, constraints);
            solver.setDefaultDamping(fitter.params().get(Parameters::COMBINE_DAMPING));
            solver.setMaxIter(50);
            solver.setIncreaseDampingAfter(5);
            solver.setDampingIncreaseFactor(1.5);

            VectorXd result = solver.solve(problem.params());
            problem.setParams(result);
            Debugging::get()->printf("Final objective = %lf", sqrt(problem.objective()));

            outV = problem.curves();
        }

        //==== track what happens to parameters ====
        out.parameters = fitter.output<RESAMPLING>()->parameters;
        PolylineConstPtr resampledCurve = fitter.output<RESAMPLING>()->output;
        const VectorC<Vector2d> &resampled = resampledCurve->pts();

        vector<int> finalPrimitives; //gather the indices of the graph vertices corresponding to the primitives
        for(int i = 0; i < (int)path.size(); ++i)
            finalPrimitives.push_back(graph->edges[path[i]].startVtx);
        if(outV.size() > (int)finalPrimitives.size())
            finalPrimitives.push_back(graph->edges[path.back()].endVtx);

        assert(outV.size() == finalPrimitives.size());

        vector<double> idxToParam(resampled.size()); //idx is the index into the resampled array
        vector<double> idxToDistSq(resampled.size(), 1e10);

        double lenSoFar = 0;
        for(int i = 0; i < outV.size(); ++i) //for each primitive see what projects to it
        {
            const FitPrimitive &primitive = primitives[graph->vertices[finalPrimitives[i]].primitiveIdx];
            for(int j = primitive.startIdx; ; ++j) //project each associated resampled point onto this primitive
            {
                if(j == (int)resampled.size()) //be careful with starts and ends of oversketched primitives
                {
                    if(closed) j = 0;
                    else
                        break;
                }

                if(j < 0)
                    continue;

                double proj = outV[i]->project(resampled[j]);
                double distSq = (resampled[j] - outV[i]->pos(proj)).squaredNorm();
                if(distSq < idxToDistSq[j])
                {
                    idxToDistSq[j] = distSq;
                    idxToParam[j] = proj + lenSoFar;
                }

                if(j == primitive.endIdx)
                    break;
            }
            lenSoFar += outV[i]->length();
        }
        double outputLength = lenSoFar;

        if(!closed)
        {
            idxToParam[0] = 0;
            idxToParam.back() = outputLength;
        }

        int minParamSample = min_element(idxToParam.begin(), idxToParam.end()) - idxToParam.begin();

        PiecewiseLinearMonotone prevToFinal(PiecewiseLinearMonotone::POSITIVE);
        //populate prevToFinal -- don't forget duplicating first point if closed
        prevToFinal.add(0, idxToParam[minParamSample]);
        lenSoFar = 0;
        for(VectorC<Vector2d>::Circulator ci = resampled.circulator(minParamSample + 1); !ci.done(); ++ci)
        {
            lenSoFar += (*ci - *(ci - 1)).norm();
            double finalParam = 0;
            if(ci.index() == minParamSample)
                finalParam += outputLength;
            else
                idxToParam[ci.index()] = max(idxToParam[ci.index()], idxToParam[(ci - 1).index()]); //ensure monotonicity
            finalParam += idxToParam[ci.index()];
            prevToFinal.add(lenSoFar, finalParam);
        }
        //adjust parameters into range
        for(int i = 0; i < (int)out.parameters.size(); ++i)
        {
            out.parameters[i] -= resampledCurve->idxToParam(minParamSample);
            if(out.parameters[i] < 0)
                out.parameters[i] += resampledCurve->length();
        }
        prevToFinal.batchEval(out.parameters);

        //==== combine with what needs to be done w.r.t. oversketching ====
        smart_ptr<const AlgorithmOutput<OVERSKETCHING> > osOutput = fitter.output<OVERSKETCHING>();
        VectorC<CurvePrimitiveConstPtr> outFinal(0, osOutput->finallyClose ? CIRCULAR : NOT_CIRCULAR);

        if(osOutput->toPrepend)
        {
            outFinal.insert(outFinal.end(), osOutput->toPrepend->primitives().begin(), osOutput->toPrepend->primitives().end() - 1);
            for(int i = 0; i < (int)out.parameters.size(); ++i)
                out.parameters[i] += (osOutput->toPrepend->length() - osOutput->toPrepend->primitives().back()->length());
        }
        outFinal.insert(outFinal.end(), outV.begin(), outV.end());

        if(osOutput->toAppend)
        {
            if(!osOutput->finallyClose)
            {
                outFinal.insert(outFinal.end(), osOutput->toAppend->primitives().begin() + 1, osOutput->toAppend->primitives().end());
            }
            else
            {
                if(osOutput->toAppend->primitives().size() >= 2)
                {
                    outFinal.insert(outFinal.end(), osOutput->toAppend->primitives().begin() + 1, osOutput->toAppend->primitives().end() - 1);
                }
                else //start and end curve is the same curve -- its original length is the one toAppend curve
                {
                    //get rid of last curve and possibly extend the first one
                    double lastCurveLen = outFinal.back()->length();
                    double firstCurveLen = outFinal[0]->length();
                    double origLen = osOutput->toAppend->primitives()[0]->length();
                    outFinal.pop_back();
                    //now extend the first curve to the combined length
                    outFinal[0] = outFinal[0]->trimmed(origLen - lastCurveLen, firstCurveLen);
                    for(int i = 0; i < (int)out.parameters.size(); ++i)
                        out.parameters[i] -= (origLen - lastCurveLen);
                }
            }
        }

        out.output = new PrimitiveSequence(outFinal);

#if 1
        for(int i = 0; i < (int)out.parameters.size(); ++i)
        {
            double paramOrig = fitter.originalSketch()->idxToParam(i);
            Debugging::get()->drawLine(fitter.originalSketch()->pts()[i], out.output->pos(out.parameters[i]), Vector3d(1, 0, 1), "Correspondence");
        }
#endif

    }
};

void Algorithm<COMBINING>::_initialize()
{
    new DefaultCombiner();
}

END_NAMESPACE_Cornu


//    Copyright 2017 Rainer Gemulla
//
//    Licensed under the Apache License, Version 2.0 (the "License");
//    you may not use this file except in compliance with the License.
//    You may obtain a copy of the License at
//
//        http://www.apache.org/licenses/LICENSE-2.0
//
//    Unless required by applicable law or agreed to in writing, software
//    distributed under the License is distributed on an "AS IS" BASIS,
//    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//    See the License for the specific language governing permissions and
//    limitations under the License.
/** \file
 *
 * Illustrates matrix factorization with parallel SGD. We first create factors and then a data matrix
 * from these factors. This process ensures that we know the best factorization of the input.
 * These matrices are distributed across a cluster. We then try to reconstruct the factors
 * using PSGD.
 */
#include <iostream>
#include <sstream>

#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>

#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <boost/math/distributions/normal.hpp>
#include <boost/random/uniform_real.hpp>
#include <mf/matrix/io/generateDistributedMatrix.h>

#include <util/evaluation.h>

#include <mpi2/mpi2.h>
#include <mf/mf.h>

log4cxx::LoggerPtr logger(log4cxx::Logger::getLogger("main"));

using namespace std;
using namespace mf;
using namespace mpi2;
using namespace rg;
using namespace boost::numeric::ublas;

// type of SGD
typedef UpdateTruncate<UpdateNzslL2> Update;
typedef RegularizeNone Regularize;
typedef SumLoss<NzslLoss, L2Loss> Loss;
typedef NzslLoss TestLoss;

int main(int argc, char* argv[]) {
	using namespace boost::program_options;
	// initialize mf library and mpi2
	boost::mpi::communicator& world = mfInit(argc, argv);

	mfStart();

	if (world.rank() == 0)
	{
#ifndef NDEBUG
		LOG4CXX_WARN(logger, "Warning: Debug mode activated (runtimes may be slow).");
#endif
		//data
		mf_size_type epochs;
		string inputSampleMatrixFile;
		string inputMatrixFile;
		string inputRowFacFile;
		string inputColFacFile;
		string outputRowFacFile;
		string outputColFacFile;
		string inputTestMatrixFile;
		string traceFile,traceVar;
		string shuffleStr;

		double lambda = 50;
		double eps0 = 0.01;
		// parameters for distribution
		int tasks;

		options_description desc("Options");
		desc.add_options()
				("help", "produce help message")
				("epochs", value<mf_size_type>(&epochs)->default_value(10), "number of epochs to run [10]")
				("lambda", value<double>(&lambda)->default_value(50), "lambda")
				("eps0", value<double>(&eps0)->default_value(0.01), "initial step size for BoldDriver")
				("tasks-per-rank", value<int>(&tasks)->default_value(1), "number of concurrent tasks [1]")
				("trace", value<string>(&traceFile)->default_value("trace.R"), "filename of trace [trace.R]")
				("traceVar", value<string>(&traceVar)->default_value("trace"), "variable name for trace [traceVar]")
				("input-file", value<string>(&inputMatrixFile), "input matrix")
				("input-test-file", value<string>(&inputTestMatrixFile), "input test matrix")
			    ("input-row-file", value<string>(&inputRowFacFile), "input initial row factor")
			    ("input-col-file", value<string>(&inputColFacFile), "input initial column factor")
			    ("output-row-file", value<string>(&outputRowFacFile), "output initial row factor")
			    ("output-col-file", value<string>(&outputColFacFile), "output initial column factor")
			    ("shuffle",value<string>(&shuffleStr)->default_value("seq"),"shuffle method eg seq, par, parAdd")
				;

		positional_options_description pdesc;
		pdesc.add("input-file", 1);
		pdesc.add("input-test-file", 2);
		pdesc.add("input-sample-matrix-file", 3);
		pdesc.add("input-row-file", 4);
		pdesc.add("input-col-file", 5);

		variables_map vm;
		store(command_line_parser(argc, argv).options(desc).positional(pdesc).run(), vm);
		notify(vm);

		if (vm.count("help") || vm.count("input-file")==0) {
			cout << "psgd with L2 NoLock (Hogwild-style) [options] <input-file> " << endl << endl;
			cout << desc << endl;
			return 1;
		}

		LOG4CXX_INFO(logger, "Using " << tasks << " parallel tasks");

		PsgdShuffle shuffle;
		if (shuffleStr.compare("seq") == 0)
			shuffle= PSGD_SHUFFLE_SEQ;
		else if (shuffleStr.compare("par") == 0)
			shuffle= PSGD_SHUFFLE_PARALLEL;
		else
			shuffle= PSGD_SHUFFLE_PARALLEL_ADDITIONAL_TASK;

		LOG4CXX_INFO(logger, "Using " << shuffle);

		// Read matrices
		Random32 random;
// 		SparseMatrix v,vTest;
// 		DenseMatrix w;
// 		DenseMatrixCM h;
// 		readMatrix(inputMatrixFile,v);
// 		readMatrix(inputTestMatrixFile,vTest);
// 		readMatrix(inputRowFacFile,w);
// 		readMatrix(inputColFacFile,h);

		Timer t;
		t.start();
		std::vector<DistributedSparseMatrix> dataVector=getDataMatrices<SparseMatrix>(inputMatrixFile,"V",true,
				tasks, 1, 1, 1, true, false, &inputTestMatrixFile);

		SparseMatrix& v = *mpi2::env().get<SparseMatrix>(dataVector[0].blocks()(0,0).var());
		SparseMatrix& vTest = *mpi2::env().get<SparseMatrix>(dataVector[1].blocks()(0,0).var());

		std::pair<DistributedDenseMatrix, DistributedDenseMatrixCM> factorsPair= getFactors(inputRowFacFile,
			inputColFacFile,  tasks, 1, 1, 1, true);

		t.stop();
		LOG4CXX_INFO(logger, "Total time for loading matrices: " << t);

		DenseMatrix w = *mpi2::env().get<DenseMatrix>(factorsPair.first.blocks()(0,0).var());
		DenseMatrixCM h = *mpi2::env().get<DenseMatrixCM>(factorsPair.second.blocks()(0,0).var());

		// parameters for SGD
		SgdOrder order = SGD_ORDER_WOR;
		Update update = Update(UpdateNzslL2(lambda), -100, 100); // truncate for numerical stability
		Regularize regularize;
		Loss loss((NzslLoss()), L2Loss(lambda));
		TestLoss testLoss;
		BalanceType balanceType = BALANCE_NONE;
		BalanceMethod balanceMethod = BALANCE_OPTIMAL;

		// initialize the DSGD

		PsgdRunner psgdRunner(random);
		PsgdJob<Update,Regularize> psgdJob(v, w, h, update, regularize, order, tasks, shuffle);
		BoldDriver decay(eps0);
		Trace trace;

		trace.addField("Loss", "L2");
		trace.addField("Shuffle_method", shuffle);
		trace.addField("input_file", inputMatrixFile);
		trace.addField("sample_matrix", inputSampleMatrixFile);
		trace.addField("tasks", tasks);

		// print the test loss
		FactorizationData<> testData(vTest, w, h);
		LOG4CXX_INFO(logger, "Initial test loss: " << testLoss(testData));

		// run HogwildSGD to try to reconstruct the original factors
		t.start();
		psgdRunner.run(psgdJob, loss, epochs, decay, trace, balanceType, balanceMethod, &testData, &testLoss);

		t.stop();
		LOG4CXX_INFO(logger, "Total time: " << t);

		// print the test loss
		LOG4CXX_INFO(logger, "Final test loss: " << testLoss(testData));

		// write trace to an R file
		LOG4CXX_INFO(logger, "Writing trace to " << traceFile);
		trace.toRfile(traceFile, traceVar);

					// write computed factors to file
				if (outputRowFacFile.length() > 0) {
					LOG4CXX_INFO(logger, "Writing row factors to " << outputRowFacFile);
					//DenseMatrix w0;
					//unblock(dw, w0);
					writeMatrix(outputRowFacFile, w);
				}
				if (outputColFacFile.length() > 0) {
					LOG4CXX_INFO(logger, "Writing column factors to " << outputColFacFile);
					//DenseMatrixCM h0;
					//unblock(dh, h0);
					writeMatrix(outputColFacFile, h);
				}
	}

	mfStop();
	mfFinalize();

	return 0;
}


/*
 * Copyright (c) 2020. Mohit Deshpande.
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

#include "ekf/ekf_node.h"

#include <chrono>

#include <ros/ros.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2/utils.h>
#include <sensor_msgs/Imu.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <laser_geometry/laser_geometry.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_ros/point_cloud.h>

#include <std_msgs/Float32.h>

#include "ekf/ekf.h"
#include "ekf/utils.h"
#include "ekf/scan_matcher.h"

#include <Eigen/Eigen>

#include <robot_config/robot_config.h>

static constexpr auto kOdometryTopic = "/wheels/encoders";
static constexpr auto kImuTopic = "/camera/imu";
static constexpr auto kScanTopic = "/scan";

static constexpr auto UPDATE_RATE = 40;
static constexpr auto STATE_SIZE = 5;
// motors can't go this fast and no one is going to move it this fast
// still WIP on why the MCU occasionally produces really high counts
static constexpr auto V_MAX_SANITY = 2.0;
static constexpr auto W_MAX_SANITY = M_2_PI;

namespace ekf {

EkfNode::EkfNode()
        : prev_cloud_(new pcl::PointCloud<pcl::PointXYZ>),
        tf_listener_(tf_buffer_) {
    ros::NodeHandle nh;

    odometry_sub_ = nh.subscribe(kOdometryTopic, 1, &EkfNode::ReceiveOdometry, this);
    imu_sub_ = nh.subscribe(kImuTopic, 1, &EkfNode::ReceiveImu, this);
    scan_sub_ = nh.subscribe(kScanTopic, 1, &EkfNode::ReceiveScan, this);

    imu_H_ = Eigen::MatrixXd::Zero(1, 5);
    imu_H_(0, 4) = 1;
    imu_R_ = Eigen::MatrixXd::Identity(1, 1) * 0.1;

    odom_H_ = Eigen::MatrixXd::Zero(1, 5);
    odom_H_(0, 3) = 1;
    odom_R_ = Eigen::MatrixXd::Identity(1, 1) * 0.1;

    scan_H_ = Eigen::MatrixXd::Zero(2, 5);
    scan_H_(0, 0) = 1;
    scan_H_(1, 1) = 1;
    scan_R_ = Eigen::MatrixXd::Identity(2, 2) * 0.1;

    // [x, y, theta, v, w]
    Eigen::VectorXd initial_state = Eigen::VectorXd::Zero(STATE_SIZE);
    Eigen::MatrixXd initial_covariance = Eigen::MatrixXd::Identity(initial_state.size(), initial_state.size());
    ekf_.state = initial_state;
    ekf_.covariance = initial_covariance;

    // publishers
    pose_pub_ = nh.advertise<geometry_msgs::PoseWithCovarianceStamped>("/pose/odom", 1);
    last_imu_stamp_ = ros::Time::now();
    last_odom_stamp_ = ros::Time::now();
}

void EkfNode::ReceiveImu(const sensor_msgs::ImuConstPtr& imu) {
    if (first_imu_msg) {
        first_imu_msg = false;
        last_imu_stamp_ = imu->header.stamp;
        return;
    }

    geometry_msgs::TransformStamped camera_link;
    try {
        camera_link = tf_buffer_.lookupTransform("camera_link", imu->header.frame_id, imu->header.stamp);
    } catch (const std::exception& e) {
        ROS_ERROR_STREAM(e.what());
        return;
    }

    geometry_msgs::Vector3 transformed_angular_velocity;
    tf2::doTransform(imu->angular_velocity, transformed_angular_velocity, camera_link);

    double dt = (imu->header.stamp - last_imu_stamp_).toSec();
    auto w = transformed_angular_velocity.z;

    // sanity checking on velocity
    if (w > W_MAX_SANITY) {
        ROS_WARN_STREAM("Impossible angular velocity!: " << w << "rad/s");
        return;
    }

    Eigen::VectorXd z(1);
    z(0) = w;

    if (!Update(ekf_, z, imu_H_, imu_R_)) {
        ROS_ERROR("Not updating state! S is singular!");
        return;
    }
    Predict(ekf_, dt);

    last_imu_stamp_ = imu->header.stamp;
}

void EkfNode::ReceiveOdometry(const motion_controller_msgs::WheelEncodersConstPtr& odometry) {
    if (first_odom_msg) {
        first_odom_msg = false;
        last_odom_stamp_ = odometry->header.stamp;
        return;
    }

    double dt = (odometry->header.stamp - last_odom_stamp_).toSec();
    double counts = 0.5 * (odometry->left + odometry->right);
    double v = counts * robot_config::DISTANCE_PER_COUNT / dt;

    // sanity checking on velociy
    if (v > V_MAX_SANITY) {
        ROS_WARN_STREAM("Impossible velocity!: " << v << "m/s (l="
            << odometry->left << ", r=" << odometry->right << ")");
        return;
    }

    Eigen::VectorXd z(1);
    z(0) = v;

    if (!Update(ekf_, z, odom_H_, odom_R_)) {
        ROS_ERROR("Not updating state! S is singular!");
        return;
    }

    Predict(ekf_, dt);

    last_odom_stamp_ = odometry->header.stamp;
}

void EkfNode::ReceiveScan(const sensor_msgs::LaserScanConstPtr& scan) {
    if (first_scan_msg) {
        sensor_msgs::PointCloud2 cloud;
        laser_projector_.transformLaserScanToPointCloud("base_link", *scan, cloud, tf_buffer_);
        pcl::moveFromROSMsg(cloud, *prev_cloud_);
        prev_scan_ekf_ = ekf_;
        first_scan_msg = false;
        return;
    }

    sensor_msgs::PointCloud2 cloud;
    laser_projector_.transformLaserScanToPointCloud("base_link", *scan, cloud, tf_buffer_);
    pcl::PointCloud<pcl::PointXYZ>::Ptr current_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::moveFromROSMsg(cloud, *current_cloud);

    Eigen::Affine3d transform(ScanMatch({}, prev_cloud_, current_cloud));

    Eigen::Vector2d z = Eigen::Vector2d::Zero();
    z(0) = transform.translation()(0) + prev_scan_ekf_.state(0);
    z(1) = transform.translation()(1) + prev_scan_ekf_.state(1);

    if (!Update(ekf_, z, scan_H_, scan_R_)) {
        ROS_ERROR("Not updating state! S is singular!");
    }

    prev_cloud_ = current_cloud;
    prev_scan_ekf_ = ekf_;
}

void EkfNode::Run() {
    ros::Rate rate(UPDATE_RATE);
    while (ros::ok()) {
        geometry_msgs::PoseWithCovarianceStamped pose;
        pose.header.frame_id = "odom";
        pose.header.stamp = ros::Time::now();
        pose.pose.pose.position.x = ekf_.state(0);
        pose.pose.pose.position.y = ekf_.state(1);
        pose.pose.pose.position.z = 0;
        tf2::Quaternion orientation;
        orientation.setRPY(0, 0, ekf_.state(2));
        pose.pose.pose.orientation = tf2::toMsg(orientation);
        pose.pose.covariance = toRos(ekf_.covariance);
        ROS_INFO_STREAM("x=" << pose.pose.pose.position.x
            << ", y=" << pose.pose.pose.position.y
            << ", theta=" << tf2::getYaw(pose.pose.pose.orientation));
        pose_pub_.publish(pose);

        geometry_msgs::TransformStamped transform;
        transform.transform.translation.x = pose.pose.pose.position.x;
        transform.transform.translation.y = pose.pose.pose.position.y;
        transform.transform.translation.z = pose.pose.pose.position.z;
        transform.transform.rotation = tf2::toMsg(orientation);
        transform.header.stamp = ros::Time::now();
        transform.header.frame_id = "odom";
        transform.child_frame_id = "base_link";
        tf_broadcaster_.sendTransform(transform);

        rate.sleep();
        ros::spinOnce();
    }
}

}

int main(int argc, char** argv) {
    ros::init(argc, argv, "ekf");
    ros::NodeHandle nh;

    ekf::EkfNode slam;
    slam.Run();

    return 0;
}


#pragma once
#include <Eigen/Dense>
#include <memory>
#include <tuple>
#include <unordered_map>
#include <utility>
#include <vector>

#include "../datastructures/multi_tree_vector.hpp"
#include "basis.hpp"
#include "sparse_vector.hpp"

namespace Time {

template <template <typename, typename> class Operator, typename I_in,
          typename I_out>
class BilinearForm {
 public:
  using WaveletBasisIn =
      std::remove_pointer_t<std::tuple_element_t<0, typename I_in::TupleNodes>>;
  using WaveletBasisOut = std::remove_pointer_t<
      std::tuple_element_t<0, typename I_out::TupleNodes>>;
  using ScalingBasisIn = typename FunctionTrait<WaveletBasisIn>::Scaling;
  using ScalingBasisOut = typename FunctionTrait<WaveletBasisOut>::Scaling;

  // Create a stateful BilinearForm.
  BilinearForm(I_in *root_vec_in, I_out *root_vec_out);

  void Apply() {
    InitializeInput();
    auto [_, f] = ApplyRecur(0, {}, {});
    FinalizeOutput(f);
  }

  void ApplyUpp() {
    InitializeInput();
    auto [_, f] = ApplyUppRecur(0, {}, {});
    FinalizeOutput(f);
  }

  void ApplyLow() {
    InitializeInput();
    auto f = ApplyLowRecur(0, {});
    FinalizeOutput(f);
  }

  auto Transpose() const {
    auto transpose = BilinearForm<Operator, I_out, I_in>();
    transpose.vec_in_ = vec_out_;
    transpose.vec_out_ = vec_in_;
    transpose.nodes_vec_in_ = nodes_vec_out_;
    transpose.nodes_vec_out_ = nodes_vec_in_;
    transpose.InitializeOutput();
    return transpose;
  }

  // Debug function, O(n^2).
  Eigen::MatrixXd ToMatrix();

 protected:
  // Protected constructor, and give transpose operator access.
  BilinearForm() : vec_in_(nullptr), vec_out_(nullptr) {}
  friend BilinearForm<Operator, I_out, I_in>;

  // Roots of the treeviews we are considering.
  I_in *vec_in_;
  I_out *vec_out_;

  // A flattened (levelwise) view of input/output vectors.
  std::shared_ptr<std::vector<std::vector<I_in *>>> nodes_vec_in_;
  std::shared_ptr<std::vector<std::vector<I_out *>>> nodes_vec_out_;

  // Another flattened (levelwise) view of the vectors, in another data format.
  std::vector<SparseVector<WaveletBasisIn>> lvl_vec_in_;
  std::vector<SparseIndices<WaveletBasisOut>> lvl_ind_out_;

  // Helper variables.
  SparseVector<WaveletBasisIn> empty_vec_in_;
  SparseIndices<WaveletBasisOut> empty_ind_out_;

  // Helper function to set the levelwise input/output vector.
  void InitializeOutput();
  void InitializeInput();
  void FinalizeOutput(const SparseVector<WaveletBasisOut> &f);

  // Recursive apply.
  std::pair<SparseVector<ScalingBasisOut>, SparseVector<WaveletBasisOut>>
  ApplyRecur(size_t l, SparseIndices<ScalingBasisOut> &&Pi_out,
             const SparseVector<ScalingBasisIn> &d);

  // Recursive apply upper part.
  std::pair<SparseVector<ScalingBasisOut>, SparseVector<WaveletBasisOut>>
  ApplyUppRecur(size_t l, SparseIndices<ScalingBasisOut> &&Pi_out,
                const SparseVector<ScalingBasisIn> &d);

  // Recursive apply lower part.
  SparseVector<WaveletBasisOut> ApplyLowRecur(
      size_t l, const SparseVector<ScalingBasisIn> &d);

  // Index sets.
  std::pair<SparseIndices<ScalingBasisOut>, SparseIndices<ScalingBasisOut>>
  ConstructPiOut(SparseIndices<ScalingBasisOut> &&Pi_out,
                 bool construct_Pi_A_out = true);

  SparseIndices<ScalingBasisIn> ConstructPiBIn(
      SparseIndices<ScalingBasisIn> &&Pi_in,
      const SparseIndices<ScalingBasisOut> &Pi_B_out);
};

// Helper functions .
template <template <typename, typename> class Operator, typename I_in,
          typename I_out>
BilinearForm<Operator, I_in, I_out> CreateBilinearForm(I_in *root_vec_in,
                                                       I_out *root_vec_out) {
  return BilinearForm<Operator, I_in, I_out>(root_vec_in, root_vec_out);
}

// Helper functions .
template <template <typename, typename> class Operator, typename WaveletBasisIn,
          typename WaveletBasisOut>
BilinearForm<Operator, datastructures::NodeVector<WaveletBasisIn>,
             datastructures::NodeVector<WaveletBasisOut>>
CreateBilinearForm(const datastructures::TreeVector<WaveletBasisIn> &vec_in,
                   const datastructures::TreeVector<WaveletBasisOut> &vec_out) {
  return {vec_in.root(), vec_out.root()};
}

}  // namespace Time

#include "bilinear_form.ipp"


#ifndef MBA_MBA_HPP
#define MBA_MBA_HPP

/*
The MIT License

Copyright (c) 2015 Denis Demidov <dennis.demidov@gmail.com>

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/

/**
 * \file   mba/mba.hpp
 * \author Denis Demidov <dennis.demidov@gmail.com>
 * \brief  Multilevel B-spline interpolation.
 */

#include <iostream>
#include <iomanip>
#include <map>
#include <list>
#include <utility>
#include <algorithm>
#include <array>
#include <memory>
#include <functional>

#include <boost/container/flat_map.hpp>
#include <boost/multi_array.hpp>
#include <type_traits>
#include <boost/io/ios_state.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/lu.hpp>

namespace mba {
namespace detail {

template <size_t N, size_t M>
struct power : std::integral_constant<size_t, N * power<N, M-1>::value> {};

template <size_t N>
struct power<N, 0> : std::integral_constant<size_t, 1> {};

/// N-dimensional grid iterator (nested loop with variable depth).
template <unsigned NDim>
class grid_iterator {
    public:
        typedef std::array<size_t, NDim> index;

        explicit grid_iterator(const std::array<size_t, NDim> &dims)
            : N(dims), idx(0)
        {
            std::fill(i.begin(), i.end(), 0);
            done = (i == N);
        }

        explicit grid_iterator(size_t dim) : idx(0) {
            std::fill(N.begin(), N.end(), dim);
            std::fill(i.begin(), i.end(), 0);
            done = (0 == dim);
        }

        size_t operator[](size_t d) const {
            return i[d];
        }

        const index& operator*() const {
            return i;
        }

        size_t position() const {
            return idx;
        }

        grid_iterator& operator++() {
            done = true;
            for(size_t d = NDim; d--; ) {
                if (++i[d] < N[d]) {
                    done = false;
                    break;
                }
                i[d] = 0;
            }

            ++idx;

            return *this;
        }

        operator bool() const { return !done; }

    private:
        index N, i;
        bool  done;
        size_t idx;
};

template <typename T, size_t N>
std::array<T, N> operator+(std::array<T, N> a, const std::array<T, N> &b) {
    std::transform(a.begin(), a.end(), b.begin(), a.begin(), std::plus<T>());
    return a;
}

template <typename T, size_t N>
std::array<T, N> operator-(std::array<T, N> a, T b) {
    std::transform(a.begin(), a.end(), a.begin(), std::bind2nd(std::minus<T>(), b));
    return a;
}

template <typename T, size_t N>
std::array<T, N> operator*(std::array<T, N> a, T b) {
    std::transform(a.begin(), a.end(), a.begin(), std::bind2nd(std::multiplies<T>(), b));
    return a;
}

// Value of k-th B-Spline basic function at t.
inline double Bspline(size_t k, double t) {
    assert(0 <= t && t < 1);
    assert(k < 4);

    switch (k) {
        case 0:
            return (t * (t * (-t + 3) - 3) + 1) / 6;
        case 1:
            return (t * t * (3 * t - 6) + 4) / 6;
        case 2:
            return (t * (t * (-3 * t + 3) + 3) + 1) / 6;
        case 3:
            return t * t * t / 6;
        default:
            return 0;
    }
}

// Checks if p is between lo and hi
template <typename T, size_t N>
bool boxed(const std::array<T,N> &lo, const std::array<T,N> &p, const std::array<T,N> &hi) {
    for(unsigned i = 0; i < N; ++i) {
        if (p[i] < lo[i] || p[i] > hi[i]) return false;
    }
    return true;
}

inline double safe_divide(double a, double b) {
    return b == 0.0 ? 0.0 : a / b;
}

template <unsigned NDim>
class control_lattice {
    public:
        typedef std::array<size_t, NDim> index;
        typedef std::array<double, NDim> point;

        virtual ~control_lattice() {}

        virtual double operator()(const point &p) const = 0;

        virtual void report(std::ostream&) const = 0;

        template <class CooIter, class ValIter>
        double residual(CooIter coo_begin, CooIter coo_end, ValIter val_begin) const {
            double res = 0.0;

            CooIter p = coo_begin;
            ValIter v = val_begin;

            for(; p != coo_end; ++p, ++v) {
                (*v) -= (*this)(*p);
                res = std::max(res, std::abs(*v));
            }

            return res;
        }
};

template <unsigned NDim>
class initial_approximation : public control_lattice<NDim> {
    public:
        typedef typename control_lattice<NDim>::point point;

        initial_approximation(std::function<double(const point&)> f)
            : f(f) {}

        double operator()(const point &p) const {
            return f(p);
        }

        void report(std::ostream &os) const {
            os << "initial approximation";
        }
    private:
        std::function<double(const point&)> f;
};

template <unsigned NDim>
class control_lattice_dense : public control_lattice<NDim> {
    public:
        typedef typename control_lattice<NDim>::index index;
        typedef typename control_lattice<NDim>::point point;

        template <class CooIter, class ValIter>
        control_lattice_dense(
                const point &coo_min, const point &coo_max, index grid_size,
                CooIter coo_begin, CooIter coo_end, ValIter val_begin
                ) : cmin(coo_min), cmax(coo_max), grid(grid_size)
        {
            for(unsigned i = 0; i < NDim; ++i) {
                hinv[i] = (grid[i] - 1) / (cmax[i] - cmin[i]);
                cmin[i] -= 1 / hinv[i];
                grid[i] += 2;
            }

            boost::multi_array<double, NDim> delta(grid);
            boost::multi_array<double, NDim> omega(grid);

            std::fill(delta.data(), delta.data() + delta.num_elements(), 0.0);
            std::fill(omega.data(), omega.data() + omega.num_elements(), 0.0);

            CooIter p = coo_begin;
            ValIter v = val_begin;

            for(; p != coo_end; ++p, ++v) {
                if (!boxed(coo_min, *p, coo_max)) continue;

                index i;
                point s;

                for(unsigned d = 0; d < NDim; ++d) {
                    double u = ((*p)[d] - cmin[d]) * hinv[d];
                    i[d] = floor(u) - 1;
                    s[d] = u - floor(u);
                }

                std::array< double, power<4, NDim>::value > w;
                double sum_w2 = 0.0;

                for(grid_iterator<NDim> d(4); d; ++d) {
                    double prod = 1.0;
                    for(unsigned k = 0; k < NDim; ++k) prod *= Bspline(d[k], s[k]);

                    w[d.position()] = prod;
                    sum_w2 += prod * prod;
                }

                for(grid_iterator<NDim> d(4); d; ++d) {
                    double w1  = w[d.position()];
                    double w2  = w1 * w1;
                    double phi = (*v) * w1 / sum_w2;

                    index j = i + (*d);

                    delta(j) += w2 * phi;
                    omega(j) += w2;
                }
            }

            phi.resize(grid);

            std::transform(
                    delta.data(), delta.data() + delta.num_elements(),
                    omega.data(), phi.data(), safe_divide
                    );
        }

        double operator()(const point &p) const {
            index i;
            point s;

            for(unsigned d = 0; d < NDim; ++d) {
                double u = (p[d] - cmin[d]) * hinv[d];
                i[d] = floor(u) - 1;
                s[d] = u - floor(u);
            }

            double f = 0;

            for(grid_iterator<NDim> d(4); d; ++d) {
                double w = 1.0;
                for(unsigned k = 0; k < NDim; ++k) w *= Bspline(d[k], s[k]);

                f += w * phi(i + (*d));
            }

            return f;
        }

        void report(std::ostream &os) const {
            boost::io::ios_all_saver stream_state(os);

            os << "dense  [" << grid[0];
            for(unsigned i = 1; i < NDim; ++i)
                os << ", " << grid[i];
            os << "] (" << phi.num_elements() * sizeof(double) << " bytes)";
        }

        void append_refined(const control_lattice_dense &r) {
            static const std::array<double, 5> s = {
                0.125, 0.500, 0.750, 0.500, 0.125
            };

            for(grid_iterator<NDim> i(r.grid); i; ++i) {
                double f = r.phi(*i);

                if (f == 0.0) continue;

                for(grid_iterator<NDim> d(5); d; ++d) {
                    index j;
                    bool skip = false;
                    for(unsigned k = 0; k < NDim; ++k) {
                        j[k] = 2 * i[k] + d[k] - 3;
                        if (j[k] >= grid[k]) {
                            skip = true;
                            break;
                        }
                    }

                    if (skip) continue;

                    double c = 1.0;
                    for(unsigned k = 0; k < NDim; ++k) c *= s[d[k]];

                    phi(j) += f * c;
                }
            }
        }

        double fill_ratio() const {
            size_t total    = phi.num_elements();
            size_t nonzeros = total - std::count(phi.data(), phi.data() + total, 0.0);

            return static_cast<double>(nonzeros) / total;
        }

    private:
        point cmin, cmax, hinv;
        index grid;

        boost::multi_array<double, NDim> phi;

};

template <unsigned NDim>
class control_lattice_sparse : public control_lattice<NDim> {
    public:
        typedef typename control_lattice<NDim>::index index;
        typedef typename control_lattice<NDim>::point point;

        template <class CooIter, class ValIter>
        control_lattice_sparse(
                const point &coo_min, const point &coo_max, index grid_size,
                CooIter coo_begin, CooIter coo_end, ValIter val_begin
                ) : cmin(coo_min), cmax(coo_max), grid(grid_size)
        {
            for(unsigned i = 0; i < NDim; ++i) {
                hinv[i] = (grid[i] - 1) / (cmax[i] - cmin[i]);
                cmin[i] -= 1 / hinv[i];
                grid[i] += 2;
            }

            std::map<index, two_doubles> dw;

            CooIter p = coo_begin;
            ValIter v = val_begin;

            for(; p != coo_end; ++p, ++v) {
                if (!boxed(coo_min, *p, coo_max)) continue;

                index i;
                point s;

                for(unsigned d = 0; d < NDim; ++d) {
                    double u = ((*p)[d] - cmin[d]) * hinv[d];
                    i[d] = floor(u) - 1;
                    s[d] = u - floor(u);
                }

                std::array< double, power<4, NDim>::value > w;
                double sum_w2 = 0.0;

                for(grid_iterator<NDim> d(4); d; ++d) {
                    double prod = 1.0;
                    for(unsigned k = 0; k < NDim; ++k) prod *= Bspline(d[k], s[k]);

                    w[d.position()] = prod;
                    sum_w2 += prod * prod;
                }

                for(grid_iterator<NDim> d(4); d; ++d) {
                    double w1  = w[d.position()];
                    double w2  = w1 * w1;
                    double phi = (*v) * w1 / sum_w2;

                    two_doubles delta_omega = {w2 * phi, w2};

                    append(dw[i + (*d)], delta_omega);
                }
            }

            phi.insert(//boost::container::ordered_unique_range,
                    boost::make_transform_iterator(dw.begin(), delta_over_omega),
                    boost::make_transform_iterator(dw.end(),   delta_over_omega)
                    );
        }

        double operator()(const point &p) const {
            index i;
            point s;

            for(unsigned d = 0; d < NDim; ++d) {
                double u = (p[d] - cmin[d]) * hinv[d];
                i[d] = floor(u) - 1;
                s[d] = u - floor(u);
            }

            double f = 0;

            for(grid_iterator<NDim> d(4); d; ++d) {
                double w = 1.0;
                for(unsigned k = 0; k < NDim; ++k) w *= Bspline(d[k], s[k]);

                f += w * get_phi(i + (*d));
            }

            return f;
        }

        void report(std::ostream &os) const {
            boost::io::ios_all_saver stream_state(os);

            size_t grid_size = grid[0];

            os << "sparse [" << grid[0];
            for(unsigned i = 1; i < NDim; ++i) {
                os << ", " << grid[i];
                grid_size *= grid[i];
            }

            size_t bytes = phi.size() * sizeof(std::pair<index, double>);
            size_t dense_bytes = grid_size * sizeof(double);

            double compression = static_cast<double>(bytes) / dense_bytes;
            os << "] (" << bytes << " bytes, compression: "
                << std::fixed << std::setprecision(2) << compression << ")";
        }
    private:
        point cmin, cmax, hinv;
        index grid;

        typedef boost::container::flat_map<index, double> sparse_grid;
        sparse_grid phi;

        typedef std::array<double, 2> two_doubles;

        static std::pair<index, double> delta_over_omega(const std::pair<index, two_doubles> &dw) {
            return std::make_pair(dw.first, safe_divide(dw.second[0], dw.second[1]));
        }

        static void append(two_doubles &a, const two_doubles &b) {
            std::transform(a.begin(), a.end(), b.begin(), a.begin(), std::plus<double>());
        }

        double get_phi(const index &i) const {
            typename sparse_grid::const_iterator c = phi.find(i);
            return c == phi.end() ? 0.0 : c->second;
        }
};

} // namespace detail

template <unsigned NDim>
class linear_approximation {
    public:
        typedef typename detail::control_lattice<NDim>::point point;

        template <class CooIter, class ValIter>
        linear_approximation(CooIter coo_begin, CooIter coo_end, ValIter val_begin)
        {
            namespace ublas = boost::numeric::ublas;

            size_t n = std::distance(coo_begin, coo_end);

            if (n <= NDim) {
                // Not enough points to get a unique plane
                std::fill(C.begin(), C.end(), 0.0);
                C[NDim] = std::accumulate(val_begin, val_begin + n, 0.0) / n;
                return;
            }

            ublas::matrix<double> A(NDim+1, NDim+1); A.clear();
            ublas::vector<double> f(NDim+1);         f.clear();

            CooIter p = coo_begin;
            ValIter v = val_begin;

            double sum_val = 0.0;

            // Solve least-squares problem to get approximation with a plane.
            for(; p != coo_end; ++p, ++v, ++n) {
                std::array<double, NDim+1> x;
                std::copy(p->begin(), p->end(), boost::begin(x));
                x[NDim] = 1.0;

                for(unsigned i = 0; i <= NDim; ++i) {
                    for(unsigned j = 0; j <= NDim; ++j) {
                        A(i,j) += x[i] * x[j];
                    }
                    f(i) += x[i] * (*v);
                }

                sum_val += (*v);
            }

            ublas::permutation_matrix<size_t> pm(NDim+1);
            ublas::lu_factorize(A, pm);

            bool singular = false;
            for(unsigned i = 0; i <= NDim; ++i) {
                if (A(i,i) == 0.0) {
                    singular = true;
                    break;
                }
            }

            if (singular) {
                std::fill(C.begin(), C.end(), 0.0);
                C[NDim] = sum_val / n;
            } else {
                ublas::lu_substitute(A, pm, f);
                for(unsigned i = 0; i <= NDim; ++i) C[i] = f(i);
            }
        }

        double operator()(const point &p) const {
            double f = C[NDim];

            for(unsigned i = 0; i < NDim; ++i)
                f += C[i] * p[i];

            return f;
        }
    private:
        std::array<double, NDim+1> C;
};

template <unsigned NDim>
class MBA {
    public:
        typedef std::array<size_t, NDim> index;
        typedef std::array<double, NDim> point;

        template <class CooIter, class ValIter>
        MBA(
                const point &coo_min, const point &coo_max, index grid,
                CooIter coo_begin, CooIter coo_end, ValIter val_begin,
                unsigned max_levels = 8, double tol = 1e-8, double min_fill = 0.5,
                std::function<double(point)> initial = std::function<double(point)>()
           )
        {
            init(
                    coo_min, coo_max, grid,
                    coo_begin, coo_end, val_begin,
                    max_levels, tol, min_fill, initial
                );
        }

        template <class CooRange, class ValRange>
        MBA(
                const point &coo_min, const point &coo_max, index grid,
                CooRange coo, ValRange val,
                unsigned max_levels = 8, double tol = 1e-8, double min_fill = 0.5,
                std::function<double(point)> initial = std::function<double(point)>()
           )
        {
            init(
                    coo_min, coo_max, grid,
                    boost::begin(coo), boost::end(coo), boost::begin(val),
                    max_levels, tol, min_fill, initial
                );
        }

        double operator()(const point &p) const {
            double f = 0.0;

            for(const auto &psi : cl) {
                f += (*psi)(p);
            }

            return f;
        }

        friend std::ostream& operator<<(std::ostream &os, const MBA &h) {
            size_t level = 0;
            for(const auto &psi : h.cl) {
                os << "level " << ++level << ": ";
                psi->report(os);
                os << std::endl;
            }
            return os;
        }

    private:
        typedef detail::control_lattice<NDim>        lattice;
        typedef detail::initial_approximation<NDim>  initial_approximation;
        typedef detail::control_lattice_dense<NDim>  dense_lattice;
        typedef detail::control_lattice_sparse<NDim> sparse_lattice;

        std::list< std::shared_ptr<lattice> > cl;

        template <class CooIter, class ValIter>
        void init(
                const point &cmin, const point &cmax, index grid,
                CooIter coo_begin, CooIter coo_end, ValIter val_begin,
                unsigned max_levels, double tol, double min_fill,
                std::function<double(point)> initial
                )
        {
            using namespace mba::detail;

            const ptrdiff_t n = std::distance(coo_begin, coo_end);
            std::vector<double> val(val_begin, val_begin + n);

            double res, eps = 0.0;
            for(ptrdiff_t i = 0; i < n; ++i)
                eps = std::max(eps, std::abs(val[i]));
            eps *= tol;

            if (initial) {
                // Start with the given approximation.
                cl.push_back(std::make_shared<initial_approximation>(initial));
                res = cl.back()->residual(coo_begin, coo_end, val.begin());
                if (res <= eps) return;
            }

            size_t lev = 1;
            // Create dense head of the hierarchy.
            {
                auto psi = std::make_shared<dense_lattice>(
                        cmin, cmax, grid, coo_begin, coo_end, val.begin());

                res = psi->residual(coo_begin, coo_end, val.begin());
                double fill = psi->fill_ratio();

                for(; (lev < max_levels) && (res > eps) && (fill > min_fill); ++lev) {
                    grid = grid * 2ul - 1ul;

                    auto f = std::make_shared<dense_lattice>(
                            cmin, cmax, grid, coo_begin, coo_end, val.begin());

                    res = f->residual(coo_begin, coo_end, val.begin());
                    fill = f->fill_ratio();

                    f->append_refined(*psi);
                    psi.swap(f);
                }

                cl.push_back(psi);
            }

            // Create sparse tail of the hierrchy.
            for(; (lev < max_levels) && (res > eps); ++lev) {
                grid = grid * 2ul - 1ul;

                cl.push_back(std::make_shared<sparse_lattice>(
                        cmin, cmax, grid, coo_begin, coo_end, val.begin()));

                res = cl.back()->residual(coo_begin, coo_end, val.begin());
            }
        }
};

} // namespace mba

#endif


/*
  Copyright (c) 2012, Matthew H. Reilly (kb1vc)
  All rights reserved.

  Redistribution and use in source and binary forms, with or without
  modification, are permitted provided that the following conditions are
  met:

  Redistributions of source code must retain the above copyright
  notice, this list of conditions and the following disclaimer.
  Redistributions in binary form must reproduce the above copyright
  notice, this list of conditions and the following disclaimer in
  the documentation and/or other materials provided with the
  distribution.

  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
  A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
  HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
  SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
  LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
  THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
  (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

#include "OSFilter.hxx"

#include <iostream>
#include <string.h>
#include <fftw3.h>
#include <math.h>
#include <boost/format.hpp>

static unsigned int ipow(unsigned int x, unsigned int y)
{
  unsigned int ret;
  ret = 1;
  unsigned int i;

  for(i = 0; i < y; i++) {
    ret *= x;
  }

  return ret;
}

SoDa::OSFilter::OSFilter(float * filter_impulse_response,
			 unsigned int filter_length,
			 float filter_gain,
			 unsigned int inout_buffer_length,
			 OSFilter * cascade,
			 unsigned int suggested_transform_length)
{
  // these are the salient dimensions for this Overlap/Save
  // widget (for terminology, see Lyons pages 719ff
  Q = filter_length; // to start with.
  M = inout_buffer_length;

  if((cascade != NULL) && (cascade->M != M)) cascade = NULL;

  if(M < 4 * Q) {
    std::cerr << "Warning -- OSFilter asked to implement a long filter against a short buffer." << std::endl;
  }

  // now find N.
  if(suggested_transform_length > (M + Q - 1)) {
    N = suggested_transform_length;
  }
  else {
    N = guessN();
  }

  // now that we have N, we can back-calculate Q.
  Q = N - M;

  tail_index = M - (Q - 1);

  // now build the transform image for the filter.
  std::complex<float> * filter_in = (std::complex<float> *) fftwf_malloc(sizeof(std::complex<float>) * N);
  filter_fft = (std::complex<float> *) fftwf_malloc(sizeof(std::complex<float>) * N);

  // create a temporary plan
  fftwf_plan tplan = fftwf_plan_dft_1d(N, (fftwf_complex*) filter_in, (fftwf_complex*) filter_fft,
				       FFTW_FORWARD, FFTW_ESTIMATE);

  // now build the filter
  // fill with zeros
  unsigned int i;
  for(i = 0; i < N; i++) filter_in[i] = std::complex<float>(0.0,0.0);
  // fill in the impulse response
  float gain_corr = 1.0 / ((float) N) * filter_gain;
  for(i = 0; i < filter_length; i++) filter_in[i] = std::complex<float>(filter_impulse_response[i] * gain_corr, 0.0);

  // transform the filter.
  fftwf_execute(tplan);

  // and forget the plan.
  fftwf_destroy_plan(tplan);

  // filter lengths must be equal too
  if((cascade != NULL) && (cascade->N != N)) cascade = NULL;

  // if there is a cascaded filter, multiply our filter coeffs by the cascaded filter coeffs
  if(cascade != NULL) {
    for(i = 0; i < N; i++) {
      std::complex<double> a = filter_fft[i];
      std::complex<double> b = cascade->filter_fft[i];
      std::complex<double> ff = a * b * ((double) N);
      filter_fft[i] = std::complex<float>(ff.real(), ff.imag());
    }
  }

  // setup the fft buffers
  setupFFT();
}

SoDa::OSFilter::OSFilter(float low_cutoff,
			 float low_pass_edge,
			 float high_pass_edge,
			 float high_cutoff,

			 unsigned int filter_length,
			 float filter_gain,
			 float sample_rate,

			 unsigned int inout_buffer_length,
			 unsigned int suggested_transform_length)
{
  // remember our edges
  low_edge = (double) low_pass_edge;
  high_edge = (double) high_pass_edge;

  // first find our buffer sizes.
  Q = filter_length;
  M = inout_buffer_length;
  if(M < 4 * Q) {
    std::cerr << "Warning -- OSFilter asked to implement a long filter against a short buffer." << std::endl;
  }

  // now find N.
  if(suggested_transform_length > (M + Q - 1)) {
    N = suggested_transform_length;
  }
  else {
    N = guessN();
  }

  // now back calculate the actual filter length
  Q = N - M;

  tail_index = M - (Q - 1);

  // OK.  now we build the filter.
  // First, build an allpass filter with the appropriate delay
  std::complex<float> * filter_in = (std::complex<float> *) fftwf_malloc(sizeof(std::complex<float>) * N);
  filter_fft = (std::complex<float> *) fftwf_malloc(sizeof(std::complex<float>) * N);

  unsigned int i, j;
  for(i = 0; i < N; i++) filter_in[i] = std::complex<float>(0.0,0.0);
  filter_in[Q/2] = std::complex<float>(1.0, 0.0);
  // we'll use the image of this filter for the phase part of our filter.

  fftwf_plan fplan = fftwf_plan_dft_1d(N, (fftwf_complex *) filter_in, (fftwf_complex *) filter_fft,
				       FFTW_FORWARD, FFTW_ESTIMATE);
  fftwf_plan ifplan = fftwf_plan_dft_1d(N, (fftwf_complex *) filter_fft, (fftwf_complex *) filter_in,
				       FFTW_BACKWARD, FFTW_ESTIMATE);

  // create an image that we can fill in.
  fftwf_execute(fplan);

  float freq_step = sample_rate / ((float) N);
  float fr;
  // now setup the filter image;
  for(fr = 0.0, i = 0, j = (N - 1); i < N/2; i++, j--, fr += freq_step) {
    if(fr < low_cutoff) {
      filter_fft[i] = std::complex<float>(0.0,0.0);
      filter_fft[j] = std::complex<float>(0.0,0.0);
    }
    else if(fr < low_pass_edge) {
      float mult = (fr - low_cutoff) / (low_pass_edge - low_cutoff);
      filter_fft[i] = filter_fft[i] * std::complex<float>(mult, 0.0);
      filter_fft[j] = filter_fft[j] * std::complex<float>(mult, 0.0);
    }
    else if(fr < high_pass_edge) {
      // keep the all pass value
    }
    else if(fr < high_cutoff) {
      float mult = 1.0 - ((fr - high_pass_edge) / (high_cutoff - high_pass_edge));
      filter_fft[i] = filter_fft[i] * std::complex<float>(mult, 0.0);
      filter_fft[j] = filter_fft[j] * std::complex<float>(mult, 0.0);
    }
    else {
      filter_fft[i] = std::complex<float>(0.0,0.0);
      filter_fft[j] = std::complex<float>(0.0,0.0);
    }
  }

  // inverse transform the filter image
  fftwf_execute(ifplan);

  // now we've got a FIR filter, but it needs to be windowed before we can trust it.
  for(i = 0; i < Q/2; i++) {
    int ii = (Q/2) - i;
    float ang = 2.0 * M_PI * ((float) ii) / ((float) Q);
    std::complex<float> wf(0.5 + 0.5 * cos(ang), 0.0);
    filter_in[i] = filter_in[i] * wf;
    filter_in[Q - i] = filter_in[Q - i] * wf;
  }
  for(i = Q; i < N; i++) {
    filter_in[i] = std::complex<float>(0.0, 0.0);
  }

  // now create the filter image
  fftwf_execute(fplan);

  // now we need to normalize the envelope so that we get the specified gain.
  float maxval = 0.0;
  for(i = 0; i < N; i++) {
    float v = abs(filter_fft[i]);
    if(v > maxval) maxval = v;
  }

  std::complex<float> normalize(filter_gain / (((float) N) * maxval), 0.0);

  for(i = 0; i < N; i++) {
    filter_fft[i] = filter_fft[i] * normalize;
  }

  // and destroy the plans
  fftwf_destroy_plan(fplan);
  fftwf_destroy_plan(ifplan);

  // and free the impulse response
  fftwf_free(filter_in);

  // and setup the FFT buffers
  setupFFT();
}

int SoDa::OSFilter::guessN()
{
    // give preferences to convenient powers of two.
    // but let's search for the nearest solution that is
    // a multiple of 2^a * 3^b * 5^c where b and c are in the range 0..3
    // and a is in the range 1..16
  unsigned int N_guess, N_best, E_best;
  N_best = 2;
  E_best = 0x80000000;
  int i;
  for(i = 1; i <= 0xff; i++) {
    unsigned int a = i & 0xf;
    unsigned int b = (i >> 4) & 0x3;
    unsigned int c = (i >> 6) & 0x3;
    N_guess = ipow(2, a) * ipow(3, b) * ipow(5, c);
    if(N_guess >= (M + Q - 1)) {
      unsigned slop = N_guess - (M + Q - 1);
      if(slop < E_best) {
	N_best = N_guess;
	E_best = slop;
      }
    }
  }

  return N_best;
}

void SoDa::OSFilter::setupFFT()
{
  unsigned int i;
  // now allocate all the storage vectors
  fft_input = (std::complex<float> *) fftwf_malloc(sizeof(std::complex<float>) * N);
  fft_output = (std::complex<float> *) fftwf_malloc(sizeof(std::complex<float>) * N);
  ifft_output = (std::complex<float> *) fftwf_malloc(sizeof(std::complex<float>) * N);

  // and create the plans
  forward_plan = fftwf_plan_dft_1d(N,
				   (fftwf_complex *) fft_input,
				   (fftwf_complex *) fft_output,
				   FFTW_FORWARD, FFTW_ESTIMATE);
  backward_plan = fftwf_plan_dft_1d(N,
				    (fftwf_complex *) fft_output,
				    (fftwf_complex *) ifft_output,
				    FFTW_BACKWARD, FFTW_ESTIMATE);

  // zero out the start of the fft_input buffer for the first iteration.
  for(i = 0; i < Q-1; i++) fft_input[i] = std::complex<float>(0.0,0.0);

}

unsigned int SoDa::OSFilter::apply(float * inbuf, float * outbuf, float outgain, int instride, int outstride)
{
  unsigned int i, j;
  // copy the input buffer.
  for(i = 0, j = Q-1; i < (M * instride); i += instride, j++) {
    fft_input[j] = std::complex<float>(inbuf[i], 0.0);
  }

  // now do the forward FFT on the input
  fftwf_execute(forward_plan);

  // save the last bits of the input buffer to the Q-1 side of the FFT input vector
  // Do this now incase in buf and outbuf are the same buffers.  (we're going to
  // over-write most of outbuf with the memcpy at the bottom.... )
  for(i = (tail_index * instride), j = 0; j < Q-1; i += instride, j++) {
    fft_input[j] = std::complex<float>(inbuf[i], 0.0);
  }

  // apply the filter.
  for(i = 0; i < N; i++) {
    fft_output[i] = (fft_output[i] * filter_fft[i]) * outgain;
  }

  // now do the backward FFT on the result
  fftwf_execute(backward_plan);

  // and copy the result to the output buffer, but discard the
  // first Q-1 chunks
  for(i = 0, j = Q-1; i < (M * outstride); i += outstride, j++) {
    outbuf[i] = ifft_output[j].real();
  }

  return M;
}

unsigned int SoDa::OSFilter::apply(std::complex<float> * inbuf, std::complex<float> * outbuf, float outgain)
{
  // This is the overlap-save FFT filter technique described in Lyons pages 719ff.
  // first we need to copy the input buffer to the M side of the FFT input vector.
  memcpy(&(fft_input[Q-1]), inbuf, sizeof(std::complex<float>) * M);

  // now do the forward FFT on the input
  fftwf_execute(forward_plan);

  // save the last bits of the input buffer to the Q-1 side of the FFT input vector
  // Do this now incase inbuf and outbuf are the same buffers.  (we're going to
  // over-write most of outbuf with the memcpy at the bottom.... )
  memcpy(fft_input, &(inbuf[tail_index]), sizeof(std::complex<float>) * (Q-1));

  // apply the filter.
  unsigned int i;
  for(i = 0; i < N; i++) {
    fft_output[i] = (fft_output[i] * filter_fft[i]) * outgain;
  }

  // now do the backward FFT on the result
  fftwf_execute(backward_plan);

  // and copy the result to the output buffer, but discard the
  // first Q-1 chunks
  memcpy(outbuf, &(ifft_output[Q-1]), sizeof(std::complex<float>) * M);

  return M;
}

void SoDa::OSFilter::dump(std::ostream & os)
{
  unsigned int i;
  os << "# idx  real   imag   abs   arg" << std::endl;
  for(i = 0; i < N; i++) {
    float mag = abs(filter_fft[i]);
    float phase = arg(filter_fft[i]);
    os << i << " " << filter_fft[i].real() << " " << filter_fft[i].imag() << " " << mag << " " << phase << std::endl;
  }
}


//---------------------------------------------------------------------------//
// Copyright (c) 2018-2020 Mikhail Komarov <nemo@nil.foundation>
// Copyright (c) 2020 Alexander Sokolov <asokolov@nil.foundation>
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//---------------------------------------------------------------------------//

#ifndef CRYPTO3_DETAIL_IMPLODER_HPP
#define CRYPTO3_DETAIL_IMPLODER_HPP

#include <boost/crypto3/detail/stream_endian.hpp>
#include <boost/crypto3/detail/unbounded_shift.hpp>
#include <boost/crypto3/detail/reverser.hpp>

#include <boost/static_assert.hpp>

namespace boost {
    namespace crypto3 {
        namespace detail {

            // By definition, for all imploders, InputValueBits < OutputValueBits,
            // so we're taking many smaller values and combining them into one value

            /*!
             * @defgroup imploder Imploder functions
             */

            /*!
             * @brief imploder_shift trait is used to determine whether the input elements are packed into
             * an output element in reverse order. Since the input and output types are integral now, this
             * trait contains the shift indicating the position of input element in the output element when
             * k input bits have already been processed.
             *
             * @ingroup imploder
             *
             * @tparam OutputEndianness
             * @tparam UnitBits
             * @tparam InputBits
             * @tparam OutputBits
             * @tparam k
             * @tparam IsLittleUnit
             */
            template<typename OutputEndianness, int UnitBits, int InputBits, int OutputBits, int k,
                     bool IsLittleUnit = is_little_unit<OutputEndianness, UnitBits>::value>
            struct imploder_shift;

            template<typename OutputEndianness, int UnitBits, int InputBits, int OutputBits, int k>
            struct imploder_shift<OutputEndianness, UnitBits, InputBits, OutputBits, k, false> {
                constexpr static int const value = OutputBits - (InputBits + k);
            };

            template<typename OutputEndianness, int UnitBits, int InputBits, int OutputBits, int k>
            struct imploder_shift<OutputEndianness, UnitBits, InputBits, OutputBits, k, true> {
                constexpr static int const value = k;
            };

            /*!
             * @brief imploder_step packs an input value represented in InputEndianness endianness
             * into an output value represented in OutputEndianness endianness when k input bits
             * have already been processed. It uses unit_reverser and bit_reverser to deal with the
             * order of units and bits in the input value, respectively. Shift constant is determined
             * by the imploder_shift trait.
             *
             * @ingroup imploder
             *
             * @tparam InputEndianness
             * @tparam OutputEndianness
             * @tparam UnitBits
             * @tparam InputBits
             * @tparam OutputBits
             * @tparam k
             */
            template<typename InputEndianness, typename OutputEndianness, int UnitBits, int InputBits, int OutputBits,
                     int k>
            struct imploder_step {
                constexpr static int const shift =
                    imploder_shift<OutputEndianness, UnitBits, InputBits, OutputBits, k>::value;

                template<typename InputValue, typename OutputValue>
                inline static void step(InputValue &in, OutputValue &out) {
                    InputValue tmp = in;
                    unit_reverser<InputEndianness, OutputEndianness, UnitBits>::reverse(tmp);
                    bit_reverser<InputEndianness, OutputEndianness, UnitBits>::reverse(tmp);
                    out |= unbounded_shl<shift>(low_bits<InputBits>(OutputValue(tmp)));
                }
            };

            /*!
             * @brief imploder processes a sequence of input values represented in InputEndianness endianness
             * into an output value represented in OutputEndianness endianness. The function implode is
             * invoked recursively, and the parameter k is used to track the number of already processed
             * input values packed into the output value. The recursion ends when all elements the output
             * value can hold have already been processed, i.e. when k == OutputBits.
             *
             * @ingroup imploder
             *
             * @tparam InputEndianness
             * @tparam OutputEndianness
             * @tparam InputBits
             * @tparam OutputBits
             * @tparam k
             */
            template<typename InputEndianness, typename OutputEndianness, int InputBits, int OutputBits, int k = 0>
            struct imploder;

            template<template<int> class InputEndian, template<int> class OutputEndian, int UnitBits, int InputBits,
                     int OutputBits, int k>
            struct imploder<InputEndian<UnitBits>, OutputEndian<UnitBits>, InputBits, OutputBits, k> {

                // To keep the implementation managable, input and output sizes must
                // be multiples or factors of the unit size.
                // If one of these is firing, you may want a bit-only stream_endian
                // rather than one that mentions bytes or octets.
                BOOST_STATIC_ASSERT(!(InputBits % UnitBits && UnitBits % InputBits));
                BOOST_STATIC_ASSERT(!(OutputBits % UnitBits && UnitBits % OutputBits));

                typedef InputEndian<UnitBits> InputEndianness;
                typedef OutputEndian<UnitBits> OutputEndianness;
                typedef imploder_step<InputEndianness, OutputEndianness, UnitBits, InputBits, OutputBits, k> step_type;
                typedef imploder<InputEndianness, OutputEndianness, InputBits, OutputBits, k + InputBits> next_type;

                template<typename InIter, typename OutputValue>
                inline static void implode(InIter &in, OutputValue &x) {
                    step_type::step(*in++, x);
                    next_type::implode(in, x);
                }
            };

            template<template<int> class InputEndian, template<int> class OutputEndian, int UnitBits, int InputBits,
                     int OutputBits>
            struct imploder<InputEndian<UnitBits>, OutputEndian<UnitBits>, InputBits, OutputBits, OutputBits> {
                template<typename InIter, typename OutputValue>
                inline static void implode(InIter &, OutputValue &) {
                }
            };

        }    // namespace detail
    }        // namespace crypto3
}    // namespace boost

#endif    // CRYPTO3_DETAIL_IMPLODER_HPP