/**
* @file  AdBidStrategy.cpp
* @brief Implementation to class AdBidStrategy.
*/

#include <functional>
#include <limits>
#include <algorithm>
#include <utility>
#include <set>
#include <queue>
#include <cmath>
#include <time.h>
#include <cstdlib>
#include <cstdio>
#include <boost/unordered_map.hpp>
#include <boost/multi_array.hpp>
#include "AdBidStrategy.h"

namespace sf1r
{

namespace sponsored
{

static const double E = 2.71828;

AdBidStrategy::AdBidStrategy()
{

}

AdBidStrategy::~AdBidStrategy()
{

}

static bool isZero(double f)
{
    return std::abs(f) < std::numeric_limits<double>::epsilon();
}

struct Point
{
    double x;
    double y;

    Point(double X = 0.0, double Y = 0.0):x(X), y(Y) {}
};

static bool operator<(const Point& p1, const Point& p2)
{
    return (p1.x < p2.x) || (isZero(p1.x - p2.x) && p1.y < p2.y);
}

static bool xsmall(const Point& p1, const Point& p2)
{
    return (p1.x < p2.x) ;
}

//judge the turning direction for line p0->p1->p2 at point p1.
//turn left return 1, turn right return -1, in the same line return 0.
static int turnDirection(const Point& p0, const Point& p1, const Point& p2)
{
    double tmp = (p2.x - p0.x)*(p1.y - p0.y) - (p2.y - p0.y)*(p1.x - p0.x);
    if (tmp < 0) return 1;
    else if (tmp > 0) return -1;
    else return 0;
}

class AnglePredicate
{
public:
    AnglePredicate(const Point& p): base_(p) {}

    bool operator()(const Point& p1, const Point& p2)
    {
        int tmp = turnDirection(base_, p1, p2);
        return (tmp > 0) || (tmp == 0 &&
                             (p1.x - base_.x)*(p1.x - base_.x) + (p1.y - base_.y)*(p1.y - base_.y) < (p2.x - base_.x)*(p2.x - base_.x) + (p2.y - base_.y)*(p2.y - base_.y)
                            );
    }

private:
    Point base_;
};

std::vector<std::pair<int, double> > AdBidStrategy::convexUniformBid( const std::vector<AdQueryStatisticInfo>& qsInfos, int budget)
{
    static const std::vector<std::pair<int, double> > NULLBID_(2, std::make_pair(0, 0.0));

    if (qsInfos.empty())
    {
        return NULLBID_;
    }

    //aggregate landscape
    boost::unordered_map<int, Point> landscape; //(cpc, <cost, clicks>)
    for (std::vector<AdQueryStatisticInfo>::const_iterator cit = qsInfos.begin(); cit != qsInfos.end(); ++cit)
    {
        std::vector<int>::const_iterator cpcit = cit->cpc_.begin();
        std::vector<double>::const_iterator ctrit = cit->ctr_.begin();
        for (; cpcit != cit->cpc_.end() && ctrit != cit->ctr_.end(); ++cpcit, ++ctrit)
        {
            Point& curP = landscape[*cpcit];
            curP.x += (*cpcit) * (*ctrit);
            curP.y += (*ctrit);
        }
    }

    std::vector<struct Point> allPoints;
    allPoints.reserve(landscape.size());
    Point minP(std::numeric_limits<double>::max(), std::numeric_limits<double>::max());
    size_t minI = -1;
    for (boost::unordered_map<int, Point>::const_iterator cit = landscape.begin(); cit != landscape.end(); ++cit)
    {
        if (cit->second.y > 0 && cit->second.x > 0)
        {
            allPoints.push_back(cit->second);

            if (cit->second < minP)
            {
                minP = cit->second;
                minI = allPoints.size() - 1;
            }
        }
    }
    //   landscape.clear();

    if (allPoints.empty())
    {
        std::vector<std::pair<int, double> > minBID_(2, std::make_pair(qsInfos.front().minBid_, 0.5));
        return minBID_;
    }

    std::swap(allPoints[0], allPoints[minI]);

    AnglePredicate ap(minP);
    std::sort(++allPoints.begin(), allPoints.end(), ap);

    //convex hull, Graham's Scan Algorithm.
    std::vector<struct Point> ch;
    int i = 0;
    while (i < 2 && i < (int)allPoints.size())
    {
        //init
        ch.push_back(allPoints[i++]);
    }

    for (; i < (int)allPoints.size(); ++i)
    {
        while(ch.size() >= 2)
        {
            int tmp = turnDirection(ch[ch.size() -2], ch[ch.size() - 1], allPoints[i]);
            if (tmp < 0)
            {
                ch.pop_back();
            }
            else
            {
                break;
            }
        }
        ch.push_back(allPoints[i]);
    }
    allPoints.clear();

    //candidate convex hull points for bid
    std::vector<struct Point> canch;
    int j = 0;
    for (; j < (int)ch.size(); ++j)
    {
        if (j < ((int)ch.size()) - 1 && ch[j] < ch[j+1])
        {
            continue;
        }
        else
        {
            break;
        }
    }

    //upper bipart of convex hull.
    canch.reserve(ch.size() - j + 2);
    canch.push_back(Point()); //zero point.
    if (ch.size()>= 2 && !isZero(ch.front().x - ch.back().x))
    {
        canch.push_back(ch[0]);
    }

    for (int i = ch.size() -1; i >= j; --i)
    {
        canch.push_back(ch[i]);
    }

    ch.clear();

    //convex combination
    std::vector<std::pair<int, double> > bid;
    int minBid = qsInfos.front().minBid_;

    int singleBudget = 1000000000;
    int totalImpression = 0;
    for (std::vector<AdQueryStatisticInfo>::const_iterator cit = qsInfos.begin(); cit != qsInfos.end(); ++cit)
    {
        totalImpression += cit->impression_;
        singleBudget = std::min(cit->minBid_, singleBudget);
    }
    if (totalImpression > 0)
    {
        singleBudget = std::max(singleBudget, (int)(budget / (totalImpression / (double)qsInfos.size())));
    }

    Point budgetPoint(singleBudget, 0.0);
    std::vector<struct Point>::const_iterator uit = std::upper_bound(canch.begin(), canch.end(), budgetPoint, xsmall);
    if (uit == canch.end())
    {
        int mybid = std::max(minBid, singleBudget);
        bid.push_back(std::make_pair(mybid, 0.5));
        bid.push_back(std::make_pair(mybid, 0.5));
    }
    else
    {
        std::vector<struct Point>::const_iterator preit = uit - 1;
        if (isZero(preit->x - singleBudget))
        {
            int mybid = isZero(preit->y) ? 0 : int(preit->x / preit->y);
            mybid = std::max(minBid, mybid);
            bid.push_back(std::make_pair(mybid, 0.5));
            bid.push_back(std::make_pair(mybid, 0.5));
        }
        else
        {
            double p = double((singleBudget - preit->x) / (uit->x - preit->x));
            int prebid = int(isZero(preit->y)? 0 : preit->x / preit->y);
            if(prebid > 0)
                prebid = std::max(prebid, minBid);
            int lastbid = std::max(int(uit->x / uit->y), minBid);

            bid.push_back(std::make_pair(prebid, p));
            bid.push_back(std::make_pair(lastbid, 1.0 - p));
        }
    }

    return bid;
}

int AdBidStrategy::realtimeBidWithRevenueMax(const AdQueryStatisticInfo& qsInfo,  int budgetUsed, int budgetLeft, int vpc /*= 1000*/)
{
    int budget = budgetLeft + budgetUsed;
    if (budget <= 0 || qsInfo.cpc_.empty())
    {
        return 0;
    }

    double U = std::numeric_limits<double>::max();
    int minBid = qsInfo.minBid_;
    if (minBid < 0)
    {
        minBid = 0;
    }
    if (minBid > 0)
    {
        U = ((double)vpc) / minBid;
    }

    double z = ((double)budgetUsed) / budget;

    //efficiency
    double eff = pow(U * E, z) / E;

    for (int i = 0; i < (int)qsInfo.cpc_.size(); ++i)
    {
        double avaiableB = budgetLeft / (qsInfo.ctr_[i] * qsInfo.impression_);
        double myeff = ((double)vpc) / qsInfo.cpc_[i];
        if (eff > myeff && qsInfo.cpc_[i] <= avaiableB)
        {
            eff = myeff;
        }
    }

    for (int i = 0; i < (int)qsInfo.cpc_.size(); ++i)
    {
        double myeff = ((double)vpc) / qsInfo.cpc_[i];
        if (myeff >= eff)
        {
            return qsInfo.cpc_[i];
        }
    }

    return int(vpc / (1 + eff));
}

int AdBidStrategy::realtimeBidWithProfitMax( const AdQueryStatisticInfo& qsInfo, int budgetUsed, int budgetLeft, int vpc /*= 1000*/)
{
    static const double MINEFF = 0.1;

    int budget = budgetLeft + budgetUsed;
    if (budget <= 0 || qsInfo.cpc_.empty())
    {
        return 0;
    }

    double U = std::numeric_limits<double>::max();
    int minBid = qsInfo.minBid_;
    if (minBid < 0)
    {
        minBid = 0;
    }
    if (minBid > 0)
    {
        U = ((double)vpc) / minBid - 1;
    }

    double z = ((double)budgetUsed) / budget;

    //efficiency
    double eff = pow(U * E / MINEFF, z) * MINEFF / E;

    for (int i = 0; i < (int)qsInfo.cpc_.size(); ++i)
    {
        double avaiableB = budgetLeft / (qsInfo.ctr_[i] * qsInfo.impression_);
        double myeff = ((double)vpc) / qsInfo.cpc_[i] - 1;
        if (eff > myeff && qsInfo.cpc_[i] <= avaiableB)
        {
            eff = myeff;
        }
    }

    int maxI = -1;
    double maxV = -1.0;
    for (int i = 0; i < (int)qsInfo.cpc_.size(); ++i)
    {
        double myeff = ((double)vpc) / qsInfo.cpc_[i] - 1;
        if (myeff >= eff)
        {
            double myV = (vpc - qsInfo.cpc_[i]) * qsInfo.ctr_[i];
            if (myV > maxV)
            {
                maxV = myV;
                maxI = i;
            }
        }
    }
    if (maxI >= 0)
    {
        return qsInfo.cpc_[maxI];
    }
    else
        return int(vpc / (1 + eff));
}

class LargerFit
{
public:
    LargerFit(const std::vector<double>& Fit): _fit(Fit) {}
    bool operator()(int l, int r)
    {
        return _fit[l] < _fit[r];
    }

private:
    const std::vector<double>& _fit;
};

static void enumKPRecursive(const std::vector<std::vector<double> >& W, const std::vector<std::vector<double> >& V, int i, std::vector<int>&curSol, double leftB, double curValue, double& maxValue, std::vector<int>& maxSol)
{
    if(i >= (int) W.size())
    {
        if(curValue > maxValue)
        {
            maxValue = curValue;
            maxSol = curSol;
        }
        return;
    }

    //do not choose cur item.
    curSol[i] = -1;
    enumKPRecursive(W, V, i+1, curSol, leftB, curValue, maxValue, maxSol);

    for(int j = 0; j < (int)W[i].size(); ++j)
    {
        if(leftB >= W[i][j])
        {
            curSol[i] = j;
            enumKPRecursive(W, V, i+1, curSol, leftB - W[i][j], curValue + V[i][j], maxValue, maxSol);
        }
    }
}

static std::vector<int> enumKP(const std::vector<std::vector<double> >& W, const std::vector<std::vector<double> >& V, int B)
{
    std::vector<int> curSol(W.size(), -1);
    std::vector<int> maxSol(W.size(), -1);
    double maxValue = 0.0;

    enumKPRecursive(W, V, 0, curSol, B, 0.0, maxValue, maxSol);

    return maxSol;
}

//dynamic programming for knapsack problem
static std::vector<int> dpKP(const std::vector<std::vector<double> >& W, const std::vector<std::vector<double> >& V, int B)
{
    typedef boost::multi_array<int, 2> array_type;
    typedef array_type::index index;
    array_type	S(boost::extents[W.size()][B+1]);
    std::vector<double> F(B + 1, 0.0);

    for(int i = 0; i < (int)W.size(); ++i)
    {
        for(int v = B; v >= 0; --v)
        {
            double maxF = F[v];
            int maxI = -1;
            for(int j = 0; j < (int)W[i].size(); ++j)
            {
                if(v >= W[i][j])
                {
                    double myF = F[v - W[i][j]] + V[i][j];
                    if(myF > maxF)
                    {
                        maxF = myF;
                        maxI = j;
                    }
                }
            }
            F[v] = maxF;
            S[i][v] = maxI;
        }
    }

    //construct solution.
    std::vector<int> Sol(W.size(), -1);
    for(int i = W.size() - 1, v = B; i >= 0; --i)
    {
        Sol[i] = S[i][v];
        if(S[i][v] != -1)
        {
            v = (int)(v - W[i][Sol[i]]);
        }
    }

    return Sol;
}

static void convertIndexToBid(const std::vector<AdQueryStatisticInfo>& qsInfos, const std::vector<int>& bidindex, std::vector<int>& bid)
{
    std::vector<AdQueryStatisticInfo>::const_iterator cit = qsInfos.begin();
    int i = 0;
    int bi = 0;
    for (; cit != qsInfos.end(); ++cit, ++i)
    {
        if (cit->bid_ != -1)
        {
            bid[i] = cit->bid_;
        }
        else
        {
            if (bidindex[bi] != -1)
            {
                bid[i] = cit->cpc_[bidindex[bi]];
            }
            else
            {
                bid[i] = 0;
            }

            ++bi;
        }
    }

}

//for debug.
static double computeValue(const std::vector<AdQueryStatisticInfo>& qsInfos, const std::vector<int>& sol)
{
    double myV = 0.0;
    std::vector<AdQueryStatisticInfo>::const_iterator cit = qsInfos.begin();
    std::vector<int>::const_iterator sit = sol.begin();
    for (; cit != qsInfos.end(); ++cit)
    {
        if (cit->bid_ == -1)
        {
            if (*sit != -1)
            {
                myV += cit->ctr_[*sit] * cit->impression_;
            }
            ++sit;
        }
    }

    return myV;
}

static void checkGABid(const std::vector<AdQueryStatisticInfo>& qsInfos, int budget, std::vector<int>& bid)
{
    bool allZeroFlag = true;
    std::vector<AdQueryStatisticInfo>::const_iterator qsIt = qsInfos.begin();
    for (std::vector<int>::iterator it = bid.begin(); it != bid.end(); ++it, ++qsIt)
    {
        if (*it > 0)
        {
            if (*it < qsIt->minBid_)
            {
                *it = qsIt->minBid_;
            }

            allZeroFlag = false;
        }
    }
    if (!allZeroFlag)
    {
        return;
    }

    int minBid = qsInfos.front().minBid_;
    std::vector<std::pair<int, double> > ubid = AdBidStrategy::convexUniformBid(qsInfos, budget);
    int lastBid = int(ubid.front().first * ubid.front().second + ubid.back().first * ubid.back().second);
    lastBid = std::max(lastBid, minBid);
    for (std::vector<int>::iterator it = bid.begin(); it != bid.end(); ++it)
    {
        *it = lastBid;
    }
}

std::vector<int> AdBidStrategy::geneticBid( const std::vector<AdQueryStatisticInfo>& qsInfos, int budget )
{
    std::vector<int> bid(qsInfos.size(), 0);

    //support for predefined bidding.
    int tmpKeywordNum = 0, tmpBidIndex = 0, tmpBudget = budget;
    for (std::vector<AdQueryStatisticInfo>::const_iterator cit = qsInfos.begin(); cit != qsInfos.end(); ++cit, ++tmpBidIndex)
    {
        if (cit->bid_ == -1)
        {
            ++tmpKeywordNum;
            bid[tmpBidIndex] = 0;
        }
        else
        {
            bid[tmpBidIndex] = cit->bid_;
            int i = 0;
            for (; i <(int)cit->cpc_.size(); ++i)
            {
                if (cit->bid_ >= cit->cpc_[i])
                {
                    break;
                }
            }
            if (i < (int)cit->cpc_.size())
            {
                tmpBudget -= cit->cpc_[i] * cit->ctr_[i] * cit->impression_;
            }
        }
    }

    const int KeywordNum = tmpKeywordNum;
    const int AvaiableBudget = tmpBudget;

    const int MaxAllowedEvolutions = 300 * KeywordNum;
    const int MinAllowedEvolutions = 50 * KeywordNum;
    static const double EndPopulationRate = 0.90;  //when 90% of the population has same fitness value, stop evolute.
    static const int PopulationSize = 40; //must be even
    static const int ElitismSize = 2;
    static const double MinFitVariance = 0.001;  //minimum max fitness variance ratio. variance / fit^2
    static const long long MaxLoopNum = 10000000000;
    static const long long MaxDPSpace = 100000000;

    if (KeywordNum <= 0 || AvaiableBudget <= 0)
    {
        return bid;
    }

    typedef std::vector<std::vector<double> > TQSDataType;
    TQSDataType W(KeywordNum), V(KeywordNum);
    std::vector<int> adP(KeywordNum);  //ad position num for each keyword.
    int kNum = 0;
    for (std::vector<AdQueryStatisticInfo>::const_iterator cit = qsInfos.begin(); cit != qsInfos.end(); ++cit)
    {
        if (cit->bid_ != -1)
        {
            continue;
        }

        const std::vector<int>& cpc = cit->cpc_;
        const std::vector<double>& ctr = cit->ctr_;

        W[kNum].reserve(cpc.size());
        V[kNum].reserve(cpc.size());
        adP[kNum] = cpc.size();

        for (int j = 0; j < (int)cpc.size(); ++j)
        {
            W[kNum].push_back(cpc[j] * cit->impression_ * ctr[j]);
            V[kNum].push_back(cit->impression_ * ctr[j]);  //max traffics. value is defined as click traffics.
        }

        ++kNum;
    }

    // judge whether problem can be solved by enumerating to directly get optimal solution.
    {
        long long timeCP = 1;
        for(int i = 0; i < KeywordNum; ++i)
        {
            timeCP *= (W[i].size() + 1);
            if(timeCP > MaxLoopNum) break;
        }
        if(timeCP < MaxLoopNum)
        {
            const std::vector<int>& mySol = enumKP(W, V, AvaiableBudget);
            convertIndexToBid(qsInfos, mySol, bid);
            checkGABid(qsInfos, budget, bid);
            return bid;
        }
    }

    // judge whether problem can be solved by dynamic programming to directly get optimal solution.
    {
        long long spaceComplexity = KeywordNum;
        spaceComplexity *= (AvaiableBudget + 1);
        if (spaceComplexity <= MaxDPSpace)
        {
            const std::vector<int>& mySol = dpKP(W, V, AvaiableBudget);
            convertIndexToBid(qsInfos, mySol, bid);
            checkGABid(qsInfos, budget, bid);
            return bid;
        }
    }

    typedef std::vector<std::vector<int> > TPopType; //every individual is a vector of index of ad position for each keyword, 0-based.
    TPopType P(PopulationSize);
    TPopType newP(PopulationSize);

    srand(time(NULL));
    for (int i = 0; i < PopulationSize; ++i)
    {
        P[i].reserve(KeywordNum);
        newP[i].reserve(KeywordNum);
        for (size_t j = 0; j < KeywordNum; ++j)
        {
            int N = adP[j] + 1;
            P[i].push_back(rand() % N - 1); //ad position is 0-based, -1 means do not bid for that keyword.
            newP[i].push_back(0);
        }

        //clear population for budget requirement.
        for (int i = 0; i < PopulationSize; ++i)
        {
            std::vector<std::pair<int, int> > kw; //(keyword index, selected ad position index)
            for (int j = 0; j < (int)(P[i].size()); ++j)
            {
                //for each keyword
                if (P[i][j] != -1)
                {
                    kw.push_back(std::make_pair(j, P[i][j]));
                }
            }

            double totalW = 0.0;
            for (std::vector<std::pair<int, int> >::const_iterator cit = kw.begin(); cit != kw.end(); ++cit)
            {
                totalW += W[cit->first][cit->second];
            }

            int aN = kw.size();
            while(totalW > AvaiableBudget)
            {
                int r = rand() % aN;
                totalW -= W[kw[r].first][kw[r].second];
                P[i][kw[r].first] = -1;
                std::swap(kw[r], kw[aN - 1]);
                --aN;
            }
        }
    }

    int iterNum = MaxAllowedEvolutions;
    double averageFit = 0.0, averageSquareFit = 0.0;

    while(iterNum--)
    {
        //selection,
        std::vector<int> SP(PopulationSize); //selected individual's index in P
        int GASize = PopulationSize - ElitismSize;

        {
            std::vector<double> fitness(PopulationSize); //total fitness.
            std::vector<double> fit(PopulationSize);    //each individual fitness.
            double fn = 0.0;
            for (int i = 0; i < PopulationSize; ++i)
            {
                double curfn = 0.0;
                for (int j = 0; j < (int)(P[i].size()); ++j)
                {
                    //for each keyword
                    if (P[i][j] != -1)
                    {
                        curfn += V[j][P[i][j]];
                    }
                }
                fn += curfn;
                fit[i] = curfn;
                fitness[i] = fn;
            }

            //check condition of stop evolution.
            if (MaxAllowedEvolutions - iterNum > MinAllowedEvolutions)
            {
                boost::unordered_map<double, int> fitNum;
                for (int i = 0; i < PopulationSize; ++i)
                {
                    fitNum[fit[i]]++;
                }
                int maxNum = -1;
                for (boost::unordered_map<double, int>::const_iterator cit = fitNum.begin(); cit != fitNum.end(); ++cit)
                {
                    if (cit->second > maxNum)
                    {
                        maxNum = cit->second;
                    }
                }

                if ((double)maxNum / PopulationSize >= EndPopulationRate)
                {
                    break;
                }
            }

            //stochastic universal sampling.
            double fstep = fn / GASize;
            double fstart = ((double)rand()) / RAND_MAX * fstep;

            if (!isZero(fstep))
            {
                for (int i = 0, j = 0; i < GASize; ++i)
                {
                    double fcur = fstart + i * fstep;
                    for (int k = j; k < PopulationSize; ++k)
                    {
                        if (fitness[k] > fcur)
                        {
                            SP[i] = k;
                            j = k;
                            break;
                        }
                    }
                }
            }
            else
            {
                for (int i = 0; i < PopulationSize; ++i)
                {
                    SP[i] = i;
                }
            }

            //random to select crossover pair
            for (int i = GASize; i >= 1; --i)
            {
                int j = rand() % i;
                std::swap(SP[j], SP[i - 1]);
            }

            //elitism
            LargerFit mylf(fit);
            std::priority_queue<int, std::vector<int>, LargerFit> pq(mylf);
            for (int i = 0; i < PopulationSize; ++i)
            {
                pq.push(i);
            }

            {
                //average fit
                int eNum = MaxAllowedEvolutions - iterNum;
                double curMaxFit = fit[pq.top()];
                if (!isZero(curMaxFit))
                {
                    averageFit = ((eNum - 1) * averageFit + curMaxFit) / eNum;
                    averageSquareFit = ((eNum - 1) * averageSquareFit + curMaxFit * curMaxFit) / eNum;
                    //variance
                    double variance = averageSquareFit - averageFit * averageFit;
                    if (eNum > MinAllowedEvolutions && variance / (curMaxFit*curMaxFit) < MinFitVariance)
                    {
                        break;
                    }
                }
            }

            for (int i = GASize; i < PopulationSize; ++i)
            {
                SP[i] = pq.top();
                pq.pop();
            }

        }

        //crossover
        int crossoverRate = 85;
        for (int i = 0; i < GASize; i += 2)
        {
            int myRate = rand() % 100;
            if (myRate < crossoverRate)
            {
                const std::vector<int>& lp = P[SP[i]];
                const std::vector<int>& rp = P[SP[i+1]];

                for (int j = 0; j < KeywordNum; ++j)
                {
                    double p = (double)rand() / RAND_MAX * 1.50 - 0.25;

                    if (lp[j] != -1 && rp[j] != -1)
                    {
                        //newP[i][j]
                        int v = (int)(lp[j] * p + rp[j] * (1.0 - p) + 0.5);
                        v = std::max(v, 0);
                        v = std::min(v, adP[j] - 1);
                        newP[i][j] = v;

                        //newP[i+1]
                        //newP[i]
                        v = (int)(lp[j] * ( 1.0 - p ) + rp[j] * p + 0.5);
                        v = std::max(v, 0);
                        v = std::min(v, adP[j] - 1);
                        newP[i+1][j] = v;
                    }
                    else if (lp[j] == -1 && rp[j] == -1)
                    {
                        newP[i][j] = -1;
                        newP[i+1][j] = -1;
                    }
                    else
                    {
                        //discrete
                        int ip = rand() % 2;
                        if (ip)
                        {
                            newP[i][j] = -1;
                            newP[i+1][j] = -1;
                        }
                        else
                        {
                            int tmp = std::max(lp[j], rp[j]);
                            newP[i][j] = tmp;
                            newP[i+1][j] = tmp;
                        }
                    }

                }
            }
            else
            {
                newP[i] = P[SP[i]];
                newP[i+1] = P[SP[i+1]];
            }
        }
        for (int i = GASize; i < PopulationSize; ++i)
        {
            newP[i] = P[SP[i]];
        }

        //mutation, mutation rate, 1/countof(var)
        int mRate = KeywordNum;
        for (int i = 0; i < GASize; ++i)
        {
            for (int j = 0; j < KeywordNum; ++j)
            {
                if (rand() % mRate == 0)
                {
                    //do mutation
                    newP[i][j] = rand() % (adP[j] + 1) - 1;
                }
            }
        }

        newP.swap(P);

        //clear population for budget requirement.
        for (int i = 0; i < PopulationSize; ++i)
        {
            std::vector<std::pair<int, int> > kw; //(keyword index, selected ad position index)
            for (int j = 0; j < (int)(P[i].size()); ++j)
            {
                //for each keyword
                if (P[i][j] != -1)
                {
                    kw.push_back(std::make_pair(j, P[i][j]));
                }
            }

            double totalW = 0.0;
            for (std::vector<std::pair<int, int> >::const_iterator cit = kw.begin(); cit != kw.end(); ++cit)
            {
                totalW += W[cit->first][cit->second];
            }

            int aN = kw.size();
            while(totalW > AvaiableBudget)
            {
                int r = rand() % aN;
                totalW -= W[kw[r].first][kw[r].second];
                P[i][kw[r].first] = -1;
                std::swap(kw[r], kw[aN - 1]);
                --aN;
            }
        }

    }

    //max fitness in population
    int maxI = -1;
    double maxfit = -1.0;
    for (int i = 0; i < PopulationSize; ++i)
    {
        double ft = 0.0;
        for (int j = 0; j < KeywordNum; ++j)
        {
            if (P[i][j] != -1)
            {
                ft += V[j][P[i][j]];
            }
        }
        if (ft > maxfit)
        {
            maxfit = ft;
            maxI = i;
        }
    }

    if (maxI != -1)
    {
        convertIndexToBid(qsInfos, P[maxI], bid);

    }

    checkGABid(qsInfos, budget, bid);
    return bid;
}

}
}


#include <omp.h>
#include <Eigen/Sparse>
#include <algorithm>
#include <boost/lexical_cast.hpp>
#include <boost/multi_array.hpp>
#include <boost/program_options.hpp>
#include <functional>
#include <iomanip>
#include <iostream>
// own includes ------------------------------------------------------------
#include "bte_config.h"
#include "collision_tensor/assembly/collision_tensor_exporter.hpp"
#include "collision_tensor/assembly/ct_factory.hpp"
#include "quadrature/qmaxwell.hpp"
#include "quadrature/qmidpoint.hpp"
#include "spectral/basis/spectral_basis_factory_ks.hpp"

using namespace std;
using namespace boltzmann;
namespace po = boost::program_options;

typedef QMaxwell QRadial;
typedef SpectralBasisFactoryKS basis_factory_t;

int main(int argc, char* argv[])
{
  double dt;
  int L, K, nptsa, nptsr, nptsi;
  string tensor_file;

  po::options_description options("options");
  options.add_options()
      ("help", "produce help message")
      (",K", po::value<int>(&K)->default_value(2), "Polynomial degree K")
      ("pg", "Petrov-Galerkin formulation")
      ("nptsr,r", po::value<int>(&nptsr)->default_value(21), "#quad. points in radius")
      ("nptsa,a", po::value<int>(&nptsa)->default_value(21), "#quad. points in angle")
      ("nptsi,i", po::value<int>(&nptsi)->default_value(81), "#quad. points for inner loop");
  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, options), vm);
  po::notify(vm);

  if (vm.count("help")) {
    cout << options << "\n";
    return 0;
  }

#ifdef GIT_SHA1
  std::string version_id = GIT_SHA1;
  cout << "\nVersionID: " << version_id << "@" << GIT_BNAME << std::endl;
#endif

  cout << "Calling arguments:\n"
       << right << setw(15) << "K" << setw(12) << K << endl
       << right << setw(15) << "#quad. points radius" << setw(12) << nptsr << endl
       << right << setw(15) << "#quad. points angle" << setw(12) << nptsa << endl
       << right << setw(15) << "#quad. points inner" << setw(12) << nptsi << endl
       << "\n\n\n";

  // Create test & trial basis
  typedef typename basis_factory_t::basis_type basis_type;
  // trial space
  basis_type trial_basis;
  basis_factory_t::create(trial_basis, K);
  basis_factory_t::write_basis_descriptor(trial_basis);
  // test space
  basis_type test_basis;
  if (vm.count("pg")) {
    cout << "Petrov-Galerkin\n";
    basis_factory_t::create_test(test_basis, K);
  } else {
    basis_factory_t::create(test_basis, K);
  }
  basis_factory_t::write_basis_descriptor(test_basis, "spectral_basis_test.desc");

  typedef collision_tensor_assembly::
      CollisionOperator<basis_type, KERNEL_TYPE::MAXWELLIAN, QMidpoint, QRadial>
          collision_operator_t;

  CollisionTensorExporter exporter("collision_tensor.h5");

  collision_operator_t collision_operator(test_basis, trial_basis, nptsa, nptsr, nptsi);

  std::vector<unsigned int> work;
  for (unsigned int i = 0; i < trial_basis.n_dofs(); ++i) {
    work.push_back(i);
  }

  collision_operator.compute(exporter, work);

  return 0;
}


#pragma once

#include <Eigen/Dense>
#include <stdexcept>
#include <boost/foreach.hpp>

namespace argus
{

VectorType flatten_matrices( const std::vector<MatrixType>& mats )
{
	unsigned int n = 0;
	BOOST_FOREACH( const MatrixType& mat, mats )
	{
		n += mat.size();
	}
	VectorType out(n);
	unsigned int ind = 0;
	BOOST_FOREACH( const MatrixType& mat, mats )
	{
		Eigen::Map<const VectorType> v( mat.data(), mat.size(), 1 );
		out.segment(ind, mat.size()) = v;
		ind += v.size();
	}
	return out;
}

MatrixType vstack_matrices( const std::vector<MatrixType>& mats )
{
	unsigned int rows = 0;
	unsigned int cols = mats[0].cols();

	BOOST_FOREACH( const MatrixType& mat, mats )
	{
		rows += mat.rows();
		if( mat.cols() != cols )
		{
			throw std::invalid_argument( "Not all mats same width!" );
		}
	}

	MatrixType out( rows, cols );
	unsigned int ind = 0;
	BOOST_FOREACH( const MatrixType& mat, mats )
	{
		out.block(ind, 0, mat.rows(), mat.cols() ) = mat;
		ind += mat.rows();
	}
	return out;
}

MatrixType hstack_matrices( const std::vector<MatrixType>& mats )
{
	unsigned int rows = mats[0].rows();
	unsigned int cols = 0;

	BOOST_FOREACH( const MatrixType& mat, mats )
	{
		cols += mat.cols();
		if( mat.rows() != rows )
		{
			throw std::invalid_argument( "Not all mats same height!" );
		}
	}

	MatrixType out( rows, cols );
	unsigned int ind = 0;
	BOOST_FOREACH( const MatrixType& mat, mats )
	{
		out.block(0, ind, mat.rows(), mat.cols() ) = mat;
		ind += mat.cols();
	}
	return out;
}

// template <class Derived>
// Derived ConcatenateHor( const Eigen::DenseBase<Derived>& l,
//                         const Eigen::DenseBase<Derived>& r )
// {
// 	if( l.size() == 0 )
// 	{
// 		return r;
// 	}
// 	if( r.size() == 0 )
// 	{
// 		return l;
// 	}

// 	if( l.rows() != r.rows() )
// 	{
// 		throw std::runtime_error( "ConcatenateHor: Dimension mismatch." );
// 	}
// 	Derived out( l.rows(), l.cols() + r.cols() );
// 	out.leftCols( l.cols() ) = l;
// 	out.rightCols( r.cols() ) = r;
// 	return out;
// }

// template <class Derived>
// Derived ConcatenateVer( const Eigen::DenseBase<Derived>& l,
//                         const Eigen::DenseBase<Derived>& r )
// {
// 	if( l.size() == 0 )
// 	{
// 		return r;
// 	}
// 	if( r.size() == 0 )
// 	{
// 		return l;
// 	}

// 	if( l.cols() != r.cols() )
// 	{
// 		throw std::runtime_error( "ConcatenateVer: Dimension mismatch." );
// 	}
// 	Derived out( l.rows() + r.rows(), l.cols() );
// 	out.topRows( l.rows() ) = l;
// 	out.bottomRows( r.rows() ) = r;
// 	return out;
// }

}


#define BOOST_TEST_MODULE "parse_integer_test"
#include <toml/parser.hpp>
#include <boost/test/included/unit_test.hpp>
#include <iostream>
#include <iomanip>
#include "parse_aux.hpp"

using namespace toml;
using namespace detail;

BOOST_AUTO_TEST_CASE(test_decimal)
{
    TOML_PARSE_CHECK_EQUAL(parse_integer,        "1234", integer,       1234);
    TOML_PARSE_CHECK_EQUAL(parse_integer,       "+1234", integer,       1234);
    TOML_PARSE_CHECK_EQUAL(parse_integer,       "-1234", integer,      -1234);
    TOML_PARSE_CHECK_EQUAL(parse_integer,           "0", integer,          0);
    TOML_PARSE_CHECK_EQUAL(parse_integer,     "1_2_3_4", integer,       1234);
    TOML_PARSE_CHECK_EQUAL(parse_integer,    "+1_2_3_4", integer,      +1234);
    TOML_PARSE_CHECK_EQUAL(parse_integer,    "-1_2_3_4", integer,      -1234);
    TOML_PARSE_CHECK_EQUAL(parse_integer, "123_456_789", integer,  123456789);
}

BOOST_AUTO_TEST_CASE(test_decimal_value)
{
    TOML_PARSE_CHECK_EQUAL(parse_value,        "1234", value, toml::value(     1234));
    TOML_PARSE_CHECK_EQUAL(parse_value,       "+1234", value, toml::value(     1234));
    TOML_PARSE_CHECK_EQUAL(parse_value,       "-1234", value, toml::value(    -1234));
    TOML_PARSE_CHECK_EQUAL(parse_value,           "0", value, toml::value(        0));
    TOML_PARSE_CHECK_EQUAL(parse_value,     "1_2_3_4", value, toml::value(     1234));
    TOML_PARSE_CHECK_EQUAL(parse_value,    "+1_2_3_4", value, toml::value(    +1234));
    TOML_PARSE_CHECK_EQUAL(parse_value,    "-1_2_3_4", value, toml::value(    -1234));
    TOML_PARSE_CHECK_EQUAL(parse_value, "123_456_789", value, toml::value(123456789));
}

BOOST_AUTO_TEST_CASE(test_hex)
{
    TOML_PARSE_CHECK_EQUAL(parse_integer, "0xDEADBEEF",  integer, 0xDEADBEEF);
    TOML_PARSE_CHECK_EQUAL(parse_integer, "0xdeadbeef",  integer, 0xDEADBEEF);
    TOML_PARSE_CHECK_EQUAL(parse_integer, "0xDEADbeef",  integer, 0xDEADBEEF);
    TOML_PARSE_CHECK_EQUAL(parse_integer, "0xDEAD_BEEF", integer, 0xDEADBEEF);
    TOML_PARSE_CHECK_EQUAL(parse_integer, "0xdead_beef", integer, 0xDEADBEEF);
    TOML_PARSE_CHECK_EQUAL(parse_integer, "0xdead_BEEF", integer, 0xDEADBEEF);
    TOML_PARSE_CHECK_EQUAL(parse_integer, "0xFF",        integer, 0xFF);
    TOML_PARSE_CHECK_EQUAL(parse_integer, "0x00FF",      integer, 0xFF);
    TOML_PARSE_CHECK_EQUAL(parse_integer, "0x0000FF",    integer, 0xFF);
}

BOOST_AUTO_TEST_CASE(test_hex_value)
{
    TOML_PARSE_CHECK_EQUAL(parse_value, "0xDEADBEEF",  value, value(0xDEADBEEF));
    TOML_PARSE_CHECK_EQUAL(parse_value, "0xdeadbeef",  value, value(0xDEADBEEF));
    TOML_PARSE_CHECK_EQUAL(parse_value, "0xDEADbeef",  value, value(0xDEADBEEF));
    TOML_PARSE_CHECK_EQUAL(parse_value, "0xDEAD_BEEF", value, value(0xDEADBEEF));
    TOML_PARSE_CHECK_EQUAL(parse_value, "0xdead_beef", value, value(0xDEADBEEF));
    TOML_PARSE_CHECK_EQUAL(parse_value, "0xdead_BEEF", value, value(0xDEADBEEF));
    TOML_PARSE_CHECK_EQUAL(parse_value, "0xFF",        value, value(0xFF));
    TOML_PARSE_CHECK_EQUAL(parse_value, "0x00FF",      value, value(0xFF));
    TOML_PARSE_CHECK_EQUAL(parse_value, "0x0000FF",    value, value(0xFF));
}

BOOST_AUTO_TEST_CASE(test_oct)
{
    TOML_PARSE_CHECK_EQUAL(parse_integer, "0o777",   integer, 64*7+8*7+7);
    TOML_PARSE_CHECK_EQUAL(parse_integer, "0o7_7_7", integer, 64*7+8*7+7);
    TOML_PARSE_CHECK_EQUAL(parse_integer, "0o007",   integer, 7);
}

BOOST_AUTO_TEST_CASE(test_oct_value)
{
    TOML_PARSE_CHECK_EQUAL(parse_value, "0o777",   value, value(64*7+8*7+7));
    TOML_PARSE_CHECK_EQUAL(parse_value, "0o7_7_7", value, value(64*7+8*7+7));
    TOML_PARSE_CHECK_EQUAL(parse_value, "0o007",   value, value(7));
}

BOOST_AUTO_TEST_CASE(test_bin)
{
    TOML_PARSE_CHECK_EQUAL(parse_integer, "0b10000",    integer, 16);
    TOML_PARSE_CHECK_EQUAL(parse_integer, "0b010000",   integer, 16);
    TOML_PARSE_CHECK_EQUAL(parse_integer, "0b01_00_00", integer, 16);
    TOML_PARSE_CHECK_EQUAL(parse_integer, "0b111111",   integer, 63);
}

BOOST_AUTO_TEST_CASE(test_bin_value)
{
    TOML_PARSE_CHECK_EQUAL(parse_value, "0b10000",    value, value(16));
    TOML_PARSE_CHECK_EQUAL(parse_value, "0b010000",   value, value(16));
    TOML_PARSE_CHECK_EQUAL(parse_value, "0b01_00_00", value, value(16));
    TOML_PARSE_CHECK_EQUAL(parse_value, "0b111111",   value, value(63));
}


#include <ros/ros.h> // ros
#include <ros/console.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/io/pcd_io.h> // pcl
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/common/transforms.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <string> // std
#include <fstream>
#include <vector>
#include <math.h>
#include <highgui.h> // opencv
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/opencv.hpp>
#include <Eigen/Dense> // eigen
#include <Eigen/Geometry>

int read_record(std::string cloud_src,
                pcl::PointCloud<pcl::PointXYZRGB>& cloud,
		std::string pose_src,
                Eigen::Vector3f& translation,
		Eigen::Affine3f& transform) {
  ROS_INFO_STREAM("Loading " << cloud_src << "...");
  pcl::PCLPointCloud2 pcl_pc2;
  if (pcl::io::loadPCDFile (cloud_src, pcl_pc2) == -1) {
    ROS_ERROR("Load failed");
    return 1;
  }
  pcl::fromPCLPointCloud2(pcl_pc2, cloud);

  ROS_INFO_STREAM("Loading " << pose_src << "...");
  std::ifstream file (pose_src);
  if (file.is_open()) {
    std::string line;
    for (int i=0; i<7; i++) {
      getline(file,line);
    }

    // Retrieve positional information
    getline(file,line);
    translation[0] = stod(line.substr(8,line.size()));
    getline(file,line);
    translation[1] = stod(line.substr(8,line.size()));
    getline(file,line);
    translation[2] = stod(line.substr(8,line.size()));

    // Rotate transform by quaternion
    transform = Eigen::Affine3f::Identity();
    getline(file,line);getline(file,line);
    float qx = stod(line.substr(8,line.size()));
    getline(file,line);
    float qy = stod(line.substr(8,line.size()));
    getline(file,line);
    float qz = stod(line.substr(8,line.size()));
    getline(file,line);
    float qw = stod(line.substr(8,line.size()));
    Eigen::Quaternion<float> q(qw,qx,qy,qz);

    transform.rotate(q);
    transform = transform.inverse();

    file.close();
  } else {
    ROS_ERROR_STREAM("Unable to open " << pose_src << ".txt" );
    return 1;
  }
  return 0;
}

int sp = 200, sa = 400; // map resolution
double max_dist = 8.0;

inline int idx(double x, double m, int s) {
  return fmod(x+m,m) / m * s;
}

cv::Mat map(sp,sa,CV_8UC3, cv::Vec3b(155,155,155)); // gray scale image format
void build_map( pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud ) {
  for (unsigned int i = 0; i < cloud->size(); i++) {
    pcl::PointXYZRGB& p = cloud->points[i];
    double polar = std::atan2(p.z, std::sqrt(p.y*p.y+p.x*p.x));
    double azimuth = std::atan2(p.y, p.x);
    int ip = idx(polar, M_PI, sp);
    int ia = idx(azimuth, 2.0 * M_PI, sa);
    uchar dist = 256 * (std::sqrt(p.x*p.x+p.y*p.y+p.z*p.z)/max_dist);
    map.at<cv::Vec3b>(ip,ia) = cv::Vec3b(dist, 0, 0);
  }
  cv::imwrite( "map.png", map);
}

int main(int argc, char** argv) {

  std::string cloud_prefix = "samples/cloud_", cloud_postfix = ".pcd";
  std::string pose_prefix = "samples/pose_", pose_postfix = ".txt";

  std::vector<std::string> clouds, poses;
  for (int i = 1; i < argc; i++) {
    clouds.push_back(cloud_prefix+std::string(argv[i])+cloud_postfix);
    poses.push_back(pose_prefix+std::string(argv[i])+pose_postfix);
  }

  pcl::visualization::PCLVisualizer viewer("Matrix transform");

  for (int i = 0; i < clouds.size(); i++) {
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
    Eigen::Vector3f translation;
    Eigen::Affine3f transform;
    read_record(clouds[i],*cloud, poses[i], translation, transform);

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed(new pcl::PointCloud<pcl::PointXYZRGB>);
    transform.translation() << translation;
    pcl::transformPointCloud (*cloud, *transformed, transform.inverse());

    build_map(transformed);

    viewer.addPointCloud(transformed, "transformed"+std::to_string(i));
  }
  while (!viewer.wasStopped()) {
    viewer.spinOnce();
  }

  return 0;
}


#define BOOST_TEST_MODULE "test_dynamic_matrix"

#ifdef UNITTEST_FRAMEWORK_LIBRARY_EXIST
#include <boost/test/unit_test.hpp>
#else
#define BOOST_TEST_NO_LIB
#include <boost/test/included/unit_test.hpp>
#endif

#include "../src/DynamicMatrix.hpp"
using MatrixNMd = ax::Matrix<double, ax::DYNAMIC, ax::DYNAMIC>;

#include "test_Defs.hpp"
using ax::test::tolerance;
using ax::test::seed;
using ax::test::Dim_N;
using ax::test::Dim_M;

#include <random>

BOOST_AUTO_TEST_CASE(DynamicMatrix_Ctor)
{
    const MatrixNMd mat(Dim_N, Dim_M);

    for(std::size_t i=0; i<Dim_N; ++i)
        for(std::size_t j=0; j<Dim_M; ++j)
            BOOST_CHECK_EQUAL(mat.at(i,j), 0e0);

    MatrixNMd mat1(Dim_N, Dim_M);

    for(std::size_t i=0; i<Dim_N; ++i)
        for(std::size_t j=0; j<Dim_M; ++j)
            if(i == j)
                mat1.at(i,j) = 1e0;
            else
                mat1.at(i,j) = 0e0;

    const MatrixNMd mat2(mat1);

    for(std::size_t i=0; i<Dim_N; ++i)
        for(std::size_t j=0; j<Dim_M; ++j)
            if(i == j)
                BOOST_CHECK_EQUAL(mat2.at(i,j), 1e0);
            else
                BOOST_CHECK_EQUAL(mat2.at(i,j), 0e0);

    const MatrixNMd mat3 = mat1;

    for(std::size_t i=0; i<Dim_N; ++i)
        for(std::size_t j=0; j<Dim_M; ++j)
            if(i == j)
                BOOST_CHECK_EQUAL(mat3.at(i,j), 1e0);
            else
                BOOST_CHECK_EQUAL(mat3.at(i,j), 0e0);
}

BOOST_AUTO_TEST_CASE(DynamicMatrix_Add)
{
    std::mt19937 mt(seed);
    std::uniform_real_distribution<double> randreal(0e0, 1e0);

    std::vector<std::vector<double>> rand1(Dim_N, std::vector<double>(Dim_M));
    for(std::size_t i=0; i<Dim_N; ++i)
        for(std::size_t j=0; j<Dim_M; ++j)
            rand1.at(i).at(j) = randreal(mt);

    std::vector<std::vector<double>> rand2(Dim_N, std::vector<double>(Dim_M));
    for(std::size_t i=0; i<Dim_N; ++i)
        for(std::size_t j=0; j<Dim_M; ++j)
            rand2.at(i).at(j) = randreal(mt);

    const MatrixNMd mat1(rand1);
    const MatrixNMd mat2(rand2);

    const MatrixNMd mat3(mat1 + mat2);
    for(std::size_t i=0; i<Dim_N; ++i)
        for(std::size_t j=0; j<Dim_M; ++j)
            BOOST_CHECK_EQUAL(mat3.at(i,j), rand1.at(i).at(j) + rand2.at(i).at(j));

    const MatrixNMd mat4 = mat1 + mat2;
    for(std::size_t i=0; i<Dim_N; ++i)
        for(std::size_t j=0; j<Dim_M; ++j)
            BOOST_CHECK_EQUAL(mat4.at(i,j), rand1.at(i).at(j) + rand2.at(i).at(j));

    const MatrixNMd mat5 = mat1 + mat1 + mat1 + mat1 + mat1;
    for(std::size_t i=0; i<Dim_N; ++i)
        for(std::size_t j=0; j<Dim_M; ++j)
            BOOST_CHECK_CLOSE(mat5.at(i,j), rand1.at(i).at(j) * 5, tolerance);
}

BOOST_AUTO_TEST_CASE(DynamicMatrix_Sub)
{
    std::mt19937 mt(seed);
    std::uniform_real_distribution<double> randreal(0e0, 1e0);

    std::vector<std::vector<double>> rand1(Dim_N, std::vector<double>(Dim_M));
    for(std::size_t i=0; i<Dim_N; ++i)
        for(std::size_t j=0; j<Dim_M; ++j)
            rand1.at(i).at(j) = randreal(mt);

    std::vector<std::vector<double>> rand2(Dim_N, std::vector<double>(Dim_M));
    for(std::size_t i=0; i<Dim_N; ++i)
        for(std::size_t j=0; j<Dim_M; ++j)
            rand2.at(i).at(j) = randreal(mt);

    const MatrixNMd mat1(rand1);
    const MatrixNMd mat2(rand2);

    const MatrixNMd mat3 = mat1 - mat2;
    for(std::size_t i=0; i<Dim_N; ++i)
        for(std::size_t j=0; j<Dim_M; ++j)
            BOOST_CHECK_EQUAL(mat3.at(i,j), rand1.at(i).at(j) - rand2.at(i).at(j));

    const MatrixNMd mat4(mat1 - mat2);
    for(std::size_t i=0; i<Dim_N; ++i)
        for(std::size_t j=0; j<Dim_M; ++j)
            BOOST_CHECK_EQUAL(mat4.at(i,j), rand1.at(i).at(j) - rand2.at(i).at(j));

    const MatrixNMd zero(Dim_N, Dim_M);
    const MatrixNMd mat5 =
        zero - mat1 - mat1 - mat1 - mat1 - mat1;

    for(std::size_t i=0; i<Dim_N; ++i)
        for(std::size_t j=0; j<Dim_M; ++j)
            BOOST_CHECK_CLOSE(mat5.at(i,j), rand1.at(i).at(j) * (-5e0), tolerance);
}

BOOST_AUTO_TEST_CASE(DynamicMatrix_Scalar_Multiple)
{
    std::mt19937 mt(seed);
    std::uniform_real_distribution<double> randreal(0e0, 1e0);

    std::vector<std::vector<double>> rand1(Dim_N, std::vector<double>(Dim_M));
    for(std::size_t i=0; i<Dim_N; ++i)
        for(std::size_t j=0; j<Dim_M; ++j)
            rand1.at(i).at(j) = randreal(mt);

    MatrixNMd mat1(rand1);

    const MatrixNMd mat2(mat1 * 2e0);
    for(std::size_t i=0; i<Dim_N; ++i)
        for(std::size_t j=0; j<Dim_M; ++j)
            BOOST_CHECK_EQUAL(mat2.at(i,j), rand1.at(i).at(j) * 2e0);

    const MatrixNMd mat3 = mat1 * 2e0;
    for(std::size_t i=0; i<Dim_N; ++i)
        for(std::size_t j=0; j<Dim_M; ++j)
            BOOST_CHECK_EQUAL(mat3.at(i,j), rand1.at(i).at(j) * 2e0);

    const MatrixNMd mat4(2e0 * mat1);
    for(std::size_t i=0; i<Dim_N; ++i)
        for(std::size_t j=0; j<Dim_M; ++j)
            BOOST_CHECK_EQUAL(mat4.at(i,j), rand1.at(i).at(j) * 2e0);

    const MatrixNMd mat5 = 2e0 * mat1;
    for(std::size_t i=0; i<Dim_N; ++i)
        for(std::size_t j=0; j<Dim_M; ++j)
            BOOST_CHECK_EQUAL(mat5.at(i,j), rand1.at(i).at(j) * 2e0);

    MatrixNMd mat6(Dim_N, Dim_M);
    mat6 = mat1 * 2e0;
    for(std::size_t i=0; i<Dim_N; ++i)
        for(std::size_t j=0; j<Dim_M; ++j)
            BOOST_CHECK_EQUAL(mat6.at(i,j), rand1.at(i).at(j) * 2e0);

    mat6 = 2e0 * mat1;
    for(std::size_t i=0; i<Dim_N; ++i)
        for(std::size_t j=0; j<Dim_M; ++j)
            BOOST_CHECK_EQUAL(mat6.at(i,j), rand1.at(i).at(j) * 2e0);
}

BOOST_AUTO_TEST_CASE(MatrixNd_Scalar_Division)
{
    std::mt19937 mt(seed);
    std::uniform_real_distribution<double> randreal(0e0, 1e0);

    std::vector<std::vector<double>> rand1(Dim_N, std::vector<double>(Dim_M));
    for(std::size_t i=0; i<Dim_N; ++i)
        for(std::size_t j=0; j<Dim_M; ++j)
            rand1.at(i).at(j) = randreal(mt);

    const MatrixNMd mat1(rand1);

    const MatrixNMd mat2(mat1 / 2e0);
    for(std::size_t i=0; i<Dim_N; ++i)
        for(std::size_t j=0; j<Dim_M; ++j)
            BOOST_CHECK_EQUAL(mat2.at(i,j), rand1.at(i).at(j) / 2e0);

    const MatrixNMd mat3 = mat1 / 2e0;
    for(std::size_t i=0; i<Dim_N; ++i)
        for(std::size_t j=0; j<Dim_M; ++j)
            BOOST_CHECK_EQUAL(mat3.at(i,j), rand1.at(i).at(j) / 2e0);

    MatrixNMd mat6(Dim_N, Dim_M);
    mat6 = mat1 / 2e0;
    for(std::size_t i=0; i<Dim_N; ++i)
        for(std::size_t j=0; j<Dim_M; ++j)
            BOOST_CHECK_EQUAL(mat6.at(i,j), rand1.at(i).at(j) / 2e0);
}

BOOST_AUTO_TEST_CASE(MatrixNd_Multiple)
{
    std::mt19937 mt(seed);
    std::uniform_real_distribution<double> randreal(0e0, 1e0);

    std::vector<std::vector<double>> rand1(Dim_N, std::vector<double>(Dim_M));
    for(std::size_t i=0; i<Dim_N; ++i)
        for(std::size_t j=0; j<Dim_M; ++j)
            rand1.at(i).at(j) = randreal(mt);

    std::vector<std::vector<double>> rand2(Dim_M, std::vector<double>(Dim_N));
    for(std::size_t i=0; i<Dim_M; ++i)
        for(std::size_t j=0; j<Dim_N; ++j)
            rand2.at(i).at(j) = randreal(mt);

    const MatrixNMd mat1(rand1);
    const MatrixNMd mat2(rand2);
    const MatrixNMd mat3 = mat1 * mat2;

    for(std::size_t i=0; i<Dim_N; ++i)
        for(std::size_t j=0; j<Dim_N; ++j)
        {
            double value = 0e0;
            for(std::size_t k=0; k<Dim_M; ++k)
                value += rand1.at(i).at(k) * rand2.at(k).at(j);

            BOOST_CHECK_EQUAL(mat3.at(i,j), value);
        }

    const MatrixNMd mat4 = mat2 * mat1;
    for(std::size_t i=0; i<Dim_M; ++i)
        for(std::size_t j=0; j<Dim_M; ++j)
        {
            double value = 0e0;
            for(std::size_t k=0; k<Dim_N; ++k)
                value += rand2.at(i).at(k) * rand1.at(k).at(j);

            BOOST_CHECK_EQUAL(mat4.at(i,j), value);
        }
}

BOOST_AUTO_TEST_CASE(MatrixNd_transpose)
{
    std::mt19937 mt(seed);
    std::uniform_real_distribution<double> randreal(0e0, 1e0);

    std::vector<std::vector<double>> rand1(Dim_N, std::vector<double>(Dim_M));
    for(std::size_t i=0; i<Dim_N; ++i)
        for(std::size_t j=0; j<Dim_M; ++j)
            rand1[i][j] = randreal(mt);

    const MatrixNMd mat1(rand1);

    const MatrixNMd mat2 = transpose(mat1);
    for(std::size_t i=0; i<Dim_N; ++i)
        for(std::size_t j=0; j<Dim_M; ++j)
            BOOST_CHECK_EQUAL(mat2.at(j, i), rand1.at(i).at(j));

    const MatrixNMd mat3 = transpose(mat1) * mat1;
    for(std::size_t i=0; i<Dim_N; ++i)
        for(std::size_t j=i+1; j<Dim_N; ++j)
            BOOST_CHECK_EQUAL(mat3.at(j, i), mat3.at(i, j));
}


////////////////////////////////////////////////////////////////////////////
// lazy_fold_tests.cpp
//
// tests on foldl foldr and compose
//
////////////////////////////////////////////////////////////////////////////
/*=============================================================================
    Copyright (c) 2000-2003 Brian McNamara and Yannis Smaragdakis
    Copyright (c) 2001-2007 Joel de Guzman
    Copyright (c) 2015 John Fletcher

    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)
==============================================================================*/

#include <boost/phoenix/core/limits.hpp>

#include <boost/detail/lightweight_test.hpp>
#include <boost/phoenix/core.hpp>
#include <boost/phoenix/function/lazy_prelude.hpp>

int main()
{
    namespace phx = boost::phoenix;
    using boost::phoenix::arg_names::arg1;
    using boost::phoenix::arg_names::arg2;
    using boost::phoenix::arg_names::arg3;
    using namespace phx;

    list<int> l = enum_from(2);
    list<int> evens = filter(even,l);
    list<int> even4 = take(4,evens)();

    BOOST_TEST(foldr(plus,0,even4)()       == 20);
    BOOST_TEST(foldr(multiplies,1,even4)() == 384);
    BOOST_TEST(foldr1(plus,even4)()        == 20 );
    BOOST_TEST(foldl(plus,arg1,even4)(0)   == 20);
    BOOST_TEST(foldl1(plus,even4)()        == 20);
    BOOST_TEST(compose(inc,foldr)(plus,0,even4)()  == 21);
    BOOST_TEST(compose(plus,foldl,foldr)(arg1,arg2,arg3)(plus,0,even4) == 40);

    return boost::report_errors();
}


#include <iostream>
#include <cmath>
#include <algorithm>
#include <vector>
#include <boost/multiprecision/cpp_int.hpp>
using namespace std;
using namespace boost::multiprecision;
int main() {
    int n; cin >> n;
    cpp_int tmp; cin >> tmp;
    for (int i = 1; i < n; i++) {
        cpp_int next; cin >> next;
        if (next == tmp) cout << "stay" << endl;
        else if (next > tmp) cout << "up " << next - tmp << endl;
        else cout << "down " << tmp - next << endl;
        tmp = next;
    }
}


#pragma once

#include <Eigen/Core>

namespace FMath
{
    // Scalar precision type
#   ifdef FMATH_SCALAR_TYPE
        using scalar = FMATH_SCALAR_TYPE;
#   else
        using scalar = double;
#   endif

    // Dynamic Eigen typedefs
    using VectorX    = Eigen::Matrix<scalar, -1, 1>;
    using RowVectorX = Eigen::Matrix<scalar, 1, -1>;
    using MatrixX    = Eigen::Matrix<scalar, -1, -1>;

    // 3D Eigen typedefs
    using Vector3    = Eigen::Matrix<scalar, 3, 1>;
    using RowVector3 = Eigen::Matrix<scalar, 1, 3>;
    using Matrix3    = Eigen::Matrix<scalar, 3, 3>;
}


#define BOOST_TEST_NO_LIB
#include <boost/test/auto_unit_test.hpp>

#include "coconut/pulp/math/common.hpp"

using namespace coconut;
using namespace coconut::pulp;
using namespace coconut::pulp::math;

namespace /* anonymous */ {

BOOST_AUTO_TEST_SUITE(PulpTestSuite);
BOOST_AUTO_TEST_SUITE(PulpMathTestSuite);
BOOST_AUTO_TEST_SUITE(PulpMathCommonTestSuite);

BOOST_AUTO_TEST_CASE(LerpLinearlyInterpolatesBetweenValues) {
	BOOST_CHECK_EQUAL(lerp(0.0f, 4.0f, 0.25f), 1.0f);
	BOOST_CHECK_EQUAL(lerp(4.0f, 0.0f, 0.75f), 1.0f);
	BOOST_CHECK_EQUAL(lerp(0.0f, 4.0f, 0.75f), 3.0f);

	BOOST_CHECK_EQUAL(lerp(0.0, 4.0, 0.0f), 0.0);
	BOOST_CHECK_EQUAL(lerp(0.0, 4.0, 1.0f), 4.0);
}

BOOST_AUTO_TEST_SUITE_END(/* PulpMathCommonTestSuite */);
BOOST_AUTO_TEST_SUITE_END(/* PulpMathTestSuite */);
BOOST_AUTO_TEST_SUITE_END(/* PulpTestSuite */);

} // anonymous namespace


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr>
// Copyright (C) 2010 Jitse Niesen <jitse@maths.leeds.ac.uk>
//
// Eigen is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 3 of the License, or (at your option) any later version.
//
// Alternatively, 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 2 of
// the License, or (at your option) any later version.
//
// Eigen 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 Lesser General Public License or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.

#include "main.h"
#include <Eigen/Eigenvalues>

template<typename Scalar,int Size> void hessenberg(int size = Size)
{
  typedef Matrix<Scalar,Size,Size> MatrixType;

  // Test basic functionality: A = U H U* and H is Hessenberg
  for(int counter = 0; counter < g_repeat; ++counter) {
    MatrixType m = MatrixType::Random(size,size);
    HessenbergDecomposition<MatrixType> hess(m);
    MatrixType Q = hess.matrixQ();
    MatrixType H = hess.matrixH();
    VERIFY_IS_APPROX(m, Q * H * Q.adjoint());
    for(int row = 2; row < size; ++row) {
      for(int col = 0; col < row-1; ++col) {
 VERIFY(H(row,col) == (typename MatrixType::Scalar)0);
      }
    }
  }

  // Test whether compute() and constructor returns same result
  MatrixType A = MatrixType::Random(size, size);
  HessenbergDecomposition<MatrixType> cs1;
  cs1.compute(A);
  HessenbergDecomposition<MatrixType> cs2(A);
  VERIFY_IS_EQUAL(cs1.matrixH().eval(), cs2.matrixH().eval());
  MatrixType cs1Q = cs1.matrixQ();
  MatrixType cs2Q = cs2.matrixQ();
  VERIFY_IS_EQUAL(cs1Q, cs2Q);

  // Test assertions for when used uninitialized
  HessenbergDecomposition<MatrixType> hessUninitialized;
  VERIFY_RAISES_ASSERT( hessUninitialized.matrixH() );
  VERIFY_RAISES_ASSERT( hessUninitialized.matrixQ() );
  VERIFY_RAISES_ASSERT( hessUninitialized.householderCoefficients() );
  VERIFY_RAISES_ASSERT( hessUninitialized.packedMatrix() );

  // TODO: Add tests for packedMatrix() and householderCoefficients()
}

void test_hessenberg()
{
  CALL_SUBTEST_1(( hessenberg<std::complex<double>,1>() ));
  CALL_SUBTEST_2(( hessenberg<std::complex<double>,2>() ));
  CALL_SUBTEST_3(( hessenberg<std::complex<float>,4>() ));
  CALL_SUBTEST_4(( hessenberg<float,Dynamic>(internal::random<int>(1,320)) ));
  CALL_SUBTEST_5(( hessenberg<std::complex<double>,Dynamic>(internal::random<int>(1,320)) ));

  // Test problem size constructors
  CALL_SUBTEST_6(HessenbergDecomposition<MatrixXf>(10));
}


//          Copyright Carl Philipp Reh 2009 - 2016.
// 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)

#include <fcppt/math/dim/comparison.hpp>
#include <fcppt/math/dim/narrow_cast.hpp>
#include <fcppt/math/dim/output.hpp>
#include <fcppt/math/dim/static.hpp>
#include <fcppt/preprocessor/disable_gcc_warning.hpp>
#include <fcppt/preprocessor/pop_warning.hpp>
#include <fcppt/preprocessor/push_warning.hpp>
#include <fcppt/config/external_begin.hpp>
#include <boost/test/unit_test.hpp>
#include <fcppt/config/external_end.hpp>

FCPPT_PP_PUSH_WARNING
FCPPT_PP_DISABLE_GCC_WARNING(-Weffc++)

BOOST_AUTO_TEST_CASE(
	math_dim_narrow_cast
)
{
FCPPT_PP_POP_WARNING

	typedef
	fcppt::math::dim::static_<
		int,
		2
	>
	i2_dim;

	typedef
	fcppt::math::dim::static_<
		int,
		3
	>
	i3_dim;

	i2_dim const dim(
		fcppt::math::dim::narrow_cast<
			i2_dim
		>(
			i3_dim(
				1,
				2,
				3
			)
		)
	);

	BOOST_CHECK_EQUAL(
		i2_dim(
			1,
			2
		),
		dim
	);
}


/*
// Copyright (c) 2000-2009, Texas Engineering Experiment Station (TEES), a
// component of the Texas A&M University System.

// All rights reserved.

// The information and source code contained herein is the exclusive
// property of TEES and may not be disclosed, examined or reproduced
// in whole or in part without explicit written authorization from TEES.
*/

#include <stapl/containers/sequential/graph/bgl_undirected_graph_adapter.hpp>
#include <stapl/containers/sequential/graph/algorithms/graph_algo.h>
#include <stapl/containers/sequential/graph/algorithms/find_cycle.h>
#include <stapl/containers/sequential/graph/algorithms/connected_components.h>
#include <stapl/containers/sequential/graph/algorithms/mst_graph.h>
#include "test_util.h"
#include <boost/graph/prim_minimum_spanning_tree.hpp>

using namespace boost;
using namespace stapl;
using namespace std;
using namespace __gnu_cxx;

namespace mst_test {

template <class G, class ColorMap>
void mst_test_core_graph(G& g, ColorMap& cmap){
  typedef typename G::vertex_descriptor VD;
  stapl::sequential::graph<stapl::DIRECTED,stapl::NONMULTIEDGES,typename G::vertex_property, double> mst_graph;

  bool err;
  err=false;

  double tw1=0, tw2=0, tw3=0, tw4=0, tw5=0;

  /////////////////STAPL_ALGO ON STAPL_GRAPH///////////////////////////////////

  cout<<"\ntest prims_mst....." << endl;
  // Initialize the library
  // PAPI_library_init(PAPI_VER_CURRENT);
  // long_long t = PAPI_get_real_usec();

  vector<VD> parentmap(g.get_num_vertices());
  vector<double> weightmap(g.get_num_vertices());
  cout << "calling the actual algo" << endl;
  mst_prims(g, parentmap, weightmap, 0);

  // t = PAPI_get_real_usec() - t;
  // std::cout <<"\t"<< t << ", " << endl;

  stapl::convert_to_graph(parentmap, weightmap, mst_graph);

  cmap.reset();
  if((mst_graph.get_num_vertices() == g.get_num_vertices())
     && (mst_graph.get_num_vertices() == mst_graph.get_num_edges()+1)
     && (is_cycle(mst_graph, cmap) == false)) {
    cmap.reset();
    if (get_cc_count(mst_graph, cmap) == 1) {
      tw1 = total_weight(mst_graph);
      cout<<"<PASSED>\t"; // << endl;
    }
  } else {
    cout<<"{FAILED}\t";
    cmap.reset();
  }
  if(N < 15) display1(mst_graph);

  /*
  //cout<<"\ntest sollins_mst.....";
  mst_graph.clear();
  mst_sollins(g, mst_graph, 0);
  cmap.reset();
  if((mst_graph.get_num_vertices() == g.get_num_vertices())
     && (mst_graph.get_num_vertices() == mst_graph.get_num_edges()/2+1)
     && (is_cycle(mst_graph, cmap) == false)) {
    cmap.reset();
    if (get_cc_count(mst_graph, cmap) == 1) {
      tw2 = total_weight(mst_graph)/2;
      cout<<"<PASSED>\t"; // << endl;
    }
  } else {
    cout<<"{FAILED}\t";
    cmap.reset();
  }
  if(N < 15) display1(mst_graph);
  */
		/*
  //cout<<"\ntest edges_mst.....";
  mst_graph.clear();
  MST_Edges(g, mst_graph, 0);
  cmap.reset();
  if((mst_graph.get_num_vertices() == g.get_num_vertices())
     && (mst_graph.get_num_vertices() == mst_graph.get_num_edges()/2+1)
     && (is_cycle(mst_graph, cmap) == false)) {
    cmap.reset();
    if (get_cc_count(mst_graph, cmap) == 1) {
      tw4 = total_weight(mst_graph)/2;
      cout<<"<PASSED>\t"; // << endl;
    }
  } else {
    cout<<"{FAILED}\t";
    cmap.reset();
    if(N < 15) display1(mst_graph);
  }
  */

  /*
/////////////////BGL_ALGO ON STAPL_GRAPH///////////////////////////////////
  std::vector < typename G::vertex_descriptor >
    p(g.get_num_vertices());

  hash_map<size_t, double> distances(g.get_num_vertices());  // better performance.
  //std::map<typename G::vertex_descriptor, int> distances;
  boost::associative_property_map< hash_map<size_t, double> >
    dist_map(distances);

  {
    using namespace boost;

    // boost::property_map<G, edge_weight_t>::type weightmap = boost::get(edge_weight, g);
    // boost::property_map<G, vertex_distance_t>::type dist_map = get(vertex_distance, g);

    std::vector<typename G::edge_property> weights_map(g.get_num_edges());
    for (typename G::edge_iterator ei = g.edges_begin(); ei != g.edges_end(); ++ei)
      weights_map[ei.descriptor().id()] = ei.property();

    vector<typename G::edge_property>* w_vec_ptr = const_cast<vector<typename G::edge_property>* >(&weights_map);

    PAPI_library_init(PAPI_VER_CURRENT);
    long_long t = PAPI_get_real_usec();

    prim_minimum_spanning_tree(g, *vertices(g).first, &p[0], dist_map,
                               stapl_graph_edge_wt_id_map<typename G::vertex_property,
                               typename G::edge_property>(const_cast<G*>(&g),
                                                          w_vec_ptr),
                               stapl_graph_id_map<typename G::vertex_property, typename G::edge_property>(),
                               default_dijkstra_visitor());

    t = PAPI_get_real_usec() - t;
    std::cout <<"\t"<< t << ", ";

    stapl::graph<stapl::DIRECTED,stapl::MULTIEDGES,typename G::vertex_property, int> boost_out_graph;
    convert_to_graph(p, distances, boost_out_graph);

    cmap.reset();
    if((boost_out_graph.get_num_vertices() == g.get_num_vertices())
       && (boost_out_graph.get_num_vertices() == boost_out_graph.get_num_edges()+1)
       && (is_cycle(boost_out_graph, cmap) == false)) {
      cmap.reset();
      if (get_cc_count(boost_out_graph, cmap) == 1) {
        tw3 = total_weight(boost_out_graph);
        cout<<"<PASSED>\t"; // << endl;
      }
    } else {
      cout<<"{FAILED}\t";
      cmap.reset();
    }
    if(N < 15) display2(boost_out_graph);
  }

  /////////////////BGL_ALGO ON BGL_GRAPH///////////////////////////////////
  {
    using namespace boost;
    typedef adjacency_list <vecS,
      vecS,
      undirectedS,
      property<vertex_distance_t, int>,
      property <edge_weight_t, double> > BOOST_GRAPH;

    BOOST_GRAPH bg;

    property_map<BOOST_GRAPH, edge_weight_t>::type weightmap;
    generate_boost_graph_from_stapl(g, bg, weightmap);

    PAPI_library_init(PAPI_VER_CURRENT);
    long_long t = PAPI_get_real_usec();
    std::vector < graph_traits < BOOST_GRAPH >::vertex_descriptor >
      parentmap(num_vertices(bg));
    property_map<BOOST_GRAPH, vertex_distance_t>::type distmap = get(vertex_distance, bg);
    property_map<BOOST_GRAPH, vertex_index_t>::type indexmap = get(vertex_index, bg);

    //vector<VD> parentmap(g.get_num_vertices());
    //vector<double> distmap(g.get_num_vertices());
    //my_mst_prims(bg, parentmap, distmap, 0);

    prim_minimum_spanning_tree
      (bg, *vertices(bg).first, &parentmap[0], distmap, weightmap, indexmap,
       default_dijkstra_visitor());

    t = PAPI_get_real_usec() - t;
    std::cout << "\t"<< t << ", ";

    stapl::graph<stapl::DIRECTED,stapl::MULTIEDGES,typename G::vertex_property, int> boost_out_graph;
    convert_to_graph(parentmap, distmap, boost_out_graph);

    cmap.reset();
    if((boost_out_graph.get_num_vertices() == g.get_num_vertices())
       && (boost_out_graph.get_num_vertices() == boost_out_graph.get_num_edges()+1)
       && (is_cycle(boost_out_graph, cmap) == false)) {
      cmap.reset();
      if (get_cc_count(boost_out_graph, cmap) == 1) {
        tw5 = total_weight(boost_out_graph);
        cout<<"<PASSED>\t"; // << endl;
      }
    } else {
      cout<<"{FAILED}\t";
      cmap.reset();
    }
    if(N < 15) display1(boost_out_graph);
}

  ///////////END///////
*/

  cout << endl;
  cout << tw1 << ", "  << tw2 << ", "  << tw3 /*<< ", " << tw4 */ << ", "  << tw5 << "\n";
}

}  //end namespace mst_test


/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2016, Georgia Institute of Technology
*  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.
*   * Neither the name of the Rice University nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  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 OWNER 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.
*********************************************************************/

/* Author: Florian Hauer */

#include <ompl/base/objectives/PathLengthOptimizationObjective.h>
#include <ompl/base/spaces/RealVectorStateSpace.h>
#include <ompl/geometric/planners/rrt/RRTXstatic.h>
#include <ompl/geometric/planners/rrt/RRTsharp.h>
#include <ompl/geometric/planners/rrt/RRTstar.h>
#include <ompl/tools/benchmark/Benchmark.h>

#include <boost/format.hpp>
#include <boost/math/constants/constants.hpp>
#include <fstream>

class ValidityChecker : public ompl::base::StateValidityChecker
{
public:
    ValidityChecker(const ompl::base::SpaceInformationPtr &si) : ompl::base::StateValidityChecker(si)
    {
    }

    bool isValid(const ompl::base::State *state) const override
    {
        const auto *state2D = state->as<ompl::base::RealVectorStateSpace::StateType>();

        double sum = 0;
        for (unsigned i = 0; i < si_->getStateSpace()->getDimension(); ++i)
            sum += state2D->values[i] * state2D->values[i];

        return sqrt(sum) > 0.1;
    }
};

int main(int argc, char **argv)
{
    if (argc != 2)
    {
        std::cout << "Usage: " << argv[0] << " dimensionOfTheProblem" << std::endl;
        exit(0);
    }
    int dim = atoi(argv[1]);

    auto space(std::make_shared<ompl::base::RealVectorStateSpace>(dim));
    ompl::geometric::SimpleSetup ss(space);
    const ompl::base::SpaceInformationPtr &si = ss.getSpaceInformation();
    space->setBounds(-1, 1);

    ss.setStateValidityChecker(std::make_shared<ValidityChecker>(si));

    ompl::base::ScopedState<> start(space), goal(space);
    for (int i = 0; i < dim; ++i)
    {
        start[i] = -1;
        goal[i] = 1;
    }

    ss.setStartAndGoalStates(start, goal);

    // by default, use the Benchmark class
    double runtime_limit = 5, memory_limit = 1024;
    int run_count = 100;
    ompl::tools::Benchmark::Request request(runtime_limit, memory_limit, run_count, 0.05, true, true, false);
    ompl::tools::Benchmark b(ss, "Diagonal");

    double range = 0.1 * sqrt(dim);

    auto lengthObj(std::make_shared<ompl::base::PathLengthOptimizationObjective>(si));
    ompl::base::OptimizationObjectivePtr oop((0.5 / sqrt(dim)) * lengthObj);

    ss.setOptimizationObjective(oop);

    bool knn = true;

    auto rrtstar(std::make_shared<ompl::geometric::RRTstar>(si));
    rrtstar->setName("RRT*");
    rrtstar->setDelayCC(true);
    // rrtstar->setFocusSearch(true);
    rrtstar->setRange(range);
    rrtstar->setKNearest(knn);
    b.addPlanner(rrtstar);
    auto rrtsh(std::make_shared<ompl::geometric::RRTsharp>(si));
    rrtsh->setRange(range);
    rrtsh->setKNearest(knn);
    b.addPlanner(rrtsh);
    /*auto rrtsh3(std::make_shared<ompl::geometric::RRTsharp>(si));
    rrtsh3->setName("RRT#v3");
    rrtsh3->setRange(range);
    rrtsh3->setKNearest(knn);
    rrtsh3->setVariant(3);
    b.addPlanner(rrtsh3);
    auto rrtsh2(std::make_shared<ompl::geometric::RRTsharp>(si));
    rrtsh2->setName("RRT#v2");
    rrtsh2->setRange(range);
    rrtsh2->setKNearest(knn);
    rrtsh2->setVariant(2);
    b.addPlanner(rrtsh2);*/
    auto rrtX1(std::make_shared<ompl::geometric::RRTXstatic>(si));
    rrtX1->setName("RRTX0.1");
    rrtX1->setEpsilon(0.1);
    rrtX1->setRange(range);
    // rrtX1->setVariant(3);
    rrtX1->setKNearest(knn);
    b.addPlanner(rrtX1);
    auto rrtX2(std::make_shared<ompl::geometric::RRTXstatic>(si));
    rrtX2->setName("RRTX0.01");
    rrtX2->setEpsilon(0.01);
    rrtX2->setRange(range);
    // rrtX2->setVariant(3);
    rrtX2->setKNearest(knn);
    b.addPlanner(rrtX2);
    auto rrtX3(std::make_shared<ompl::geometric::RRTXstatic>(si));
    rrtX3->setName("RRTX0.001");
    rrtX3->setEpsilon(0.001);
    rrtX3->setRange(range);
    // rrtX3->setVariant(3);
    rrtX3->setKNearest(knn);
    b.addPlanner(rrtX3);
    b.benchmark(request);
    b.saveResultsToFile(boost::str(boost::format("Diagonal.log")).c_str());

    exit(0);
}


#include <Eigen/Sparse>
#include "EigenUtils.hpp"

int main(int argc, char **argv) {
  using SolverType = Eigen::ConjugateGradient<sparsebench::eigenutils::EigenSparseMatrix>;
  sparsebench::eigenutils::runSolver<SolverType>(argc, argv);
  return 0;
}


//
// Copyright (c) 2015-2019 CNRS INRIA
// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//

#include "pinocchio/bindings/python/spatial/explog.hpp"
#include <boost/python.hpp>

namespace pinocchio
{
  namespace python
  {
    namespace bp = boost::python;

    void exposeExplog()
    {

      bp::def("exp3",&exp3_proxy<Eigen::Vector3d>,
              bp::arg("Angular velocity (vector of size 3)"),
              "Exp: so3 -> SO3. Return the integral of the input"
              " angular velocity during time 1.");

      bp::def("Jexp3",&Jexp3_proxy<Eigen::Vector3d>,
              bp::arg("v: Angular velocity (vector of size 3)"),
              "Jacobian of exp(R) which maps from the tangent of SO(3) at exp(v) to"
              " the tangent of SO(3) at Identity.");

      bp::def("log3",&log3_proxy<Eigen::Matrix3d>,
              bp::arg("Rotation matrix (matrix of size 3x3))"),
              "Log: SO3 -> so3. Pseudo-inverse of log from SO3"
              " -> { v in so3, ||v|| < 2pi }.Exp: so3 -> SO3.");

      bp::def("Jlog3",&Jlog3_proxy<Eigen::Matrix3d>,
              bp::arg("Rotation matrix R (matrix of size 3x3)"),
              "Jacobian of log(R) which maps from the tangent of SO(3) at R to"
              " the tangent of SO(3) at Identity.");

      bp::def("exp6",&exp6_proxy<double,0>,
              bp::arg("Spatial velocity (Motion)"),
              "Exp: se3 -> SE3. Return the integral of the input"
              " spatial velocity during time 1.");

      bp::def("exp6",&exp6_proxy<Motion::Vector6>,
              bp::arg("Spatial velocity (vector 6x1)"),
              "Exp: se3 -> SE3. Return the integral of the input"
              " spatial velocity during time 1.");

      bp::def("Jexp6",&Jexp6_proxy<double,0>,
              bp::arg("v: Spatial velocity (Motion)"),
              "Jacobian of exp(v) which maps from the tangent of SE(3) at exp(v) to"
              " the tangent of SE(3) at Identity.");

      bp::def("Jexp6",&Jexp6_proxy<Motion::Vector6>,
              bp::arg("v: Spatial velocity (vector 6x1)"),
              "Jacobian of exp(v) which maps from the tangent of SE(3) at exp(v) to"
              " the tangent of SE(3) at Identity.");

      bp::def("log6",(Motion (*)(const SE3 &))&log6<double,0>,
              bp::arg("Spatial transform (SE3)"),
              "Log: SE3 -> se3. Pseudo-inverse of exp from SE3"
              " -> { v,w in se3, ||w|| < 2pi }.");

      bp::def("log6",&log6_proxy<Eigen::Matrix4d>,
              bp::arg("Homegenious matrix (matrix 4x4)"),
              "Log: SE3 -> se3. Pseudo-inverse of exp from SE3"
              " -> { v,w in se3, ||w|| < 2pi }.");

      bp::def("Jlog6",&Jlog6_proxy<double,0>,
              bp::arg("Spatial transform M (SE3)"),
              "Jacobian of log(M) which maps from the tangent of SE(3) at M to"
              " the tangent of SE(3) at Identity.");

    }

  } // namespace python
} // namespace pinocchio


/*
 * @file testRotateFactor.cpp
 * @brief Test RotateFactor class
 * @author Frank Dellaert
 * @date December 17, 2013
 */

#include <gtsam/slam/RotateFactor.h>
#include <gtsam/base/Testable.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>

#include <boost/bind.hpp>
#include <boost/assign/std/vector.hpp>
#include <vector>

using namespace std;
using namespace boost::assign;
using namespace gtsam;

static const double kDegree = M_PI / 180;

//*************************************************************************
// Create some test data
// Let's assume IMU is aligned with aero (X-forward,Z down)
// And camera is looking forward.
static const Point3 cameraX(0, 1, 0), cameraY(0, 0, 1), cameraZ(1, 0, 0);
static const Rot3 iRc(cameraX, cameraY, cameraZ);

// Now, let's create some rotations around IMU frame
static const Unit3 p1(1, 0, 0), p2(0, 1, 0), p3(0, 0, 1);
static const Rot3 i1Ri2 = Rot3::AxisAngle(p1, 1), //
i2Ri3 = Rot3::AxisAngle(p2, 1), //
i3Ri4 = Rot3::AxisAngle(p3, 1);

// The corresponding rotations in the camera frame
static const Rot3 c1Zc2 = iRc.inverse() * i1Ri2 * iRc, //
c2Zc3 = iRc.inverse() * i2Ri3 * iRc, //
c3Zc4 = iRc.inverse() * i3Ri4 * iRc;

// The corresponding rotated directions in the camera frame
static const Unit3 z1 = iRc.inverse() * p1, //
z2 = iRc.inverse() * p2, //
z3 = iRc.inverse() * p3;

typedef noiseModel::Isotropic::shared_ptr Model;

//*************************************************************************
TEST (RotateFactor, checkMath) {
  EXPECT(assert_equal(c1Zc2, Rot3::AxisAngle(z1, 1)));
  EXPECT(assert_equal(c2Zc3, Rot3::AxisAngle(z2, 1)));
  EXPECT(assert_equal(c3Zc4, Rot3::AxisAngle(z3, 1)));
}

//*************************************************************************
TEST (RotateFactor, test) {
  Model model = noiseModel::Isotropic::Sigma(3, 0.01);
  RotateFactor f(1, i1Ri2, c1Zc2, model);
  EXPECT(assert_equal(Z_3x1, f.evaluateError(iRc), 1e-8));

  Rot3 R = iRc.retract(Vector3(0.1, 0.2, 0.1));
#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS)
  Vector expectedE = Vector3(-0.0248752, 0.202981, -0.0890529);
#else
  Vector expectedE = Vector3(-0.0246305, 0.20197, -0.08867);
#endif
  EXPECT( assert_equal(expectedE, f.evaluateError(R), 1e-5));

  Matrix actual, expected;
  // Use numerical derivatives to calculate the expected Jacobian
  {
    expected = numericalDerivative11<Vector3,Rot3>(
        boost::bind(&RotateFactor::evaluateError, &f, _1, boost::none), iRc);
    f.evaluateError(iRc, actual);
    EXPECT(assert_equal(expected, actual, 1e-9));
  }
  {
    expected = numericalDerivative11<Vector3,Rot3>(
        boost::bind(&RotateFactor::evaluateError, &f, _1, boost::none), R);
    f.evaluateError(R, actual);
    EXPECT(assert_equal(expected, actual, 1e-9));
  }
}

//*************************************************************************
TEST (RotateFactor, minimization) {
  // Let's try to recover the correct iRc by minimizing
  NonlinearFactorGraph graph;
  Model model = noiseModel::Isotropic::Sigma(3, 0.01);
  graph.emplace_shared<RotateFactor>(1, i1Ri2, c1Zc2, model);
  graph.emplace_shared<RotateFactor>(1, i2Ri3, c2Zc3, model);
  graph.emplace_shared<RotateFactor>(1, i3Ri4, c3Zc4, model);

  // Check error at ground truth
  Values truth;
  truth.insert(1, iRc);
  EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8);

  // Check error at initial estimate
  Values initial;
  Rot3 initialE = iRc.retract(kDegree * Vector3(20, -20, 20));
  initial.insert(1, initialE);

#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS)
  EXPECT_DOUBLES_EQUAL(3545.40, graph.error(initial), 1);
#else
  EXPECT_DOUBLES_EQUAL(3349, graph.error(initial), 1);
#endif

  // Optimize
  LevenbergMarquardtParams parameters;
  //parameters.setVerbosity("ERROR");
  LevenbergMarquardtOptimizer optimizer(graph, initial, parameters);
  Values result = optimizer.optimize();

  // Check result
  Rot3 actual = result.at<Rot3>(1);
  EXPECT(assert_equal(iRc, actual,1e-1));

  // Check error at result
  EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4);
}

//*************************************************************************
TEST (RotateDirectionsFactor, test) {
  Model model = noiseModel::Isotropic::Sigma(2, 0.01);
  RotateDirectionsFactor f(1, p1, z1, model);
  EXPECT(assert_equal(Z_2x1, f.evaluateError(iRc), 1e-8));

  Rot3 R = iRc.retract(Vector3(0.1, 0.2, 0.1));

#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS)
  Vector expectedE = Vector2(-0.0890529, -0.202981);
#else
  Vector expectedE = Vector2(-0.08867, -0.20197);
#endif

  EXPECT( assert_equal(expectedE, f.evaluateError(R), 1e-5));

  Matrix actual, expected;
  // Use numerical derivatives to calculate the expected Jacobian
  {
    expected = numericalDerivative11<Vector,Rot3>(
        boost::bind(&RotateDirectionsFactor::evaluateError, &f, _1,
            boost::none), iRc);
    f.evaluateError(iRc, actual);
    EXPECT(assert_equal(expected, actual, 1e-9));
  }
  {
    expected = numericalDerivative11<Vector,Rot3>(
        boost::bind(&RotateDirectionsFactor::evaluateError, &f, _1,
            boost::none), R);
    f.evaluateError(R, actual);
    EXPECT(assert_equal(expected, actual, 1e-9));
  }
}

//*************************************************************************
TEST (RotateDirectionsFactor, minimization) {
  // Let's try to recover the correct iRc by minimizing
  NonlinearFactorGraph graph;
  Model model = noiseModel::Isotropic::Sigma(2, 0.01);
  graph.emplace_shared<RotateDirectionsFactor>(1, p1, z1, model);
  graph.emplace_shared<RotateDirectionsFactor>(1, p2, z2, model);
  graph.emplace_shared<RotateDirectionsFactor>(1, p3, z3, model);

  // Check error at ground truth
  Values truth;
  truth.insert(1, iRc);
  EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8);

  // Check error at initial estimate
  Values initial;
  Rot3 initialE = iRc.retract(kDegree * Vector3(20, -20, 20));
  initial.insert(1, initialE);

#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS)
  EXPECT_DOUBLES_EQUAL(3335.9, graph.error(initial), 1);
#else
  EXPECT_DOUBLES_EQUAL(3162, graph.error(initial), 1);
#endif

  // Optimize
  LevenbergMarquardtParams parameters;
  LevenbergMarquardtOptimizer optimizer(graph, initial, parameters);
  Values result = optimizer.optimize();

  // Check result
  Rot3 actual = result.at<Rot3>(1);
  EXPECT(assert_equal(iRc, actual,1e-1));

  // Check error at result
  EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4);
}

//*************************************************************************
TEST(RotateDirectionsFactor, Initialization) {
  // Create a gravity vector in a nav frame that has Z up
  const Point3 n_gravity(0, 0, -10);
  const Unit3 n_p(-n_gravity);

  // NOTE(frank): avoid singularities by using 85/275 instead of 90/270
  std::vector<double> angles = {0, 45, 85, 135, 180, 225, 275, 315};
  for (double yaw : angles) {
    const Rot3 nRy = Rot3::Yaw(yaw * kDegree);
    for (double pitch : angles) {
      const Rot3 yRp = Rot3::Pitch(pitch * kDegree);
      for (double roll : angles) {
        const Rot3 pRb = Rot3::Roll(roll * kDegree);

        // Rotation from body to nav frame:
        const Rot3 nRb = nRy * yRp * pRb;
        const Vector3 rpy = nRb.rpy() / kDegree;

        // Simulate measurement of IMU in body frame:
        const Point3 b_acc = nRb.unrotate(-n_gravity);
        const Unit3 b_z(b_acc);

        // Check initialization
        const Rot3 actual_nRb = RotateDirectionsFactor::Initialize(n_p, b_z);
        const Vector3 actual_rpy = actual_nRb.rpy() / kDegree;
        EXPECT_DOUBLES_EQUAL(rpy.x(), actual_rpy.x(), 1e-5);
        EXPECT_DOUBLES_EQUAL(rpy.y(), actual_rpy.y(), 1e-5);
      }
    }
  }
}

/* ************************************************************************* */
int main() {
  TestResult tr;
  return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */


#pragma once
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include "SFINAE.hpp"

///@file
///@brief Contains general template aliases.

namespace mackey {

	///Scalar of matrix
	template<typename T>
	using scalar_t = typename T::Scalar;

	///Storage type of matrix
	template<typename T>
	using storage_t = typename T::StorageIndex;

	///Dense row matrix
	template<typename T>
	using row_vector_t = Eigen::Matrix<scalar_t<T>,1,-1>;

	///Dense column matrix
	template<typename T>
	using col_vector_t = Eigen::Matrix<scalar_t<T>, -1, 1>;

	///Dense column major
	template<typename T>
	using dense_t= Eigen::Matrix<scalar_t<T>,-1,-1>;

	///Sparse column major
	template<typename T>
	using sparse_t = Eigen::SparseMatrix<scalar_t<T>, 0, storage_t<T>>;

	template<typename T>
	using row_major_t = std::conditional<SFINAE::is_Dense<T>::value, Eigen::Matrix<scalar_t<T>, -1, -1,1>, Eigen::SparseMatrix<scalar_t<T>, 1, storage_t<T>>>;

	template<typename T>
	using col_major_t = std::conditional<SFINAE::is_Dense<T>::value, Eigen::Matrix<scalar_t<T>, -1, -1>, Eigen::SparseMatrix<scalar_t<T>, 0, storage_t<T>>>;

	///Eigen triplets type
	template<typename T>
	using triplet_t = Eigen::Triplet<scalar_t<T>, storage_t<T>>;

	template<typename, typename>
	struct Arrow;

	template<typename, typename>
	class Chains;

	template<typename, typename>
	class Junction;

	///Arrow given group_t
	template<typename T>
	using arrow_t = Arrow<typename T::rank_t, typename T::diff_t>;

	///Chains given group_t
	template<typename T>
	using chains_t = Chains<typename T::rank_t, typename T::diff_t>;

	///Junction given group_t
	template<typename T>
	using junction_t = Junction<typename T::rank_t, typename T::diff_t>;

	template<typename, typename>
	class Homology;

	///Type of generators in homology
	template<typename rank_t, typename diff_t>
	using gen_t = typename Homology<rank_t, diff_t>::gen_t;

}


//
// Copyright 2020 Olzhas Zhumabek <anonymous.from.applecity@gmail.com>
//
// Use, modification and distribution are subject to 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)
//
#include <boost/gil/algorithm.hpp>
#include <boost/gil/extension/io/png.hpp>
#include <boost/gil/image.hpp>
#include <boost/gil/image_processing/diffusion.hpp>
#include <boost/gil/image_view.hpp>
#include <boost/gil/io/typedefs.hpp>
#include <boost/gil/typedefs.hpp>

#include <cmath>
#include <iostream>
#include <string>
#include <type_traits>

namespace gil = boost::gil;

void gray_version(std::string const& input_path, std::string const& output_path,
                  unsigned int iteration_count, float kappa)
{
    gil::gray8_image_t input;
    gil::read_image(input_path, input, gil::png_tag{});
    auto input_view = gil::view(input);

    gil::gray32f_image_t gray(input.dimensions());
    auto gray_view = gil::view(gray);

    gil::transform_pixels(input_view, gray_view, [](const gil::gray8_pixel_t& p) { return p[0]; });
    double sum_before = 0;
    gil::for_each_pixel(gray_view, [&sum_before](gil::gray32f_pixel_t p) { sum_before += p[0]; });
    gil::gray32f_image_t output(gray.dimensions());
    auto output_view = gil::view(output);

    // gil::anisotropic_diffusion(gray_view, output_view, iteration_count, {kappa, delta_t});
    gil::default_anisotropic_diffusion(gray_view, output_view, iteration_count, kappa);
    double sum_after = 0;
    gil::for_each_pixel(output_view, [&sum_after](gil::gray32f_pixel_t p) { sum_after += p[0]; });

    gil::gray8_image_t true_output(output.dimensions());
    gil::transform_pixels(output_view, gil::view(true_output),
                          [](gil::gray32f_pixel_t p) { return static_cast<gil::uint8_t>(p[0]); });

    gil::write_view(output_path, gil::view(true_output), gil::png_tag{});

    std::cout << "sum of intensity before diffusion: " << sum_before << '\n'
              << "sum of intensity after diffusion: " << sum_after << '\n'
              << "difference: " << sum_after - sum_before << '\n';
}

void rgb_version(const std::string& input_path, const std::string& output_path,
                 unsigned int iteration_count, float kappa)
{
    gil::rgb8_image_t input;
    gil::read_image(input_path, input, gil::png_tag{});
    auto input_view = gil::view(input);

    gil::rgb32f_image_t gray(input.dimensions());
    auto gray_view = gil::view(gray);

    gil::transform_pixels(input_view, gray_view, [](const gil::rgb8_pixel_t& p) {
        return gil::rgb32f_pixel_t(p[0], p[1], p[2]);
    });
    double sum_before[3] = {};
    gil::for_each_pixel(gray_view, [&sum_before](gil::rgb32f_pixel_t p) {
        sum_before[0] += p[0];
        sum_before[1] += p[1];
        sum_before[2] += p[2];
    });
    gil::rgb32f_image_t output(gray.dimensions());
    auto output_view = gil::view(output);

    // gil::anisotropic_diffusion(gray_view, output_view, iteration_count, {kappa, delta_t});
    gil::default_anisotropic_diffusion(gray_view, output_view, iteration_count, kappa);
    double sum_after[3] = {};
    gil::for_each_pixel(output_view, [&sum_after](gil::rgb32f_pixel_t p) {
        sum_after[0] += p[0];
        sum_after[1] += p[1];
        sum_after[2] += p[2];
    });

    gil::rgb8_image_t true_output(output.dimensions());
    gil::transform_pixels(output_view, gil::view(true_output), [](gil::rgb32f_pixel_t p) {
        return gil::rgb8_pixel_t(static_cast<gil::uint8_t>(p[0]), static_cast<gil::uint8_t>(p[1]),
                                 static_cast<gil::uint8_t>(p[2]));
    });

    gil::write_view(output_path, gil::view(true_output), gil::png_tag{});
    std::cout << "sum of intensity before diffusion: (" << sum_before[0] << ", " << sum_before[1]
              << ", " << sum_before[2] << ")\n"
              << "sum of intensity after diffusion: (" << sum_after[0] << ", " << sum_after[1]
              << ", " << sum_after[2] << ")\n"
              << "difference: (" << sum_after[0] - sum_before[0] << ", "
              << sum_after[1] - sum_before[1] << ", " << sum_after[2] - sum_before[2] << ")\n";
}

int main(int argc, char* argv[])
{
    if (argc != 6)
    {
        std::cerr << "usage: " << argv[0]
                  << " <input.png> <output.png>"
                     " <colorspace: gray|rgb> <positive iteration count> <positive kappa~30>\n";
        return -1;
    }
    std::string input_path = argv[1];
    std::string output_path = argv[2];
    std::string colorspace = argv[3];

    unsigned int iteration_count = static_cast<unsigned int>(std::stoul(argv[4]));
    float kappa = std::stof(argv[5]);
    if (colorspace == "gray")
    {
        gray_version(input_path, output_path, iteration_count, kappa);
    }
    else if (colorspace == "rgb")
    {
        rgb_version(input_path, output_path, iteration_count, kappa);
    }
    else
    {
        std::cerr << "unknown colorspace option passed (did you type gray with A?)\n";
    }
}


/* vim: set tabstop=4 expandtab shiftwidth=4 softtabstop=4: */

/**
 * \file boost/numeric/ublasx/operation/pow.hpp
 *
 * \brief Apply the \c std::pow function to each element of a vector or a matrix
 *  expression.
 *
 * <hr/>
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 *
 * Copyright (c) 2015, Marco Guazzone
 *
 * 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 BOOST_NUMERIC_UBLASX_OPERATION_POW_HPP
#define BOOST_NUMERIC_UBLASX_OPERATION_POW_HPP

#include <boost/numeric/ublas/traits.hpp>
#include <boost/numeric/ublasx/expression/matrix_binary_functor.hpp>
#include <boost/numeric/ublasx/expression/vector_binary_functor.hpp>
#include <cmath>
#include <complex>

namespace boost { namespace numeric { namespace ublasx {

using namespace ::boost::numeric::ublas;

namespace detail {

template <typename VectorExprT, typename Arg2T>
struct vector_pow_functor1_traits
{
    typedef VectorExprT input_expression_type;
    typedef typename vector_traits<input_expression_type>::value_type signature_argument1_type;
    typedef Arg2T signature_argument2_type;
    //typedef signature_argument_type signature_result_type;
    typedef typename promote_traits<
                signature_argument1_type,
                signature_argument2_type
            >::promote_type signature_result_type;
    typedef vector_binary_functor1_traits<
                input_expression_type,
                Arg2T,
                signature_result_type (signature_argument1_type, signature_argument2_type)
            > binary_functor_expression_type;
    typedef typename binary_functor_expression_type::result_type result_type;
    typedef typename binary_functor_expression_type::expression_type expression_type;
};

template <typename Arg1T, typename VectorExprT>
struct vector_pow_functor2_traits
{
    typedef VectorExprT input_expression_type;
    typedef Arg1T signature_argument1_type;
    typedef typename vector_traits<input_expression_type>::value_type signature_argument2_type;
    //typedef signature_argument_type signature_result_type;
    typedef typename promote_traits<
                signature_argument1_type,
                signature_argument2_type
            >::promote_type signature_result_type;
    typedef vector_binary_functor2_traits<
                Arg1T,
                input_expression_type,
                signature_result_type (signature_argument1_type, signature_argument2_type)
            > binary_functor_expression_type;
    typedef typename binary_functor_expression_type::result_type result_type;
    typedef typename binary_functor_expression_type::expression_type expression_type;
};

template <typename MatrixExprT, typename Arg2T>
struct matrix_pow_functor1_traits
{
    typedef MatrixExprT input_expression_type;
    typedef typename matrix_traits<input_expression_type>::value_type signature_argument1_type;
    typedef Arg2T signature_argument2_type;
    //typedef signature_argument_type signature_result_type;
    typedef typename promote_traits<
                signature_argument1_type,
                signature_argument2_type
            >::promote_type signature_result_type;
    typedef matrix_binary_functor1_traits<
                input_expression_type,
                Arg2T,
                signature_result_type (signature_argument1_type, signature_argument2_type)
            > binary_functor_expression_type;
    typedef typename binary_functor_expression_type::result_type result_type;
    typedef typename binary_functor_expression_type::expression_type expression_type;
};

template <typename Arg1T, typename MatrixExprT>
struct matrix_pow_functor2_traits
{
    typedef MatrixExprT input_expression_type;
    typedef Arg1T signature_argument1_type;
    typedef typename matrix_traits<input_expression_type>::value_type signature_argument2_type;
    //typedef signature_argument_type signature_result_type;
    typedef typename promote_traits<
                signature_argument1_type,
                signature_argument2_type
            >::promote_type signature_result_type;
    typedef matrix_binary_functor2_traits<
                Arg1T,
                input_expression_type,
                signature_result_type (signature_argument1_type, signature_argument2_type)
            > binary_functor_expression_type;
    typedef typename binary_functor_expression_type::result_type result_type;
    typedef typename binary_functor_expression_type::expression_type expression_type;
};

// Wrappers to the std::pow function to avoid compiler errors

template <typename T1, typename T2>
BOOST_UBLAS_INLINE
typename promote_traits<T1,T2>::promote_type pow(T1 x, T2 y)
{
    return ::std::pow(x, y);
}

template <typename T1, typename T2>
BOOST_UBLAS_INLINE
std::complex<T1> pow(std::complex<T1> const& x, T2 y)
{
    return ::std::pow(x, y);
}

template <typename T1, typename T2>
BOOST_UBLAS_INLINE
std::complex<T1> pow(T1 x, std::complex<T2> const& y)
{
    // Remember: if z=(a + ib) is a complex number and c is a scalar => c^z = e^{ln(c)*z}
    return ::std::exp(::std::log(x)*y);
}

} // Namespace detail

/**
 * \brief Applies the \c std::pow function to a given vector expression,
 *  where each element of the vector is treated as the base of the
 *  exponentiation.
 *
 * \tparam VectorExprT The type of the input vector expression.
 *
 * \param ve The input vector expression.
 * \param p The exponent.
 * \return A vector expression representing the application of \c std::pow to
 *  each element of \a ve.
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */
template <typename VectorExprT, typename T>
BOOST_UBLAS_INLINE
typename detail::vector_pow_functor1_traits<VectorExprT,T>::result_type pow(vector_expression<VectorExprT> const& ve, T p)
{
    typedef typename detail::vector_pow_functor1_traits<VectorExprT,T>::expression_type expression_type;
    typedef typename detail::vector_pow_functor1_traits<VectorExprT,T>::signature_argument1_type signature_argument1_type;
    typedef typename detail::vector_pow_functor1_traits<VectorExprT,T>::signature_argument2_type signature_argument2_type;
    typedef typename detail::vector_pow_functor1_traits<VectorExprT,T>::signature_result_type signature_result_type;

//  return expression_type(ve(), detail::pow<signature_result_type>);
//  signature_result_type (*)(ptr_pow_fun)(signature_argument_type)(BOOST_NUMERIC_UBLASX_OPERATION_POW_NS_::pow);
    typedef signature_result_type(*fun_ptr_type)(signature_argument1_type, signature_argument2_type);
    fun_ptr_type ptr_pow_fun(&detail::pow);
    return expression_type(ve(), p, ptr_pow_fun);
}

/**
 * \brief Applies the \c std::pow function to a given vector expression,
 *  where each element of the vector is treated as the power of the
 *  exponentiation.
 *
 * \tparam VectorExprT The type of the input vector expression.
 *
 * \param b The base.
 * \param ve The input vector expression.
 * \return A vector expression representing the application of \c std::pow to
 *  each element of \a ve.
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */
template <typename T, typename VectorExprT>
BOOST_UBLAS_INLINE
typename detail::vector_pow_functor2_traits<T,VectorExprT>::result_type pow(T b, vector_expression<VectorExprT> const& ve)
{
    typedef typename detail::vector_pow_functor2_traits<T,VectorExprT>::expression_type expression_type;
    typedef typename detail::vector_pow_functor2_traits<T,VectorExprT>::signature_argument1_type signature_argument1_type;
    typedef typename detail::vector_pow_functor2_traits<T,VectorExprT>::signature_argument2_type signature_argument2_type;
    typedef typename detail::vector_pow_functor2_traits<T,VectorExprT>::signature_result_type signature_result_type;

//  return expression_type(ve(), detail::pow<signature_result_type>);
//  signature_result_type (*)(ptr_pow_fun)(signature_argument_type)(BOOST_NUMERIC_UBLASX_OPERATION_POW_NS_::pow);
    typedef signature_result_type(*fun_ptr_type)(signature_argument1_type, signature_argument2_type);
    fun_ptr_type ptr_pow_fun(&detail::pow);
    return expression_type(b, ve(), ptr_pow_fun);
}

/**
 * \brief Applies the \c std::pow function to a given matrix expression,
 *  where each element of the matrix is treated as the base of the
 *  exponentiation.
 *
 * \tparam MatrixExprT The type of the input matrix expression.
 *
 * \param me The input matrix expression.
 * \param p The exponent.
 * \return A matrix expression representing the application of \c std::pow to
 *  each element of \a me.
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */
template <typename MatrixExprT, typename T>
BOOST_UBLAS_INLINE
typename detail::matrix_pow_functor1_traits<MatrixExprT,T>::result_type pow(matrix_expression<MatrixExprT> const& me, T p)
{
    typedef typename detail::matrix_pow_functor1_traits<MatrixExprT,T>::expression_type expression_type;
    typedef typename detail::matrix_pow_functor1_traits<MatrixExprT,T>::signature_argument1_type signature_argument1_type;
    typedef typename detail::matrix_pow_functor1_traits<MatrixExprT,T>::signature_argument2_type signature_argument2_type;
    typedef typename detail::matrix_pow_functor1_traits<MatrixExprT,T>::signature_result_type signature_result_type;

//  return expression_type(me(), detail::pow<signature_result_type>(signature_argument_type));
    typedef signature_result_type(*fun_ptr_type)(signature_argument1_type, signature_argument2_type);
    fun_ptr_type ptr_pow_fun(&detail::pow);
    return expression_type(me(), p, ptr_pow_fun);
}

/**
 * \brief Applies the \c std::pow function to a given matrix expression,
 *  where each element of the matrix is treated as the power of the
 *  exponentiation.
 *
 * \tparam MatrixExprT The type of the input matrix expression.
 *
 * \param b The base.
 * \param me The input matrix expression.
 * \return A matrix expression representing the application of \c std::pow to
 *  each element of \a me.
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */
template <typename T, typename MatrixExprT>
BOOST_UBLAS_INLINE
typename detail::matrix_pow_functor2_traits<T,MatrixExprT>::result_type pow(T b, matrix_expression<MatrixExprT> const& me)
{
    typedef typename detail::matrix_pow_functor2_traits<T,MatrixExprT>::expression_type expression_type;
    typedef typename detail::matrix_pow_functor2_traits<T,MatrixExprT>::signature_argument1_type signature_argument1_type;
    typedef typename detail::matrix_pow_functor2_traits<T,MatrixExprT>::signature_argument2_type signature_argument2_type;
    typedef typename detail::matrix_pow_functor2_traits<T,MatrixExprT>::signature_result_type signature_result_type;

//  return expression_type(me(), detail::pow<signature_result_type>(signature_argument_type));
    typedef signature_result_type(*fun_ptr_type)(signature_argument1_type, signature_argument2_type);
    fun_ptr_type ptr_pow_fun(&detail::pow);
    return expression_type(b, me(), ptr_pow_fun);
}

}}} // Namespace boost::numeric::ublasx

#endif // BOOST_NUMERIC_UBLASX_OPERATION_POW_HPP