/**
 * Copyright (c) 2012 Andrew Prock. All rights reserved.
 * $Id: PokerEvaluation.cpp 2649 2012-06-30 04:53:24Z prock $
 */

#include <pokerstove/peval/Card.h>
#include <pokerstove/peval/PokerEvaluation.h>
#include <pokerstove/peval/PokerEvaluationTables.h>
#include <pokerstove/util/lastbit.h>

#include <iostream>
#include <stdexcept>

#include <boost/format.hpp>

using namespace std;
using namespace pokerstove;

// some useful consts that don't need to be exposd
const int MAJOR_MASK = 0xF << MAJOR_SHIFT;
const int MINOR_MASK = 0xF << MINOR_SHIFT;
const int KICKER_MASK = 0x1FFF;

PokerEvaluation::PokerEvaluation() : _evalcode(0) {}

PokerEvaluation::PokerEvaluation(int ecode) : _evalcode(ecode) {}

int PokerEvaluation::code() const
{
    return _evalcode;
}

int PokerEvaluation::reducedCode() const
{
    if (isFlipped())
    {
        PokerEvaluation e = *this;
        e.flip();
        return e.reducedCode2to7();
    }

    if (_evalcode == 0)
        return 0;

    switch (type())
    {
        case NO_PAIR:
        case ONE_PAIR:
        case THREE_FLUSH:
        case FLUSH: {
            PokerEvaluation e(_evalcode);
            int kick1 = topRankTable[kickerBits()];
            int kick2 = topRankTable[kickerBits() ^ 0x01 << kick1];
            int kbits = (0x01 << kick1) | (0x01 << kick2);
            e.setKickerBits(kbits);
            return e.code();
        }

        case FULL_HOUSE:
        case THREE_OF_A_KIND:
        case FOUR_OF_A_KIND:
            return showdownCode();

        default:
            return code();
    }
}

int PokerEvaluation::reducedCode2to7() const
{
    if (_evalcode == 0)
        return 0;
    switch (type())
    {
        case NO_PAIR: {
            PokerEvaluation e(_evalcode);
            int kick1 = topRankTable[kickerBits()];
            int kick2 = topRankTable[kickerBits() ^ 0x01 << kick1];
            int kick3 = topRankTable[kickerBits() ^ 0x01 << kick1 ^ 0x01 << kick2];
            int kbits = (0x01 << kick1) | (0x01 << kick2);
            if (kick1 < 10)
                kbits |= (0x01 << kick3);
            e.setKickerBits(kbits);
            e.flip();
            return e.code();
        }

        case THREE_OF_A_KIND: {
            PokerEvaluation e(_evalcode);
            vector<int> ranks(3);
            ranks[0] = botRankTable[kickerBits()];
            ranks[1] = botRankTable[kickerBits() ^ 0x01 << ranks[0]];
            ranks[2] = e.majorRank().code();
            sort(ranks.begin(), ranks.end());
            e = PokerEvaluation((TWO_PAIR << VSHIFT) ^ (ranks[2] << MAJOR_SHIFT) ^ (ranks[1] << MINOR_SHIFT) ^ (0x01 << ranks[0]));
            e.flip();
            return e.code();
        }

        case ONE_PAIR: {
            PokerEvaluation e(_evalcode);
            vector<int> ranks(4);
            uint16_t kickers = kickerBits();
            ranks[0] = lastbit(kickers);
            kickers ^= 0x01 << ranks[0];
            ranks[1] = lastbit(kickers);
            kickers ^= 0x01 << ranks[1];
            ranks[2] = lastbit(kickers);
            kickers ^= 0x01 << ranks[2];
            ranks[3] = e.majorRank().code();
            sort(ranks.begin(), ranks.end());
            e = PokerEvaluation((ONE_PAIR << VSHIFT) ^ (ranks[2] << MAJOR_SHIFT) ^ (0x01 << ranks[0]) ^ (0x01 << ranks[1]));
            e.flip();
            return e.code();
        }

        case TWO_PAIR: {
            PokerEvaluation e(_evalcode);
            vector<int> ranks(3);
            uint16_t kickers = kickerBits();
            ranks[0] = lastbit(kickers);
            ranks[1] = e.minorRank().code();
            ranks[2] = e.majorRank().code();
            sort(ranks.begin(), ranks.end());
            e = PokerEvaluation((TWO_PAIR << VSHIFT) ^ (ranks[2] << MAJOR_SHIFT) ^ (ranks[1] << MINOR_SHIFT) ^ (0x01 << ranks[0]));
            e.flip();
            return e.code();
        }

        case FLUSH: {
            PokerEvaluation e(_evalcode);
            int kick1 = topRankTable[kickerBits()];
            int kbits = (0x01 << kick1);
            e.setKickerBits(kbits);
            e.flip();
            return e.code();
        }

        case FULL_HOUSE: {
            PokerEvaluation e(_evalcode);
            vector<int> ranks(2);
            ranks[0] = e.minorRank().code();
            ranks[1] = e.majorRank().code();
            sort(ranks.begin(), ranks.end());
            e = PokerEvaluation((FULL_HOUSE << VSHIFT) ^ (ranks[1] << MAJOR_SHIFT) ^ (ranks[0] << MINOR_SHIFT));
            e.flip();
            return e.code();
        }

        case FOUR_OF_A_KIND: {
            PokerEvaluation e(_evalcode);
            vector<int> ranks(2);
            uint16_t kickers = kickerBits();
            ranks[0] = lastbit(kickers);
            ranks[1] = e.majorRank().code();
            e = PokerEvaluation((FOUR_OF_A_KIND << VSHIFT) ^ (ranks[1] << MAJOR_SHIFT) ^ (0x01 << ranks[0]));
            e.flip();
            return e.code();
        }

        default:
            return code();
    }
}

int PokerEvaluation::showdownCode() const
{
    if (_evalcode == 0)
        return 0;

    int minor = 0;
    switch (type())
    {
        case FULL_HOUSE:    // fall through case, prints deuces full nicer
            if (majorRank() == Rank::Two())
                minor = 1;
        case THREE_OF_A_KIND:
        case FOUR_OF_A_KIND: {
            PokerEvaluation e(_evalcode);
            e.setMinorRank(minor);
            e.setKickerBits(0);
            return e.code();
        }

        default:
            return code();
    }
}

int PokerEvaluation::type() const
{
    return _evalcode >> VSHIFT;
}

int PokerEvaluation::kickerBits() const
{
    return _evalcode & KICKER_MASK;
}
Rank PokerEvaluation::majorRank() const
{
    return Rank((_evalcode >> MAJOR_SHIFT) & 0x0F);
}
Rank PokerEvaluation::minorRank() const
{
    return Rank((_evalcode >> MINOR_SHIFT) & 0x0F);
}

void PokerEvaluation::setKickerBits(int k)
{
    _evalcode = (_evalcode & ~KICKER_MASK) | k;
}
void PokerEvaluation::setMajorRank(int r)
{
    _evalcode = (_evalcode & ~MAJOR_MASK) | r << MAJOR_SHIFT;
}
void PokerEvaluation::setMinorRank(int r)
{
    _evalcode = (_evalcode & ~MINOR_MASK) | r << MINOR_SHIFT;
}

// this converts the evaluation to one where
// the ace is valued as low instead of high,
// thus 2 > A internally.
void PokerEvaluation::playAceLow()
{
    if (acePlaysLow())
        return;

    _evalcode |= ACE_LOW_BIT;

    // make room for the ace in kicker land, and if
    // we have an ace, push it to the bottom
    int kickers = kickerBits();
    kickers = kickers << 1;
    if (kickers & ACE_LOW_BIT)
        kickers++;
    setKickerBits(kickers);

    switch (type())
    {
        case TWO_PAIR:
        case FULL_HOUSE:
            setMinorRank((minorRank().code() + 1) % Rank::NUM_RANK);

        case ONE_PAIR:
        case THREE_OF_A_KIND:
        case STRAIGHT:
        case FOUR_OF_A_KIND:
        case STRAIGHT_FLUSH:
            setMajorRank((majorRank().code() + 1) % Rank::NUM_RANK);
            break;
    };
}

void PokerEvaluation::playAceHigh()
{
    if (acePlaysLow() == false)
        return;

    _evalcode ^= ACE_LOW_BIT;

    // make room for the ace in kicker land, and if
    // we have an ace, push it to the bottom
    int kickers = kickerBits();
    if (kickers & 0x01)
        kickers |= ACE_LOW_BIT;
    kickers = kickers >> 1;
    setKickerBits(kickers);
}

bool PokerEvaluation::acePlaysLow() const
{
    if (_evalcode & ACE_LOW_BIT)
        return true;
    return false;
}

void PokerEvaluation::fixWheel2to7(int rankMask)
{
    if (type() == STRAIGHT && majorRank() == Rank::Five())
    {
        _evalcode = (NO_PAIR << VSHIFT) ^ rankMask;
    }
    else if (type() == STRAIGHT_FLUSH && majorRank() == Rank::Five())
    {
        _evalcode = (FLUSH << VSHIFT) ^ rankMask;
    }
}

string PokerEvaluation::makeRankString(int r, bool acelow) const
{
    if (acelow)
    {
        if (r == 0)
            r = Rank::NUM_RANK - 1;
        else
            r--;
    }

    Rank rank(r);
    return rank.str();
}

string PokerEvaluation::strKickers(int n) const
{
    string out;
    for (int i = Rank::NUM_RANK - 1; i >= 0; i--)
        if ((n & (0x01 << i)) > 0)
        {
            //      Rank r(i);
            //      out += r.toString ();
            out += makeRankString(i, acePlaysLow());
        }
    return out;
}

string PokerEvaluation::strTop(int n) const
{
    int i = (n >> 20) & 0x0F;
    //    Rank r(i);
    //    return r.toString ();
    return makeRankString(i, acePlaysLow());
}

string PokerEvaluation::strBot(int n) const
{
    int i = (n >> 16) & 0x0F;
    //    Rank r(i);
    //    return r.toString ();
    return makeRankString(i, acePlaysLow());
}

string PokerEvaluation::bitstr() const
{
    string ret;
    int n = _evalcode;
    for (int i = 31; i >= 0; i--)
    {
        if ((n & (0x01 << i)) > 0)
            ret += "1";
        else
            ret += "0";
        if (i % 8 == 0)
            ret += " ";
    }
    return ret;
}

string PokerEvaluation::str() const
{
    string ret;
    int n = _evalcode;

    if (n == 0)
        return "";

    if (n != 0)
    {
        if (isFlipped())
        {
            PokerEvaluation e(_evalcode);
            e.flip();
            return e.str();
        }
        // ret += " ";
        ret += toStringCannon();
    }
    return ret;
}

string PokerEvaluation::toStringCannon() const
{
    const string highcard = "high card:    ";
    const string onepair = "one pair:     ";
    const string threeflush = "three flush:  ";
    const string threestraight = "three str8:   ";
    const string twopair = "two pair:     ";
    const string trips = "trips:        ";
    const string threestr8flush = "3 str8 flush: ";
    const string straight = "straight:     ";
    const string flush = "flush:        ";
    const string fullhouse = "full house:   ";
    const string quads = "quads:        ";
    const string straightflush = "str8 flush:   ";

    string ret;
    int n = _evalcode;

    if (isFlipped())
    {
        PokerEvaluation e(_evalcode);
        e.flip();
        return e.toStringCannon();
    }

    string hand;
    string topr;
    string botr;
    string kick;

    int val = n >> 24;
    switch (val)
    {
        case NO_PAIR:
            hand += highcard;
            kick += strKickers(n);
            break;

        case ONE_PAIR:
            hand += onepair;
            topr += strTop(n);
            kick += strKickers(n);
            break;

        case THREE_FLUSH:
            hand += threeflush;
            kick += strKickers(n);
            break;

        case THREE_STRAIGHT:
            hand += threestraight;
            topr += strTop(n);
            break;

        case TWO_PAIR:
            hand += twopair;
            topr += strTop(n);
            botr += strBot(n);
            kick += strKickers(n);
            break;

        case THREE_OF_A_KIND:
            hand += trips;
            topr += strTop(n);
            kick += strKickers(n);
            break;

        case THREE_STRAIGHT_FLUSH:
            hand += threestr8flush;
            topr += strTop(n);
            break;

        case STRAIGHT:
            hand += straight;
            topr += strTop(n);
            break;

        case FLUSH:
            hand += flush;
            topr += strKickers(n);
            break;

        case FULL_HOUSE:
            hand += fullhouse;
            topr += strTop(n);
            botr += strBot(n);
            break;

        case FOUR_OF_A_KIND:
            hand += quads;
            topr += strTop(n);
            botr += strKickers(n);
            break;

        case STRAIGHT_FLUSH:
            hand += straightflush;
            kick += strTop(n);
            break;
    }

    string ranks = topr + botr + kick;
    ret = (boost::format("%s %-5s") % hand % ranks).str();

    return ret;
}

void PokerEvaluation::generateLowballLookupA5()
{
    cout << "const int lowballA5Ranks[] = {\n";
    // this is a 13 bit lookup table
    for (int i = 0; i <= 0x1FFF; i++)
    {
        // ok, we are passing in plain old rankbits, but
        // geting A-5 lowball bits out, let's convert first off
        int bits = i << 1;
        if (bits & ACE_LOW_BIT)
            bits = bits - ACE_LOW_BIT + 1;

        int lowbits = 0;
        int nbits = 0;
        for (int b = 0; b < 13; b++)
        {
            if (bits & 0x01 << b)
            {
                lowbits += 0x01 << b;
                nbits++;
            }
            if (nbits == 5)
                break;
        }
        cout << boost::format("    %6d, // %s\n") % lowbits % toBitString(lowbits);
    }
    cout << "};\n";
}

void PokerEvaluation::generateBottomRankMask()
{
    cout << "const int bottomRankMask[] = {\n";
    // this is a 13 bit lookup table
    for (int i = 0; i <= 0x1FFF; i++)
    {
        int bits = i;
        int lowbits = 0;
        for (int b = 0; b < 13; b++)
        {
            if (bits & 0x01 << b)
            {
                lowbits += 0x01 << b;
                break;
            }
        }
        cout << boost::format("    %6d, // %s\n") % lowbits % toBitString(lowbits);
    }
    cout << "};\n";
}

std::ostream& operator<<(std::ostream& sout, const pokerstove::PokerEvaluation& e)
{
    sout << e.str();
    return sout;
}


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

#ifndef CRYPTO3_MAC_SIPHASH_FUNCTIONS_HPP
#define CRYPTO3_MAC_SIPHASH_FUNCTIONS_HPP

#include <boost/integer.hpp>
#include <boost/container/static_vector.hpp>

#include <nil/crypto3/mac/detail/siphash/siphash_policy.hpp>

namespace nil {
    namespace crypto3 {
        namespace mac {
            namespace detail {
                template<std::size_t Rounds, std::size_t FinalRounds>
                struct siphash_functions : public siphash_policy<Rounds, FinalRounds> {
                    typedef siphash_policy<Rounds, FinalRounds> policy_type;

                    constexpr static const std::size_t rounds = policy_type::rounds;
                    constexpr static const std::size_t final_rounds = policy_type::final_rounds;

                    constexpr static const std::size_t word_bits = policy_type::word_bits;
                    typedef typename policy_type::word_type word_type;

                    constexpr static const std::size_t key_bits = policy_type::key_bits;
                    constexpr static const std::size_t key_words = policy_type::key_words;
                    typedef typename policy_type::key_type key_type;

                    constexpr static const std::size_t key_schedule_bits = policy_type::key_schedule_bits;
                    constexpr static const std::size_t key_schedule_words = policy_type::key_schedule_words;
                    typedef typename policy_type::key_schedule_type key_schedule_type;

                    template<std::size_t InternalRounds>
                    void sip_rounds(key_schedule_type& V, word_type M) {
                        word_type V0 = V[0], V1 = V[1], V2 = V[2], V3 = V[3];

                        V3 ^= M;

                        for (size_t i = 0; i != InternalRounds; ++i) {
                            V0 += V1;
                            V2 += V3;
                            V1 = policy_type::template rotl<13>(V1);
                            V3 = policy_type::template rotl<16>(V3);
                            V1 ^= V0;
                            V3 ^= V2;
                            V0 = policy_type::template rotl<32>(V0);

                            V2 += V1;
                            V0 += V3;
                            V1 = policy_type::template rotl<17>(V1);
                            V3 = policy_type::template rotl<21>(V3);
                            V1 ^= V2;
                            V3 ^= V0;
                            V2 = policy_type::template rotl<32>(V2);
                        }
                        V0 ^= M;

                        V[0] = V0;
                        V[1] = V1;
                        V[2] = V2;
                        V[3] = V3;
                    }
                };
            }    // namespace detail
        }        // namespace mac
    }            // namespace crypto3
}    // namespace nil

#endif    // CRYPTO3_SIPHASH_POLICY_HPP


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2006 Giorgio Facchinetti

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

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

#ifndef quantlib_multistep_multistep_swaption_hpp
#define quantlib_multistep_multistep_swaption_hpp

#include <ql/models/marketmodels/products/multiproductmultistep.hpp>
#include <boost/shared_ptr.hpp>

namespace QuantLib {

    class StrikedTypePayoff;

    /*!
    Price a swaption associated to a contiguous subset of rates. Useful only for
    testing purposes. Steps through all rate times up to start of swap.
    */

    class MultiStepSwaption : public MultiProductMultiStep {
      public:
        MultiStepSwaption(const std::vector<Time>& rateTimes,
                                     Size startIndex,
                                     Size endIndex,
                                     boost::shared_ptr<StrikedTypePayoff> &, Size step=1);
        //! \name MarketModelMultiProduct interface
        //@{
        std::vector<Time> possibleCashFlowTimes() const;
        Size numberOfProducts() const;
        Size maxNumberOfCashFlowsPerProductPerStep() const;
        void reset();
        bool nextTimeStep(
                     const CurveState& currentState,
                     std::vector<Size>& numberCashFlowsThisStep,
                     std::vector<std::vector<CashFlow> >& cashFlowsGenerated);
        std::unique_ptr<MarketModelMultiProduct> clone() const;
         //@}

      private:

        Size startIndex_;
        Size endIndex_;
		Size step_;
        boost::shared_ptr<StrikedTypePayoff> payoff_;
        std::vector<Time> paymentTimes_;
        // things that vary in a path
        Size currentIndex_;
    };

    // Inline definitions

    inline std::vector<Time>
    MultiStepSwaption::possibleCashFlowTimes() const
    {
        return paymentTimes_;
    }

    inline Size MultiStepSwaption::numberOfProducts() const {
        return 1UL;
    }

    inline Size
    MultiStepSwaption::maxNumberOfCashFlowsPerProductPerStep() const {
        return 1;
    }

    inline void MultiStepSwaption::reset()
    {
       currentIndex_=0;
    }

}

#endif


/*
 * Licensed to the Apache Software Foundation (ASF) under one
 * or more contributor license agreements.  See the NOTICE file
 * distributed with this work for additional information
 * regarding copyright ownership.  The ASF licenses this file
 * to you under the Apache License, Version 2.0 (the
 * "License"); you may not use this file except in compliance
 * with the License.  You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing,
 * software distributed under the License is distributed on an
 * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
 * KIND, either express or implied.  See the License for the
 * specific language governing permissions and limitations
 * under the License.
 *
 * @file mlp_igd.cpp
 *
 * @brief Multilayer Perceptron functions
 *
 *//* ----------------------------------------------------------------------- */
#include <boost/lexical_cast.hpp>

#include <dbconnector/dbconnector.hpp>
#include <modules/shared/HandleTraits.hpp>

#include "mlp_igd.hpp"

#include "task/mlp.hpp"
#include "task/l2.hpp"
#include "algo/igd.hpp"
#include "algo/loss.hpp"

#include "type/tuple.hpp"
#include "type/model.hpp"
#include "type/state.hpp"

namespace madlib {

namespace modules {

namespace convex {

// These 2 classes contain public static methods that can be called
typedef IGD<MLPIGDState<MutableArrayHandle<double> >, MLPIGDState<ArrayHandle<double> >,
        MLP<MLPModel<MutableArrayHandle<double> >, MLPTuple > > MLPIGDAlgorithm;

typedef Loss<MLPIGDState<MutableArrayHandle<double> >, MLPIGDState<ArrayHandle<double> >,
        MLP<MLPModel<MutableArrayHandle<double> >, MLPTuple > > MLPLossAlgorithm;

typedef MLP<MLPModel<MutableArrayHandle<double> >,MLPTuple> MLPTask;

typedef MLPModel<MutableArrayHandle<double> > MLPModelType;

/**
 * @brief Perform the multilayer perceptron transition step
 *
 * Called for each tuple.
 */
AnyType
mlp_igd_transition::run(AnyType &args) {
    // For the first tuple: args[0] is nothing more than a marker that
    // indicates that we should do some initial operations.
    // For other tuples: args[0] holds the computation state until last tuple
    MLPIGDState<MutableArrayHandle<double> > state = args[0];

    // initilize the state if first tuple
    if (state.algo.numRows == 0) {
        if (!args[3].isNull()) {
            MLPIGDState<ArrayHandle<double> > previousState = args[3];

            state.allocate(*this, previousState.task.numberOfStages,
                           previousState.task.numbersOfUnits);
            state = previousState;
        } else {
            // configuration parameters
            ArrayHandle<double> numbersOfUnits = args[4].getAs<ArrayHandle<double> >();
            int numberOfStages = numbersOfUnits.size() - 1;

            double stepsize = args[5].getAs<double>();

            state.allocate(*this, numberOfStages,
                           reinterpret_cast<const double *>(numbersOfUnits.ptr()));
            state.task.stepsize = stepsize;

            const int activation = args[6].getAs<int>();
            const int is_classification = args[7].getAs<int>();

            const bool warm_start = args[9].getAs<bool>();
            const double lambda = args[11].getAs<double>();
            state.task.lambda = lambda;
            MLPTask::lambda = lambda;
            double is_classification_double = (double) is_classification;
            double activation_double = (double) activation;
            MappedColumnVector coeff = args[10].getAs<MappedColumnVector>();
            state.task.model.rebind(&is_classification_double,&activation_double,
                                    &coeff.data()[0], numberOfStages,
                                    &numbersOfUnits[0]);
        }
        // resetting in either case
        state.reset();
    }

    // meta data
    const uint16_t N = state.task.numberOfStages;
    const double *n = state.task.numbersOfUnits;

    // tuple
    ColumnVector indVar;
    MappedColumnVector depVar;
    try {
        indVar = args[1].getAs<MappedColumnVector>();
        MappedColumnVector y = args[2].getAs<MappedColumnVector>();
        depVar.rebind(y.memoryHandle(), y.size());
    } catch (const ArrayWithNullException &e) {
        return args[0];
    }
    MLPTuple tuple;
    tuple.indVar = indVar;
    tuple.depVar.rebind(depVar.memoryHandle(), depVar.size());
    tuple.weight = args[8].getAs<double>();

    MLPIGDAlgorithm::transition(state, tuple);
    MLPLossAlgorithm::transition(state, tuple);
    state.algo.numRows ++;

    return state;
}

/**
 * @brief Perform the perliminary aggregation function: Merge transition states
 */
AnyType
mlp_igd_merge::run(AnyType &args) {
    MLPIGDState<MutableArrayHandle<double> > stateLeft = args[0];
    MLPIGDState<ArrayHandle<double> > stateRight = args[1];

    if (stateLeft.algo.numRows == 0) { return stateRight; }
    else if (stateRight.algo.numRows == 0) { return stateLeft; }

    MLPIGDAlgorithm::merge(stateLeft, stateRight);
    MLPLossAlgorithm::merge(stateLeft, stateRight);

    // The following numRows update, cannot be put above, because the model
    // averaging depends on their original values
    stateLeft.algo.numRows += stateRight.algo.numRows;

    return stateLeft;
}

/**
 * @brief Perform the multilayer perceptron final step
 */
AnyType
mlp_igd_final::run(AnyType &args) {
    // We request a mutable object. Depending on the backend, this might perform
    // a deep copy.
    MLPIGDState<MutableArrayHandle<double> > state = args[0];

    if (state.algo.numRows == 0) { return Null(); }

    L2<MLPModelType>::lambda = state.task.lambda;
    state.algo.loss = state.algo.loss/static_cast<double>(state.algo.numRows);
    state.algo.loss += L2<MLPModelType>::loss(state.task.model);
    MLPIGDAlgorithm::final(state);
    return state;
}

/**
 * @brief Return the difference in RMSE between two states
 */
AnyType
internal_mlp_igd_distance::run(AnyType &args) {
    MLPIGDState<ArrayHandle<double> > stateLeft = args[0];
    MLPIGDState<ArrayHandle<double> > stateRight = args[1];
    return std::abs(stateLeft.algo.loss - stateRight.algo.loss);
}

/**
 * @brief Return the coefficients and diagnostic statistics of the state
 */
AnyType
internal_mlp_igd_result::run(AnyType &args) {
    MLPIGDState<ArrayHandle<double> > state = args[0];

    HandleTraits<ArrayHandle<double> >::ColumnVectorTransparentHandleMap
        flattenU;
    flattenU.rebind(&state.task.model.u[0](0, 0),
            state.task.model.arraySize(state.task.numberOfStages,
                    state.task.numbersOfUnits));
    double loss = state.algo.loss;

    AnyType tuple;
    tuple << flattenU
          << loss;
    return tuple;
}

AnyType
internal_predict_mlp::run(AnyType &args) {
    MLPModel<MutableArrayHandle<double> > model;
    ColumnVector indVar;
    int is_response = args[5].getAs<int>();
    MappedColumnVector x_means = args[6].getAs<MappedColumnVector>();
    MappedColumnVector x_stds = args[7].getAs<MappedColumnVector>();
    MappedColumnVector coeff = args[0].getAs<MappedColumnVector>();
    MappedColumnVector layerSizes = args[4].getAs<MappedColumnVector>();
    // Input layer doesn't count
    size_t numberOfStages = layerSizes.size()-1;
    double is_classification = args[2].getAs<double>();
    double activation = args[3].getAs<double>();
    bool get_class = is_classification && is_response;

    model.rebind(&is_classification,&activation,&coeff.data()[0],numberOfStages,&layerSizes.data()[0]);
    try {
        indVar = (args[1].getAs<MappedColumnVector>()-x_means).cwiseQuotient(x_stds);
    } catch (const ArrayWithNullException &e) {
        return args[0];
    }
    ColumnVector prediction = MLPTask::predict(model, indVar, get_class);
    return prediction;
}

} // namespace convex

} // namespace modules

} // namespace madlib


//Machine-generated by miind.py. Edit at your own risk.

#include <boost/timer/timer.hpp>
#include <GeomLib.hpp>
#include <MPILib/include/MPINetworkCode.hpp>
#include <MPILib/include/RateAlgorithmCode.hpp>
#include <MPILib/include/SimulationRunParameter.hpp>
#include <MPILib/include/report/handler/RootReportHandler.hpp>
#include <MPILib/include/WilsonCowanAlgorithm.hpp>
#include <MPILib/include/PersistantAlgorithm.hpp>
#include <MPILib/include/DelayAlgorithmCode.hpp>
#include <MPILib/include/RateFunctorCode.hpp>

typedef MPILib::MPINetwork<double, MPILib::utilities::CircularDistribution> Network;

int main(int argc, char *argv[]) {
    Network network;
    boost::timer::auto_cpu_timer t;

#ifdef ENABLE_MPI
    // initialise the mpi environment this cannot be forwarded to a class
    boost::mpi::environment env(argc, argv);
#endif

    try {	// generating algorithms
        const MPILib::Time t_mem_0 = 50e-3;
        const double f_noise_0 = 1.0;
        MPILib::Rate f_max_0 = 10.0;
        MPILib::Rate I_ext_0 = 0;
        MPILib::WilsonCowanParameter  par_wil_0(t_mem_0,f_max_0,f_noise_0,I_ext_0);
        MPILib::WilsonCowanAlgorithm alg_wc_0(par_wil_0);
        MPILib::RateAlgorithm<double> rate_alg_1(100.0);
        // generating nodes
        MPILib::NodeId id_0 = network.addNode(alg_wc_0,MPILib::EXCITATORY_GAUSSIAN);
        MPILib::NodeId id_1 = network.addNode(rate_alg_1,MPILib::EXCITATORY_GAUSSIAN);
        // generating connections
        double con_1_0(0.1);
        network.makeFirstInputOfSecond(id_1,id_0,con_1_0);
        // generation simulation parameter
        const MPILib::Time tmin = 0;
        const MPILib::Time tmax = 0.3;
        const MPILib::Rate fmin = 0;
        const MPILib::Rate fmax = 10;
        const MPILib::Potential statemin = 0;
        const MPILib::Potential statemax = 0.02;
        const MPILib::Potential densemin = 0;
        const MPILib::Potential densemax = 250;
        MPILib::CanvasParameter par_canvas(tmin,tmax,fmin,fmax,statemin,statemax,densemin,densemax);

        MPILib::report::handler::RootReportHandler handler("wilsoncowan",true,true, par_canvas);
        handler.addNodeToCanvas(id_0);
        SimulationRunParameter par_run( handler,1000000,0,0.3,1e-03,1e-03,"wilson.log",1e-03);
        network.configureSimulation(par_run);
        network.evolve();
    } catch(std::exception exc) {
        std::cout << exc.what() << std::endl;
#ifdef ENABLE_MPI
        //Abort the MPI environment in the correct way :
        env.abort(1);
#endif
    }

    MPILib::utilities::MPIProxy().barrier();
    t.stop();
    if (MPILib::utilities::MPIProxy().getRank() == 0) {

        std::cout << "Overall time spend\n";
        t.report();
    }
    return 0;
}


/*ckwg +29
 * Copyright 2017 by Kitware, Inc.
 * 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 name of Kitware, Inc. nor the names of any 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 AUTHORS OR CONTRIBUTORS BE LIABLE FOR
 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#include <vital/types/homography.h>

#include <Eigen/Core>

#include <pybind11/pybind11.h>
#include <pybind11/eigen.h>

namespace py=pybind11;
namespace kwiver {
namespace vital  {
namespace python {

class PyHomographyBase
{

  public:

    virtual ~PyHomographyBase() = default;

    virtual char get_type() {return '0';};
    virtual py::object get_matrix() {return py::none();};

    virtual py::object inverse() {return py::none();};
    virtual py::object map(py::object) {return py::none();};
    virtual py::object normalize() {return py::none();};

    bool operator==(std::shared_ptr<PyHomographyBase> &);
    bool operator!=(std::shared_ptr<PyHomographyBase> &other) {return !(*this == other);};
    virtual std::shared_ptr<PyHomographyBase> operator*(std::shared_ptr<PyHomographyBase> &) {return std::shared_ptr<PyHomographyBase>();};
};

class PyHomographyD
: public PyHomographyBase
{
  kwiver::vital::homography_<double> homog;

  public:

    PyHomographyD() {};
    PyHomographyD(Eigen::Matrix<double, 3, 3> mat) : homog(kwiver::vital::homography_<double>(mat)) {};
    PyHomographyD(kwiver::vital::homography_<double> mat) : homog(mat) {};

    char get_type() {return 'd';};
    py::object get_matrix() { return py::cast<Eigen::Matrix<double, 3, 3>>(homog.get_matrix()); };

    py::object inverse();
    py::object map(py::object);
    py::object normalize();

    std::shared_ptr<PyHomographyBase> operator*(std::shared_ptr<PyHomographyBase> &);
};

class PyHomographyF
: public PyHomographyBase
{
  kwiver::vital::homography_<float> homog;

  public:

    PyHomographyF() {};
    PyHomographyF(Eigen::Matrix<float, 3, 3> mat) : homog(kwiver::vital::homography_<float>(mat)) {};
    PyHomographyF(kwiver::vital::homography_<float> mat) : homog(mat) {};

    char get_type() {return 'f';};
    py::object get_matrix() { return py::cast<Eigen::Matrix<float, 3, 3>>(homog.get_matrix()); };

    py::object inverse();
    py::object map(py::object);
    py::object normalize();

    std::shared_ptr<PyHomographyBase> operator*(std::shared_ptr<PyHomographyBase> &);
};

py::object
PyHomographyD
::inverse()
{
  auto new_homog = this->homog.inverse();
  auto new_matrix = new_homog->matrix();
  return py::cast<Eigen::Matrix<double, 3, 3>>(new_matrix);
}

py::object
PyHomographyD
::map(py::object p_obj)
{
  auto p = p_obj.cast<Eigen::Matrix<double,2,1>>();
  auto new_matrix = this->homog.map_point(p);
  return py::cast<Eigen::Matrix<double, 2, 1>>(new_matrix);
}

py::object
PyHomographyD
::normalize()
{
  auto new_homog = this->homog.normalize();
  auto new_matrix = new_homog->matrix();
  return py::cast<Eigen::Matrix<double, 3, 3>>(new_matrix);
}

std::shared_ptr<PyHomographyBase>
PyHomographyD::
operator*(std::shared_ptr<PyHomographyBase> &other)
{
  auto mat = other->get_matrix().cast<Eigen::Matrix<double,3,3>>(); //this part is a bit silly, could be done better
  auto other_homog = kwiver::vital::homography_<double>(mat);
  return std::shared_ptr<PyHomographyBase>(new PyHomographyD(this->homog*other_homog));
}

py::object
PyHomographyF
::inverse()
{
  auto new_homog = this->homog.inverse();
  auto new_matrix = new_homog->matrix();
  return py::cast<Eigen::Matrix<double, 3, 3>>(new_matrix);
}

py::object
PyHomographyF
::map(py::object p_obj)
{
  auto p = p_obj.cast<Eigen::Matrix<float,2,1>>();
  auto new_matrix = this->homog.map_point(p);
  return py::cast<Eigen::Matrix<float, 2, 1>>(new_matrix);
}

py::object
PyHomographyF
::normalize()
{
  auto new_homog = this->homog.normalize();
  auto new_matrix = new_homog->matrix();
  return py::cast<Eigen::Matrix<double, 3, 3>>(new_matrix);
}

std::shared_ptr<PyHomographyBase>
PyHomographyF::
operator*(std::shared_ptr<PyHomographyBase> &other)
{
  auto mat = other->get_matrix().cast<Eigen::Matrix<float,3,3>>(); //this part is a bit silly, could be done better
  auto other_homog = kwiver::vital::homography_<float>(mat);
  return std::shared_ptr<PyHomographyBase>(new PyHomographyF(this->homog*other_homog));
}

bool
PyHomographyBase::
operator==(std::shared_ptr<PyHomographyBase> &other)
{
  if(this->get_type() == 'd')
  {
    auto this_mat = this->get_matrix().cast<Eigen::Matrix<double, 3, 3>>();
    auto other_mat = other->get_matrix().cast<Eigen::Matrix<double, 3, 3>>();
    return this_mat.isApprox(other_mat);
  }
  else if(this->get_type() == 'f')
  {
    auto this_mat = this->get_matrix().cast<Eigen::Matrix<float, 3, 3>>();
    auto other_mat = other->get_matrix().cast<Eigen::Matrix<float, 3, 3>>();
    return this_mat.isApprox(other_mat, 0.001);
  }

  return false;
}

std::shared_ptr<PyHomographyBase>
new_homography(char ctype)
{
  std::shared_ptr<PyHomographyBase> retVal;
  if(ctype == 'd')
  {
    retVal = std::shared_ptr<PyHomographyBase>(new PyHomographyD());
  }
  else if(ctype == 'f')
  {
    retVal = std::shared_ptr<PyHomographyBase>(new PyHomographyF());
  }
  return retVal;
}

std::shared_ptr<PyHomographyBase>
new_homography_from_matrix(py::object data_obj, char ctype)
{
  std::shared_ptr<PyHomographyBase> retVal;
  if(ctype == 'd')
  {
    auto data = data_obj.cast<Eigen::Matrix<double, 3,3>>();
    retVal = std::shared_ptr<PyHomographyBase>(new PyHomographyD(data));
  }
  else if(ctype == 'f')
  {
    auto data = data_obj.cast<Eigen::Matrix<float, 3,3>>();
    retVal = std::shared_ptr<PyHomographyBase>(new PyHomographyF(data));
  }
  return retVal;
}

std::shared_ptr<PyHomographyBase>
new_random_homography(char ctype)
{
  py::object data_obj;
  if(ctype == 'd')
  {
    data_obj = py::cast<Eigen::Matrix<double, 3, 3>>(Eigen::MatrixXd::Random(3,3));
  }
  else if(ctype == 'f')
  {
    data_obj = py::cast<Eigen::Matrix<float, 3, 3>>(Eigen::MatrixXf::Random(3,3));
  }
  return new_homography_from_matrix(data_obj, ctype);
}

}
}
}

using namespace kwiver::vital::python;
PYBIND11_MODULE(homography, m)
{
  py::class_<PyHomographyBase, std::shared_ptr<PyHomographyBase>>(m, "Homography")
  .def(py::init(&new_homography),
    py::arg("type")='d')
  .def_static("from_matrix", &new_homography_from_matrix,
    py::arg("data"), py::arg("type")='d')
  .def_static("random", &new_random_homography,
    py::arg("type")='d')
  .def_property_readonly("type_name", &PyHomographyBase::get_type)
  .def("as_matrix", &PyHomographyBase::get_matrix)
  .def("inverse", &PyHomographyBase::inverse)
  .def("map", &PyHomographyBase::map,
    py::arg("point"))
  .def("normalize", &PyHomographyBase::normalize)
  .def("__eq__", &PyHomographyBase::operator==)
  .def("__ne__", &PyHomographyBase::operator!=)
  .def("__mul__", &PyHomographyBase::operator*)
  ;
}


/*****************************************************************************
 *
 * This file is part of Mapnik (c++ mapping toolkit)
 *
 * Copyright (C) 2017 Artem Pavlenko
 *
 * This library 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 2.1 of the License, or (at your option) any later version.
 *
 * This library 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 for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 *
 *****************************************************************************/

#include <mapnik/geometry/interior.hpp>
#include <mapnik/geometry_envelope.hpp>
#include <mapnik/box2d.hpp>
#include <mapnik/geometry_centroid.hpp>

#include <algorithm>
#include <cmath>
#include <iostream>
#include <queue>

#pragma GCC diagnostic push
#include <mapnik/warning_ignore.hpp>
#include <boost/optional.hpp>
#pragma GCC diagnostic pop

namespace mapnik { namespace geometry {

// Interior algorithm is realized as a modification of Polylabel algorithm
// from https://github.com/mapbox/polylabel.
// The modification aims to improve visual output by prefering
// placements closer to centroid.

namespace detail {

// get squared distance from a point to a segment
template <class T>
T segment_dist_sq(const point<T>& p,
                  const point<T>& a,
                  const point<T>& b)
{
    auto x = a.x;
    auto y = a.y;
    auto dx = b.x - x;
    auto dy = b.y - y;

    if (dx != 0 || dy != 0) {

        auto t = ((p.x - x) * dx + (p.y - y) * dy) / (dx * dx + dy * dy);

        if (t > 1) {
            x = b.x;
            y = b.y;

        } else if (t > 0) {
            x += dx * t;
            y += dy * t;
        }
    }

    dx = p.x - x;
    dy = p.y - y;

    return dx * dx + dy * dy;
}

template <class T>
void point_to_ring_dist(point<T> const& point, linear_ring<T> const& ring,
                        bool & inside, double & min_dist_sq)
{
    for (std::size_t i = 0, len = ring.size(), j = len - 1; i < len; j = i++)
    {
        const auto& a = ring[i];
        const auto& b = ring[j];

        if ((a.y > point.y) != (b.y > point.y) &&
            (point.x < (b.x - a.x) * (point.y - a.y) / (b.y - a.y) + a.x)) inside = !inside;

        min_dist_sq = std::min(min_dist_sq, segment_dist_sq(point, a, b));
    }
}

// signed distance from point to polygon outline (negative if point is outside)
template <class T>
double point_to_polygon_dist(const point<T>& point, const polygon<T>& polygon)
{
    bool inside = false;
    double min_dist_sq = std::numeric_limits<double>::infinity();

    point_to_ring_dist(point, polygon.exterior_ring, inside, min_dist_sq);

    for (const auto& ring : polygon.interior_rings)
    {
        point_to_ring_dist(point, ring, inside, min_dist_sq);
    }

    return (inside ? 1 : -1) * std::sqrt(min_dist_sq);
}

template <class T>
struct fitness_functor
{
    fitness_functor(point<T> const& centroid, point<T> const& polygon_size)
        : centroid(centroid),
          max_size(std::max(polygon_size.x, polygon_size.y))
        {}

    T operator()(point<T> const& cell_center, T distance_polygon) const
    {
        if (distance_polygon <= 0)
        {
            return distance_polygon;
        }
        point<T> d(cell_center.x - centroid.x, cell_center.y - centroid.y);
        double distance_centroid = std::sqrt(d.x * d.x + d.y * d.y);
        return distance_polygon * (1 - distance_centroid / max_size);
    }

    point<T> centroid;
    T max_size;
};

template <class T>
struct cell
{
    template <class FitnessFunc>
    cell(const point<T>& c_, T h_,
         const polygon<T>& polygon,
         const FitnessFunc& ff)
        : c(c_),
          h(h_),
          d(point_to_polygon_dist(c, polygon)),
          fitness(ff(c, d)),
          max_fitness(ff(c, d + h * std::sqrt(2)))
        {}

    point<T> c; // cell center
    T h; // half the cell size
    T d; // distance from cell center to polygon
    T fitness; // fitness of the cell center
    T max_fitness; // a "potential" of the cell calculated from max distance to polygon within the cell
};

template <class T>
point<T> polylabel(polygon<T> const& polygon, box2d<T> const& bbox , T precision = 1)
{
    const point<T> size { bbox.width(), bbox.height() };

    const T cell_size = std::min(size.x, size.y);
    T h = cell_size / 2;

    // a priority queue of cells in order of their "potential" (max distance to polygon)
    auto compare_func = [] (const cell<T>& a, const cell<T>& b)
    {
        return a.max_fitness < b.max_fitness;
    };
    using Queue = std::priority_queue<cell<T>, std::vector<cell<T>>, decltype(compare_func)>;
    Queue queue(compare_func);

    if (cell_size == 0)
    {
        return { bbox.minx(), bbox.miny() };
    }

    point<T> centroid;
    if (!mapnik::geometry::centroid(polygon, centroid))
    {
        auto center = bbox.center();
        return { center.x, center.y };
    }

    fitness_functor<T> fitness_func(centroid, size);

    // cover polygon with initial cells
    for (T x = bbox.minx(); x < bbox.maxx(); x += cell_size)
    {
        for (T y = bbox.miny(); y < bbox.maxy(); y += cell_size)
        {
            queue.push(cell<T>({x + h, y + h}, h, polygon, fitness_func));
        }
    }

    // take centroid as the first best guess
    auto best_cell = cell<T>(centroid, 0, polygon, fitness_func);

    while (!queue.empty())
    {
        // pick the most promising cell from the queue
        auto current_cell = queue.top();
        queue.pop();

        // update the best cell if we found a better one
        if (current_cell.fitness > best_cell.fitness)
        {
            best_cell = current_cell;
        }

        // do not drill down further if there's no chance of a better solution
        if (current_cell.max_fitness - best_cell.fitness <= precision) continue;

        // split the cell into four cells
        h = current_cell.h / 2;
        queue.push(cell<T>({current_cell.c.x - h, current_cell.c.y - h}, h, polygon, fitness_func));
        queue.push(cell<T>({current_cell.c.x + h, current_cell.c.y - h}, h, polygon, fitness_func));
        queue.push(cell<T>({current_cell.c.x - h, current_cell.c.y + h}, h, polygon, fitness_func));
        queue.push(cell<T>({current_cell.c.x + h, current_cell.c.y + h}, h, polygon, fitness_func));
    }

    return best_cell.c;
}

} // namespace detail

template <class T>
bool interior(polygon<T> const& polygon, double scale_factor, point<T> & pt)
{
    if (polygon.exterior_ring.empty())
    {
        return false;
    }

    const box2d<T> bbox = envelope(polygon.exterior_ring);

    // Let the precision be 1% of the polygon size to be independent to map scale.
    double precision = (std::max(bbox.width(), bbox.height()) / 100.0) * scale_factor;

    pt = detail::polylabel(polygon, bbox, precision);
    return true;
}

template
bool interior(polygon<double> const& polygon, double scale_factor, point<double> & pt);

} }


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_SWAR_FUNCTIONS_SIMD_SSE_SSE2_ARITH_HPP_INCLUDED
#define BOOST_SIMD_SWAR_FUNCTIONS_SIMD_SSE_SSE2_ARITH_HPP_INCLUDED
#ifdef BOOST_SIMD_HAS_SSE2_SUPPORT

#include <boost/simd/swar/functions/arith.hpp>
#include <boost/simd/include/functions/simd/make.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::arith_
                                      , boost::simd::tag::sse2_
                                      , (T)
                                      , ((target_ < simd_ < double_<T>
                                          , boost::simd::tag::sse_
                                          >
                                         >
                                      ))
                                     )
  {
    typedef typename T::type result_type;
    result_type operator()(T const& ) const
    {
      return _mm_set_pd(1.0,0.0);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::arith_
                                      , boost::simd::tag::sse2_
                                      , (T)
                                      , ((target_ < simd_ <  single_<T>
                                          , boost::simd::tag::sse_
                                          >
                                         >
                                      ))
                                    )
  {
    typedef typename T::type result_type;
    result_type operator()(T const& ) const
    {
      return _mm_set_ps(3.0f, 2.0f, 1.0f, 0.0f);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::arith_
                                      , boost::simd::tag::sse2_
                                      , (T)
                                      , ((target_ < simd_ < ints64_<T>
                                          , boost::simd::tag::sse_
                                          >
                                         >
                                      ))
                                     )
  {
    typedef typename T::type result_type;
    result_type operator()(T const& ) const
    {
      return make<result_type>(0, 1);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::arith_
                                    , boost::simd::tag::sse2_
                                    , (T)
                                    , ((target_ < simd_ < ints32_<T>
                                                        , boost::simd::tag::sse_
                                                        >
                                                >
                                      ))
                                    )
  {
    typedef typename T::type result_type;

    result_type operator()(T const& ) const
    {
      return _mm_set_epi32(3,2,1,0);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::arith_
                                    , boost::simd::tag::sse2_
                                    , (T)
                                    , ((target_ < simd_ < ints16_<T>
                                                        , boost::simd::tag::sse_
                                                        >
                                                >
                                      ))
                                    )
  {
    typedef typename T::type result_type;

    result_type operator()(T const& ) const
    {
      return _mm_set_epi16(7,6,5,4,3,2,1,0);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::arith_
                                    , boost::simd::tag::sse2_
                                    , (T)
                                    , ((target_ < simd_ < ints8_<T>
                                                        , boost::simd::tag::sse_
                                                        >
                                                >
                                      ))
                                    )
  {
    typedef typename T::type result_type;

    result_type operator()(T const& ) const
    {
      return _mm_set_epi8( 15,14,13,12,11,10
                                        , 9, 8, 7, 6, 5, 4
                                        , 3, 2, 1, 0
                                        );
    }
  };

} } }

#endif
#endif


#include "RBGL.hpp"
#include "mincut.hpp"
#include <boost/graph/edmonds_karp_max_flow.hpp>
#include <boost/graph/push_relabel_max_flow.hpp>
#include <boost/graph/kolmogorov_max_flow.hpp>

typedef enum { E_MF_Push_Relabel, E_MF_Edmonds_Karp, E_MF_Kolmogorov } E_MF_METHOD;

static SEXP BGL_max_flow_internal(SEXP num_verts_in, SEXP num_edges_in,
                                  SEXP R_edges_in, SEXP R_capacity_in,
                                  SEXP src, SEXP sink, E_MF_METHOD method )
{
    using namespace boost;

    typedef adjacency_list_traits<vecS, vecS, directedS> Tr;
    typedef Tr::edge_descriptor Tr_edge_desc;

    typedef adjacency_list<vecS, vecS, directedS, no_property,
    		property<edge_capacity_t, double,
    		property<edge_residual_capacity_t, double,
    		property<edge_reverse_t, Tr_edge_desc> > > >
		FlowGraph;

    typedef graph_traits<FlowGraph>::edge_iterator   edge_iterator;
    typedef graph_traits<FlowGraph>::vertex_descriptor vertex_descriptor;

    FlowGraph flow_g;

    property_map < FlowGraph, edge_capacity_t >::type
    cap = get(edge_capacity, flow_g);
    property_map < FlowGraph, edge_residual_capacity_t >::type
    res_cap = get(edge_residual_capacity, flow_g);
    property_map < FlowGraph, edge_reverse_t >::type
    rev_edge = get(edge_reverse, flow_g);

    Tr::edge_descriptor e1, e2;
    bool in1, in2;

    if (!isInteger(R_edges_in)) error("R_edges_in should be integer");

    int NV = INTEGER(num_verts_in)[0];
    int NE = asInteger(num_edges_in);
    int* edges_in = INTEGER(R_edges_in);
    int*    capacity_i = (isReal(R_capacity_in)) ? 0 : INTEGER(R_capacity_in);
    double* capacity_d = (isReal(R_capacity_in)) ? REAL(R_capacity_in) : 0;

    for (int i = 0; i < NE ; i++, edges_in += 2)
    {
        tie(e1, in1) = boost::add_edge(*edges_in, *(edges_in+1), flow_g);
        tie(e2, in2) = boost::add_edge(*(edges_in+1), *edges_in, flow_g);
        if ( !in1 || !in2 )
            error("unable to add edge: (%d, %d)", *edges_in, *(edges_in+1));

        // fill in capacity_map
        cap[e1] = capacity_i ? (*capacity_i++) : (*capacity_d++);
        cap[e2] = 0;

        // fill in reverse_edge_map
        rev_edge[e1] = e2;
        rev_edge[e2] = e1;
    }

    double maxflow = 0;

    int vsrc = INTEGER(src)[0];
    int vsink = INTEGER(sink)[0];

    if ( 0 <= vsrc && vsrc < NV && 0 <= vsink && vsink < NV )
    {
    	vertex_descriptor s = vertex(vsrc, flow_g);
    	vertex_descriptor t = vertex(vsink, flow_g);

    	if ( method == E_MF_Push_Relabel )
	     maxflow = push_relabel_max_flow(flow_g, s, t);
	else if ( method == E_MF_Edmonds_Karp )
             maxflow = edmonds_karp_max_flow(flow_g, s, t);
	else if ( method == E_MF_Kolmogorov )
	{
	     error("kolmogorov_max_flow from BGL doesn't work");
	     //maxflow = kolmogorov_max_flow(flow_g, s, t);
	}
	else
	     error("unknown method for max_flow");
    }

    SEXP ansList, conn, eList, fList;
    PROTECT(ansList = allocVector(VECSXP,3));
    PROTECT(conn = NEW_NUMERIC(1));
    PROTECT(eList = allocMatrix(INTSXP, 2, asInteger(num_edges_in)));
    PROTECT(fList = allocMatrix(REALSXP, 1, asInteger(num_edges_in)));

    REAL(conn)[0] = maxflow;

    edge_iterator ei, e_end;
    int i = 0, j = 0;
    for (tie(ei, e_end) = edges(flow_g); ei != e_end; ++ei)
        if (cap[*ei] > 0) {
            INTEGER(eList)[i++] = source(*ei, flow_g);
            INTEGER(eList)[i++] = target(*ei, flow_g);
            REAL(fList)[j++] = cap[*ei] - res_cap[*ei];
        }

    SET_VECTOR_ELT(ansList,0,conn);
    SET_VECTOR_ELT(ansList,1,eList);
    SET_VECTOR_ELT(ansList,2,fList);
    UNPROTECT(4);

    return(ansList);
}

extern "C"
{
    SEXP BGL_min_cut_U (SEXP num_verts_in, SEXP num_edges_in,
                        SEXP R_edges_in,   SEXP R_weights_in )
    {
        using namespace boost;

        Graph_ud g(num_verts_in, num_edges_in, R_edges_in, R_weights_in);

        typedef graph_traits < Graph_ud >::vertex_descriptor Vertex;
        typedef graph_traits < Graph_ud >::edges_size_type dst;
        std::vector<Vertex> s_set, vs_set;
        dst cut_capacity = min_cut(g, std::back_inserter(s_set), std::back_inserter(vs_set) );

        SEXP ansList, conn;
        SEXP sList, vsList;  // for subsets of nodes in mincut: S, V - S

        PROTECT(ansList = allocVector(VECSXP,3));
        PROTECT(conn = NEW_NUMERIC(1));
        PROTECT(sList = allocVector(INTSXP, s_set.size()));
        PROTECT(vsList = allocVector(INTSXP,vs_set.size()));

        REAL(conn)[0] = (double)cut_capacity;

        std::vector<Vertex>::iterator vi;
        int i = 0;
        for ( i = 0, vi = s_set.begin(); vi != s_set.end(); i++, ++vi)
            INTEGER(sList)[i] = *vi;

        for ( i = 0, vi = vs_set.begin(); vi != vs_set.end(); i++, ++vi)
            INTEGER(vsList)[i] = *vi;

        SET_VECTOR_ELT(ansList,0,conn);
        SET_VECTOR_ELT(ansList,1,sList);
        SET_VECTOR_ELT(ansList,2,vsList);
        UNPROTECT(4);

        return(ansList);
    }

    SEXP BGL_edmonds_karp_max_flow(SEXP num_verts_in, SEXP num_edges_in,
                                   SEXP R_edges_in, SEXP R_capacity_in,
				   SEXP src, SEXP sink )
    {
        SEXP ansList = BGL_max_flow_internal(num_verts_in, num_edges_in,
                                             R_edges_in, R_capacity_in,
					     src, sink, E_MF_Edmonds_Karp);
        return(ansList);
    }

    SEXP BGL_push_relabel_max_flow(SEXP num_verts_in, SEXP num_edges_in,
                                   SEXP R_edges_in, SEXP R_capacity_in,
				   SEXP src, SEXP sink )
    {
        SEXP ansList = BGL_max_flow_internal(num_verts_in, num_edges_in,
                                             R_edges_in, R_capacity_in,
					     src, sink, E_MF_Push_Relabel);
        return(ansList);
    }

    SEXP BGL_kolmogorov_max_flow(SEXP num_verts_in, SEXP num_edges_in,
                                   SEXP R_edges_in, SEXP R_capacity_in,
				   SEXP src, SEXP sink )
    {
        SEXP ansList = BGL_max_flow_internal(num_verts_in, num_edges_in,
                                             R_edges_in, R_capacity_in,
					     src, sink, E_MF_Kolmogorov);
        return(ansList);
    }

}


#include "worldengine/generation.h"
#include "basic.h"
#include "simulations/biome.h"
#include "simulations/erosion.h"
#include "simulations/humidity.h"
#include "simulations/hydrology.h"
#include "simulations/icecap.h"
#include "simulations/irrigation.h"
#include "simulations/permeability.h"
#include "simulations/precipitation.h"
#include "simulations/temperature.h"

#include <queue>
#include <random>

#include <boost/log/trivial.hpp>
#include <boost/random.hpp>

#include <OpenSimplexNoise.h>

namespace WorldEngine
{

typedef std::pair<uint32_t, uint32_t> CoordType;

static void Around(std::vector<CoordType>& coordList,
                   int32_t                 x,
                   int32_t                 y,
                   int32_t                 width,
                   int32_t                 height);

/**
 * @brief Fill the ocean from the borders of the map
 * @param ocean Ocean data
 * @param elevation Elevation data
 * @param seaLevel The elevation representing the sea level
 */
static void FillOcean(OceanArrayType&           ocean,
                      const ElevationArrayType& elevation,
                      float                     seaLevel);

/**
 * @brief Make the ocean floor less noisy. The underwater erosion should cause
 * the ocean floor to be more uniform.
 * @param ocean Ocean data
 * @param elevation Elevation data
 * @param oceanLevel The elevation representing the ocean level
 */
static void HarmonizeOcean(const OceanArrayType& ocean,
                           ElevationArrayType&   elevation,
                           float                 oceanLevel);

/**
 * @brief A dynamic programming approach to gather how far the next land is from
 * a given coordinate up to a maximum distance of max_radius. Result is 0 for
 * land coordinates and -1 for coordinates further than max_radius away from
 * land.
 * @param ocean
 * @param maxRadius
 */
static boost::multi_array<int32_t, 2>
NextLandDynamic(const OceanArrayType& ocean, int32_t maxRadius = 5);

void AddNoiseToElevation(World& world, uint32_t seed)
{
   uint32_t octaves = 8;
   double   freq    = 16.0 * octaves;

   ElevationArrayType& elevation = world.GetElevationData();

   OpenSimplexNoise::Noise noise(seed);

   for (uint32_t y = 0; y < world.height(); y++)
   {
      for (uint32_t x = 0; x < world.width(); x++)
      {
         double n = Noise(noise, x / freq * 2.0, y / freq * 2.0, octaves);
         elevation[y][x] += static_cast<float>(n);
      }
   }
}

void CenterLand(World& world)
{
   ElevationArrayType& elevation = world.GetElevationData();
   PlateArrayType&     plates    = world.GetPlateData();

   std::vector<ElevationDataType> rowSums;
   std::vector<ElevationDataType> colSums;

   // Find row with the lowest elevation
   for (uint32_t y = 0; y < world.height(); y++)
   {
      ElevationDataType sum =
         std::accumulate(elevation[y].begin(), elevation[y].end(), 0.0f);
      rowSums.push_back(sum);
   }
   int32_t yWithMinSum = static_cast<int32_t>(
      std::min_element(rowSums.cbegin(), rowSums.cend()) - rowSums.cbegin());
   BOOST_LOG_TRIVIAL(debug)
      << "CenterLand(): Height complete (min y = " << yWithMinSum << ")";

   // Find column with the lowest elevation
   for (uint32_t x = 0; x < world.width(); x++)
   {
      ElevationArrayType::array_view<1>::type colView =
         elevation[boost::indices[ElevationArrayType::index_range()][x]];
      ElevationDataType sum =
         std::accumulate(colView.begin(), colView.end(), 0.0f);
      colSums.push_back(sum);
   }
   int32_t xWithMinSum = static_cast<int32_t>(
      std::min_element(colSums.cbegin(), colSums.cend()) - colSums.cbegin());
   BOOST_LOG_TRIVIAL(debug)
      << "CenterLand(): Width complete (min x = " << xWithMinSum << ")";

   int32_t latShift = 0;
   int32_t xOffset  = xWithMinSum;
   int32_t yOffset  = yWithMinSum - latShift;
   if (yOffset < 0)
      yOffset += world.height();

   // Rotate so the column with the lowest elevation is at the left edge
   for (uint32_t y = 0; y < world.height(); y++)
   {
      std::rotate(elevation[y].begin(),
                  elevation[y].begin() + xOffset,
                  elevation[y].end());
      std::rotate(
         plates[y].begin(), plates[y].begin() + xOffset, plates[y].end());
   }

   // Rotate so the row with the lowest elevation is at the top
   for (uint32_t x = 0; x < world.width(); x++)
   {
      ElevationArrayType::array_view<1>::type elevationColView =
         elevation[boost::indices[ElevationArrayType::index_range()][x]];
      PlateArrayType::array_view<1>::type platesColView =
         plates[boost::indices[ElevationArrayType::index_range()][x]];

      std::rotate(elevationColView.begin(),
                  elevationColView.begin() + yOffset,
                  elevationColView.end());
      std::rotate(platesColView.begin(),
                  platesColView.begin() + yOffset,
                  platesColView.end());
   }

   BOOST_LOG_TRIVIAL(debug) << "CenterLand(): Rotate complete";
}

void GenerateWorld(World& world, const Step& step, uint32_t seed)
{
   if (!step.includePrecipitations_)
   {
      return;
   }

   std::mt19937                                      generator(seed);
   boost::random::uniform_int_distribution<uint32_t> distribution(0,
                                                                  UINT32_MAX);

   // Seed map should be appended to to maximize compatibility between versions
   std::unordered_map<Simulation, uint32_t> seedMap;
   seedMap.insert({Simulation::Precipitation, distribution(generator)});
   seedMap.insert({Simulation::Erosion, distribution(generator)});
   seedMap.insert({Simulation::Watermap, distribution(generator)});
   seedMap.insert({Simulation::Irrigation, distribution(generator)});
   seedMap.insert({Simulation::Temperature, distribution(generator)});
   seedMap.insert({Simulation::Humidity, distribution(generator)});
   seedMap.insert({Simulation::Permeability, distribution(generator)});
   seedMap.insert({Simulation::Biome, distribution(generator)});
   seedMap.insert({Simulation::Icecap, distribution(generator)});

   TemperatureSimulation(world, seedMap[Simulation::Temperature]);
   PrecipitationSimulation(world, seedMap[Simulation::Precipitation]);

   if (!step.includeErosion_)
   {
      return;
   }

   ErosionSimulation(world);
   WatermapSimulation(world, seedMap[Simulation::Watermap]);
   IrrigationSimulation(world);
   HumiditySimulation(world);
   PermeabilitySimulation(world, seedMap[Simulation::Permeability]);
   BiomeSimulation(world);
   IcecapSimulation(world, seedMap[Simulation::Icecap]);
}

void InitializeOceanAndThresholds(World& world, float oceanLevel)
{
   ElevationArrayType& e     = world.GetElevationData();
   OceanArrayType&     ocean = world.GetOceanData();

   FillOcean(ocean, e, oceanLevel);

   float hl = FindThresholdF(e, 0.10f); // Highest 10% of land is hills
   float ml = FindThresholdF(e, 0.03f); // Highest 3% of land is mountains
   world.SetThreshold(ElevationThreshold::Sea, oceanLevel);
   world.SetThreshold(ElevationThreshold::Hill, hl);
   world.SetThreshold(ElevationThreshold::Mountain, ml);

   HarmonizeOcean(ocean, e, oceanLevel);

   SeaDepth(world, oceanLevel);
}

void PlaceOceansAtMapBorders(World& world)
{
   // Lower the elevation near the border of the map
   uint32_t oceanBorder =
      std::min(30u, std::max(world.width() / 5, world.height() / 5));
   ElevationArrayType& elevation = world.GetElevationData();

   auto PlaceOcean = [&](uint32_t x, uint32_t y, uint32_t i) {
      elevation[y][x] = elevation[y][x] * i / oceanBorder;
   };

   for (uint32_t x = 0; x < world.width(); x++)
   {
      for (uint32_t i = 0; i < oceanBorder; i++)
      {
         PlaceOcean(x, i, i);
         PlaceOcean(x, world.height() - i - 1, i);
      }
   }

   for (uint32_t y = 0; y < world.height(); y++)
   {
      for (uint32_t i = 0; i < oceanBorder; i++)
      {
         PlaceOcean(i, y, i);
         PlaceOcean(world.width() - i - 1, y, i);
      }
   }
}

static void Around(std::vector<CoordType>& coordList,
                   int32_t                 x,
                   int32_t                 y,
                   int32_t                 width,
                   int32_t                 height)
{
   for (int32_t dy = -1; dy <= 1; dy++)
   {
      int32_t ny = y + dy;
      if (0 <= ny && ny < height)
      {
         for (int32_t dx = -1; dx <= 1; dx++)
         {
            int32_t nx = x + dx;
            if (0 <= nx && nx < width && (dx != 0 || dy != 0))
            {
               coordList.push_back(CoordType(nx, ny));
            }
         }
      }
   }
}

static void FillOcean(OceanArrayType&           ocean,
                      const ElevationArrayType& elevation,
                      float                     seaLevel)
{
   const uint32_t height = static_cast<uint32_t>(elevation.shape()[0]);
   const uint32_t width  = static_cast<uint32_t>(elevation.shape()[1]);

   ocean.resize(boost::extents[height][width]);

   std::queue<CoordType> toExpand;

   // Handle top and bottom border of the map
   for (uint32_t x = 0; x < width; x++)
   {
      if (elevation[0][x] <= seaLevel)
      {
         toExpand.push(CoordType(x, 0));
      }
      if (elevation[height - 1][x] <= seaLevel)
      {
         toExpand.push(CoordType(x, height - 1));
      }
   }

   // Handle left- and rightmost border of the map
   for (uint32_t y = 0; y < height; y++)
   {
      if (elevation[y][0] <= seaLevel)
      {
         toExpand.push(CoordType(0, y));
      }
      if (elevation[y][width - 1] <= seaLevel)
      {
         toExpand.push(CoordType(width - 1, y));
      }
   }

   while (toExpand.size() > 0)
   {
      const CoordType& coord = toExpand.front();
      uint32_t         x     = coord.first;
      uint32_t         y     = coord.second;
      toExpand.pop();

      if (!ocean[y][x])
      {
         std::vector<CoordType> around;
         ocean[y][x] = true;

         Around(around, x, y, width, height);

         for (const CoordType& p : around)
         {
            int px = p.first;
            int py = p.second;
            if (!ocean[py][px] && elevation[py][px] <= seaLevel)
            {
               toExpand.push(p);
            }
         }
      }
   }
}

static void HarmonizeOcean(const OceanArrayType& ocean,
                           ElevationArrayType&   elevation,
                           float                 oceanLevel)
{
   const uint32_t width  = static_cast<uint32_t>(ocean.shape()[1]);
   const uint32_t height = static_cast<uint32_t>(ocean.shape()[0]);

   float shallowSea = oceanLevel * 0.85f;
   float midpoint   = shallowSea / 2.0f;

   for (uint32_t y = 0; y < height; y++)
   {
      for (uint32_t x = 0; x < width; x++)
      {
         if (ocean[y][x] && elevation[y][x] < shallowSea)
         {
            if (elevation[y][x] < midpoint)
            {
               elevation[y][x] =
                  midpoint - ((midpoint - elevation[y][x]) / 5.0f);
            }
            else if (elevation[y][x] > midpoint)
            {
               elevation[y][x] =
                  midpoint + ((elevation[y][x] - midpoint) / 5.0f);
            }
         }
      }
   }
}

static boost::multi_array<int32_t, 2>
NextLandDynamic(const OceanArrayType& ocean, int32_t maxRadius)
{
   const int32_t width  = static_cast<int32_t>(ocean.shape()[1]);
   const int32_t height = static_cast<int32_t>(ocean.shape()[0]);

   boost::multi_array<int32_t, 2> nextLand(boost::extents[height][width]);

   std::transform(ocean.data(),
                  ocean.data() + ocean.num_elements(),
                  nextLand.data(),
                  [](const bool& ocean) -> int32_t {
                     // Non-ocean tiles are zero distance away from next land
                     return (ocean) ? -1 : 0;
                  });

   for (int32_t distance = 0; distance < maxRadius; distance++)
   {
      for (int32_t y = 0; y < height; y++)
      {
         for (int32_t x = 0; x < width; x++)
         {
            if (nextLand[y][x] != distance)
            {
               continue;
            }

            for (int32_t dy = -1; dy <= 1; dy++)
            {
               int32_t ny = y + dy;
               if (0 <= ny && ny < height)
               {
                  for (int32_t dx = -1; dx <= 1; dx++)
                  {
                     int32_t nx = x + dx;
                     if (0 <= nx && nx < width)
                     {
                        if (nextLand[ny][nx] == -1)
                        {
                           nextLand[ny][nx] = distance + 1;
                        }
                     }
                  }
               }
            }
         }
      }
   }

   return nextLand;
}

void SeaDepth(World& world, float seaLevel)
{
   // We want to multiply the raw sea depth by one of these factors depending on
   // the distance from the next land
   const std::vector<float> factors({0.0f, 0.3f, 0.5f, 0.7f, 0.9f});

   const ElevationArrayType& elevation = world.GetElevationData();
   SeaDepthArrayType&        seaDepth  = world.GetSeaDepthData();

   seaDepth.resize(boost::extents[world.height()][world.width()]);

   boost::multi_array<int32_t, 2> nextLand =
      NextLandDynamic(world.GetOceanData());

   for (uint32_t y = 0; y < world.height(); y++)
   {
      for (uint32_t x = 0; x < world.width(); x++)
      {
         seaDepth[y][x] = seaLevel - elevation[y][x];

         int32_t distToNextLand = nextLand[y][x];
         if (distToNextLand > 0)
         {
            seaDepth[y][x] *= factors[distToNextLand - 1];
         }
      }
   }

   AntiAlias(seaDepth, 10);

   auto minmax = std::minmax_element(seaDepth.data(),
                                     seaDepth.data() + seaDepth.num_elements());

   const float minDepth = *minmax.first;
   const float maxDepth = *minmax.second;

   std::transform(seaDepth.data(),
                  seaDepth.data() + seaDepth.num_elements(),
                  seaDepth.data(),
                  [&minDepth, &maxDepth](const float& a) -> float {
                     return (a - minDepth) / (maxDepth - minDepth);
                  });
}

} // namespace WorldEngine


/* ------------Includes---------------------- */
#include <cmath>
#include <iostream>
#include <utility>
#include <unistd.h>
#include "../include/CAN_utils.h"
#include "../include/CO_message.h"
#include "../include/vehicle.h"
#include "../include/motor.h"
#include "../include/caster.h"
#include <Eigen/Core>
#include <Eigen/QR>
#include <Eigen/Geometry>
#include <Eigen/Dense>
#include <Eigen/LU>
#include <omniveyor_common/definitions.h>

// #define VELOCITY_SATURATION
// #define SEND_ZERO_TORQUES
#define CLAMP_TORQUES

using std::cout;
using std::endl;

/* --------------------------- STATIC FUNCTION PROTOTYPES ------------------------------- */
static double casterPosXSign(int casterNum);
static double casterPosYSign(int casterNum);
static Eigen::Vector3d clampVelocity(Eigen::Vector3d xd_com_in);
static Eigen::Vector3d clampAcceleration(Eigen::Vector3d xdd_com_in);
static float saturate(float x);

/* --------------------------- PUBLIC MEMBER FUNCTIONS ------------------------------- */

/* Constructor. Caster number starts at 1 */
Vehicle::Vehicle()
{
	_initialized = false;
	_enabled = false;
	_stopped = false;
	_control = VELOCITY;

	_x_local.setZero();
	_xd_local.setZero();
	_gx.setZero();
	_gxd.setZero();
	_gxd_com.setZero();
	_gxd_des.setZero();
	_gxdd.setZero();

	_g_cf.setZero();
	_cf_des_local.setZero();

	_q_steer.setZero();
	_qd.setZero();
	_qd_des.setZero();
	_tq.setZero();
	_tq_des.setZero();
	_lambda.setZero();
	_mu.setZero();
	_C.setZero();
	_Cp.setZero();
	_Jq.setZero();
	_C_pinv.setZero();

	_heading = 0;

	_Kp.setZero(); // zero gains for torque control initially
	_Kv.setZero(); // zero gains for torque control initially

	// create casters
	for(int i = 0; i < NUM_CASTERS; i++)
	{
		casters[i] = new Caster(i+1);
	}

	/* create a socket for the control thread */
	s = create_can_socket (UNUSED_NODE_ID, 0xF);
	if (s < 0)
	{
		perror ("Unable to open control socket\n");
		exit (-1);
	}
	//CO_Set_bitrate(s, 500000);
}

/* Destructor */
Vehicle::~Vehicle()
{
	_initialized = false;
	/* quick stop to motors - wait (x amount of time)*/

	/* destroy caster objects */
	for(int i = 0; i < NUM_CASTERS; i++)
	{
		delete casters[i];
	}
	/* reset comm */
	struct CO_message msg;
	msg.type = NMT;
	msg.m.NMT.data = 0x81;
	CO_send_message (s, 0, &msg);
	shutdown(s,1);
	//close(s);
}

/* Intializes all casters */
int Vehicle::init()
{
	for(int i = 0; i < NUM_CASTERS; i++)
	{
		if(casters[i]->init())
		{
			return -1;
		}
	}
	_initialized = true;
	return 0;
}

/* Enable all casters*/
int Vehicle::enable()
{
	if(!_initialized)
	{
		return -1;
	}

	for(int i = 0; i < NUM_CASTERS; i++)
	{
		if(casters[i]->enable())
		{
			return -1;
		}
	}

	_enabled = true;
	_stopped = false;
	return 0;
}

/* Disable all casters */
int Vehicle::disable()
{
	if(!_initialized)
	{
		return -1;
	}

	for(int i = 0; i < NUM_CASTERS; i++)
	{
		if(casters[i]->disable())
		{
			return -1;
		}
	}

	_enabled = false;
	return 0;
}

/* Stop all motors */
// HAVEN'T GOTTEN THIS TO WORK YET - CAN USE WHATEVER METHOD YOU WANT
int Vehicle::stop()
{
	if (!_enabled)
	{
		return -1;
	}

	for(int i = 0; i < NUM_CASTERS; i++)
	{
		if(casters[i]->stop() == 0)
		{
			cout << "stopping caster " << i << endl;
			// return -1;
		}
	}

	_stopped = true; // wait for stop?
	return 0;
}

int Vehicle::start()
{
	if (!_enabled)
	{
		return -1;
	}

	for(int i = 0; i < NUM_CASTERS; i++)
	{
		if(casters[i]->start() == 0)
		{
			cout << "starting caster " << i << endl;
			// return -1;
		}
	}

	_stopped = false; // wait for stop?
	return 0;
}

/* Set control mode to velocity or torque control.
 * This can only be set when the motors are disabled.
 */
int Vehicle::setCtrlMode(enum ctrl_mode cm)
{
	if(_enabled && !_stopped)
	{
		return -1; // only set mode if disabled
	}

	// Set mode
	_control = cm;
	cout << "Setting vehicle control mode to : " << cm << endl;
	for(int i = 0; i < NUM_CASTERS; i++)
	{
		if(casters[i]->setCtrlMode(cm))
		{
			return -1;
		}
	}

	return 0;
}

/* Update the local and global coordinates of the
 * operational point of the mobile base
 */
void Vehicle::updateOdometry(){
	updateJointData();
	// Eigen::Matrix<double, NUM_MOTORS, 1> deltaQ = getDeltaQ();

	// Calculate C_p^+
	updateConstraintPinvMatrix();

	// Find velocity and position of the base in the frame of the base with respect to the world origin
	// _xd_local = _C_pinv * deltaQ / CONTROL_PERIOD_s;
	_xd_local = _C_pinv * _qd;
	_x_local += _xd_local * CONTROL_PERIOD_s;

	_heading = _gx(2)+ 0.5*_xd_local(2)*CONTROL_PERIOD_s;

	// Rotation matrix to convert vectors from local frame to global frame
	Eigen::Matrix3d R = Eigen::Matrix3d::Zero();
	R(0, 0) =  cos(_heading);
    R(0, 1) = -sin(_heading);
    R(1, 0) =  sin(_heading);
    R(1, 1) =  cos(_heading);
    R(2, 2) =  1.0;

    _gxd = R * _xd_local;
    _gx += _gxd * CONTROL_PERIOD_s;
}

/* Returns the change in steer and roll angle */
Eigen::Matrix<double, NUM_MOTORS, 1> Vehicle::getDeltaQ(){
	Eigen::Matrix<double, NUM_MOTORS, 1> deltaQ;
	for(int i = 0; i < NUM_CASTERS; i++)
	{
		deltaQ(2*i) = casters[i]->getSteerDeltaQ();
		deltaQ(2*i + 1) = casters[i]->getRollDeltaQ();
	}
	return deltaQ;
}

/* Set vehicle velocities in x, y, and theta. vx,vy in m/s and vth in rad/s */
// (6.1)
int Vehicle::setGlobalVelocity(Eigen::Vector3d xd_com_in)
{
	// Retrieve latest joint data
	updateJointData();

	// Calculate C
	updateConstraintMatrix();

	// Store velocity command and determine current xd_des (limit acceleration)
	_gxd_com = clampVelocity(xd_com_in);
	_gxd_des = _gxd_des + clampAcceleration(_gxd_com - _gxd_des);

	// Calculate joint velocities to command
	_qd_des = _C * _gxd_des;

	// Set velocities for each caster
	for(int i = 0; i < NUM_CASTERS; i++)
	{
		if(casters[i]->setVelocities(_qd_des(2*i),_qd_des(2*i+1)))
		{
			return -1;
		}
	}

	return 0;
}

/* Set desired vehicle position, velocity, and acceleration for torque controller */
// (6.4)
void Vehicle::setTargets(Eigen::Array3d gx_des_in, Eigen::Array3d gxd_des_in, Eigen::Array3d gxdd_des_in)
{
	// update _lambda and mu
	updateDynamics();

	_gx_des   = gx_des_in;
	_gxd_des  = gxd_des_in;
	_gxdd_des = gxdd_des_in;

	// Calculate and set control forces/torques
#ifdef VELOCITY_SATURATION
	Eigen::Array3d dxd;
	dxd = _Kp / _Kv * (gx_des_in - _gx.array());
	Eigen::Array3d nu;
	nu(0) = saturate(MAX_VEL_X/abs(dxd(0)));
	nu(1) = saturate(MAX_VEL_Y/abs(dxd(1)));
	nu(2) = saturate(MAX_VEL_TH/abs(dxd(2)));
	Eigen::Vector3d g_cf_unit = -_Kv * (_gxd.array() - nu * dxd);
#else
	Eigen::Vector3d g_cf_unit = -_Kp * (_gx.array() - gx_des_in) - _Kv * (_gxd.array() - gxd_des_in) + gxdd_des_in;

#endif

	// Rotation matrix to convert vector from global frame to local frame
	Eigen::Matrix3d R = Eigen::Matrix3d::Zero();
	R(0, 0) = cos(_heading);
    R(0, 1) = sin(_heading);
    R(1, 0) = -sin(_heading);
    R(1, 1) = cos(_heading);
    R(2, 2) = 1.0;

	// Map global command to local coordinates
	Eigen::Array3d cf_unit_local = R * g_cf_unit;

	_cf_des_local = _lambda * cf_unit_local.matrix();
	// cf_des = _lambda * cf_unit.matrix() + mu;

	setTorque(_cf_des_local);
}

/* set _Kp gains */
void Vehicle::setKp(Eigen::Array3d Kp_des){
	_Kp = Kp_des;
}

/* set Kv gains */
void Vehicle::setKv(Eigen::Array3d Kv_des){
	_Kv = Kv_des;
}

/* Get control mode (velocity/torque) */
enum ctrl_mode Vehicle::getCtrlMode() const
{
	return _control;
}

/* Get position in m for x and y and rad for theta */
Eigen::Vector3d Vehicle::getGlobalPosition() const
{
	return _gx;
}

Eigen::Vector3d Vehicle::getLocalPosition() const
{
	return  _x_local;
}

/* Get vehicle velocity in m/s for x and y and rad/s for theta */
Eigen::Vector3d Vehicle::getGlobalVelocity() const
{
	return _gxd;
}

Eigen::Vector3d Vehicle::getLocalVelocity() const
{
	return _xd_local;
}

/* Get joint steering angles in rad */
Eigen::Matrix<double, NUM_CASTERS, 1> Vehicle::getJointSteeringAngles() const
{
	// updateJointData();
	return _q_steer;
}

/* Get desired joint velocities in rad/s */
Eigen::Matrix<double, NUM_MOTORS, 1> Vehicle::getDesJointVelocities() const
{
	return _qd_des;
}

/* Get joint velocities in rad/s */
Eigen::Matrix<double, NUM_MOTORS, 1> Vehicle::getJointVelocities() const
{
	return _qd;
}

/* Get vehicle (operational space) forces in x and y in N and torque in theta in Nm */
Eigen::Vector3d Vehicle::getLocalCommandForces() const
{
	return _cf_des_local;
}

/* Get joint torques in Nm */
Eigen::Matrix<double, NUM_MOTORS, 1> Vehicle::getDesJointTorques() const
{
	return _tq_des;
}

/* Get initialization status of vehicle */
bool Vehicle::isInitialized() const
{
	return _initialized;
}

/* Get enable status of vehicle */
bool Vehicle::isEnabled() const
{
	return _enabled;
}

/* Get status of vehicle */
bool Vehicle::isStopped() const
{
	return _stopped;
}

Eigen::Matrix3d Vehicle::getLambda() const
{
	return _lambda;
}

Eigen::MatrixXd Vehicle::getCPinv() const
{
	return _C_pinv;
}

/* Returns true if at least one bumper is hit */
bool Vehicle::isBumperHit() const
{
	bool retVal = false;
	for(int i = 0; i < NUM_CASTERS; i++)
	{
		if (casters[i]->getBumperState()){
			retVal = true;
		}
	}
	return retVal;
}

/* Returns a 4D vector containing the bumper state of each bumper */
Eigen::Vector4d Vehicle::getBumperState() const
{
	Eigen::Vector4d bumper_state;
	for(int i = 0; i < NUM_CASTERS; i++)
	{
		bumper_state[i] = casters[i]->getBumperState();
	}
	return bumper_state;
}

double Vehicle::getHeading() const
{
	return _heading;
}

void Vehicle::getElectricalStatus (robotElectrical_T *status)
{
	for (int i=0; i<NUM_CASTERS; i++){
		casters[i]->getAmps(&(status->steerMotorCurrent[i]), &(status->rollMotorCurrent[i]));
		casters[i]->getVolts(&(status->steerMotorVoltage[i]), &(status->rollMotorVoltage[i]));
	}
}

bool Vehicle::reachedTarget(Eigen::Vector3d curr_pos, Eigen::Vector3d curr_target, double max_linear_dist, double max_rot_dist) const
{
	return ((fabs(curr_pos[0]-curr_target[0]) < max_linear_dist) && (fabs(curr_pos[1]-curr_target[1]) < max_linear_dist) && (fabs(curr_pos[2]-curr_target[2]) < max_rot_dist));
}

/* --------------------------- PRIVATE MEMBER FUNCTIONS ------------------------------- */

/* Input: operational space forces fx,fy (N) and torque tz (Nm), all in local frame */
/* Outputs torques to casters */
int Vehicle::setTorque(const Eigen::Vector3d& cf_command)
{
	// Retrieve latest joint data
	updateJointData();

	// Calculate C#
	//updateConstraintMatrix();

	// Calculate C_p^+
	updateConstraintPinvMatrix();

	// Calculate joint torques
	// (6.11)
	_tq_des = _C_pinv.transpose() * cf_command;

#ifdef CLAMP_TORQUES
	clampTorque();
#endif

#ifdef SEND_ZERO_TORQUES
	_tq_des.setZero();
#endif

	for(int i = 0; i < NUM_CASTERS; i++)
	{
		if(casters[i]->setTorques(_tq_des(2*i),_tq_des(2*i+1)))
		{
			return -1;
		}
	}
	return 0;
}

/* Update joint data */
void Vehicle::updateJointData()
{
	std::pair<double,double> jointVel;
	for(int i = 0; i < NUM_CASTERS; i++)
	{
		_q_steer(i) = casters[i]->getSteerPosition();
		jointVel = casters[i]->getVelocities();
		_qd(2*i) = jointVel.first;
		_qd(2*i + 1) = jointVel.second;
	}
}

/* Mass Matrix taken from pg 20 of PCV Dyanmics.pdf*/
void Vehicle::updateDynamics()
{
	_lambda = Eigen::Matrix3d::Zero();
	_mu = Eigen::Vector3d::Zero();
	for(int i = 0; i < NUM_CASTERS; i++)
	{
		// Calculate relevant matrices and vectors
		Eigen::Matrix3d C_th = calcC_thMatrix(i);
		Eigen::Matrix3d A = calcAMatrix();
		Eigen::Vector3d p = calcpVector(i);
		Eigen::Matrix3d J_dot = calcJ_dotMatrix(i);
		Eigen::Vector3d Q_dot = calcQ_dotVector(i);

		//Calculate lambda_pumpkin and mu_pumpkin
		Eigen::Matrix3d lambda_pumpkin = calcLambdaPumpkinMatrix(i);
		Eigen::Vector3d mu_pumpkin = calcMuPumpkinVector(i);

		Eigen::Matrix3d lambda_i = C_th.transpose() * A * C_th + lambda_pumpkin;
		_lambda += lambda_i;

		Eigen::Vector3d mu_i = C_th.transpose()*(p - A*C_th*J_dot*Q_dot) + mu_pumpkin;
 		_mu += mu_i;
	}
	_lambda += calcLambdaVehicleMatrix();
}

/* Update kinematic constraint matrix, C#. Assumes joint data is up to date.
   Converts from operational space (xdot) to joint space (qdot). See pg 27 PCV_Dyanmics.pdf*/
void Vehicle::updateConstraintMatrix()
{
	for(int i = 0; i < NUM_CASTERS; i++)
	{
		int steerRow = 2*i;
		int rollRow = 2*i + 1;

		// Caster position values
		double hx = casterPosXSign(i+1) * DIST_TO_CASTER_X;
		double hy = casterPosYSign(i+1) * DIST_TO_CASTER_Y;

		// Take sin/cos of steeering angle
		double steerSin = sin(_q_steer(i));
		double steerCos = cos(_q_steer(i));

		// Compute steer row
		/**************** Alternative sign convention *******************/
		// C(steerRow,0) = -steerSin / PC_b;
		// C(steerRow,1) = steerCos / PC_b;
		// C(steerRow,2) = (hx * steerCos + hy * steerSin) / PC_b - 1.0;
		/****************************************************************/
		_C(steerRow,0) = steerSin / PC_b;
		_C(steerRow,1) = -steerCos / PC_b;
		_C(steerRow,2) = -(hx * steerCos + hy * steerSin) / PC_b - 1.0;

		// Compute roll row
		_C(rollRow,0) = steerCos / PC_r;
		_C(rollRow,1) = steerSin / PC_r;
		_C(rollRow,2) = (hx * steerSin - hy * steerCos) / PC_r;
	}
}

/* Update Moore-Penrose pseudoinverse of the constraint matrix C_p. Assumes constraint matrix, C#, is up to date */
void Vehicle::updateConstraintPinvMatrix()
{
	// Update Cp matrix with current data
	updateCpMatrix();

	Eigen::MatrixXd CptCli = Eigen::MatrixXd::Zero(3, NUM_MOTORS);

	for(int i = 0; i < NUM_CASTERS; i++)
	{
		// Cli IS 2x2 BLOCK DIAGONAL, SO MULTIPLY
	    // IN PIECES TO AVOID SLOW NxN MATRIX OPERATIONS

		// Take sin/cos of steering angle
		double steerSin = sin(_q_steer(i));
		double steerCos = cos(_q_steer(i));

		// Caster position values
		double hx = casterPosXSign(i+1) * DIST_TO_CASTER_X;
		double hy = casterPosYSign(i+1) * DIST_TO_CASTER_Y;

	    int j = 2*i;

	    // Note: sign of p-dot is: wheel as viewed from ground
	    CptCli(0, j ) =  PC_b * steerSin;
	    CptCli(0,j+1) =  PC_r * steerCos;
	    CptCli(1, j ) = -PC_b * steerCos;
	    CptCli(1,j+1) =  PC_r * steerSin;
	    CptCli(2, j ) = -PC_b * ((hx * steerCos + hy * steerSin) + PC_b);
	    CptCli(2,j+1) =  PC_r * (hx * steerSin - hy * steerCos);
	}

	_C_pinv = (_Cp.transpose()*_Cp).llt().solve(CptCli);
}

Eigen::Matrix3d Vehicle::calcAMatrix(){
	Eigen::Matrix3d A;
	A(0, 0) = PC_Mf*pow(PC_e, 2) + PC_If + PC_Ii + PC_Ih + PC_Is*pow(Ns, 2) + PC_It*Nr;
	A(0, 1) = PC_Ii*Nw - PC_Ih*Nw - PC_It*pow(Nr, 2)*Nw;
	A(0, 2) = PC_Mf*pow(PC_e, 2) + PC_If + PC_Ii + PC_Ih - PC_Is*Ns - PC_It*Nr;

	A(1, 0) = A(0, 1);
	A(1, 1) = PC_Mf*pow(PC_r, 2) + PC_Ii*pow(Nw, 2) + PC_Ih*pow(Nw, 2) + PC_It*pow(Nr, 2)*pow(Nw, 2) + PC_Ij;
	A(1, 2) = PC_Ii*Nw - PC_Ih*Nw + PC_It*Nr*Nw;

	A(2, 0) = A(0, 2);
	A(2, 1) = A(1, 2);
	A(2, 2) = PC_Mf*pow(PC_e, 2) + PC_If + PC_Ii + PC_Ih + PC_Is + PC_It;
	return A;
}

// PCV Dynamics page 21
Eigen::Vector3d Vehicle::calcpVector(int caster_no){
	double steer_d = _qd(2*caster_no);
	double roll_d  = _qd(2*caster_no+1);
	double gth_d   = _gxd(2);

	Eigen::Vector3d p;
	p(0) = PC_Mf*PC_r*PC_e*roll_d*(steer_d+gth_d);
	p(1) = -PC_Mf*PC_r*PC_e*pow((steer_d+gth_d), 2);
	p(2) = PC_Mf*PC_r*PC_e*roll_d*(steer_d+gth_d);
	return p;
}

// From PCV Dynamics page 29
Eigen::Matrix3d Vehicle::calcC_thMatrix(int caster_no)
{
	// Caster position values
	double hx = casterPosXSign(caster_no+1) * DIST_TO_CASTER_X;
	double hy = casterPosYSign(caster_no+1) * DIST_TO_CASTER_Y;

	// Take sin/cos of steering angle
	double steerSin = sin(_q_steer(caster_no));
	double steerCos = cos(_q_steer(caster_no));

	Eigen::Matrix3d C_th;
	/***  Alternate sign convention ***/
	// C_th(0, 0) = -steerSin / PC_b;
	// C_th(0, 1) = steerCos / PC_b;
	// C_th(0, 2) = (hx * steerCos + hy * steerSin) / PC_b - 1.0;
	/**********************************/
	C_th(0, 0) = steerSin / PC_b;
	C_th(0, 1) = -steerCos / PC_b;
	C_th(0, 2) = -(hx * steerCos + hy * steerSin) / PC_b - 1.0;

	C_th(1, 0) = steerCos / PC_r;
	C_th(1, 1) = steerSin / PC_r;
	C_th(1, 2) = (hx * steerSin - hy * steerCos) / PC_r;

	C_th(2, 0) = 0.0;
	C_th(2, 1) = 0.0;
	C_th(2, 2) = 1.0;

	return C_th;
}

// From PCV Dynamics page 25
Eigen::Matrix3d Vehicle::calcJ_dotMatrix(int caster_no)
{
	Eigen::Matrix3d J_dot;
	double steer_d = _qd(2*caster_no);
	double gth_d = _gxd(2);

	// Caster position values
	double hx = casterPosXSign(caster_no+1) * DIST_TO_CASTER_X;
	double hy = casterPosYSign(caster_no+1) * DIST_TO_CASTER_Y;

	// Take sin/cos of steeering angle
	double steerSin = sin(_q_steer(caster_no));
	double steerCos = cos(_q_steer(caster_no));

	// Take sin/cos of steer angle velocity plus PCV angle velocity
	double Sum = steer_d + gth_d;

	J_dot(0, 0) = PC_b * steerCos * Sum;
	J_dot(0, 1) = -PC_r * steerSin * Sum;
	J_dot(0, 2) = hx * gth_d + PC_b * steerCos * Sum;

	J_dot(1, 0) = PC_b * steerSin * Sum;
	J_dot(1, 1) = PC_r * steerCos * Sum;
	J_dot(1, 2) = hy * gth_d + PC_b * steerSin * Sum;

	J_dot(2, 0) = 0.0;
	J_dot(2, 1) = 0.0;
	J_dot(2, 2) = 0.0;

	return J_dot;
}

Eigen::Matrix3d Vehicle::calcLambdaPumpkinMatrix(int caster_no)
{
	Eigen::Matrix3d lambda_pumpkin = Eigen::Matrix3d::Zero();

	// Caster position values
	double hx = casterPosXSign(caster_no+1) * DIST_TO_CASTER_X;
	double hy = casterPosYSign(caster_no+1) * DIST_TO_CASTER_Y;

	lambda_pumpkin(0, 0) = PC_Mp;
	lambda_pumpkin(0, 2) = -PC_Mp*hy;

	lambda_pumpkin(1, 1) = PC_Mp;
	lambda_pumpkin(1, 2) = PC_Mp*hx;

	lambda_pumpkin(2, 0) = lambda_pumpkin(0, 2);
	lambda_pumpkin(2, 1) = lambda_pumpkin(1, 2);
	lambda_pumpkin(2, 2) = PC_Ip + PC_Mp*pow(DIST_TO_CASTER ,2);
	return lambda_pumpkin;
}

Eigen::Matrix3d Vehicle::calcLambdaVehicleMatrix()
{
	Eigen::Matrix3d lambda_vehicle = Eigen::Matrix3d::Zero();

	lambda_vehicle(0, 0) = PC_Mv; // Effective mass along the x direction is the mass of the vehicle
	lambda_vehicle(1, 1) = PC_Mv; // Effective mass along the y direction is the mass of the vehicle
	lambda_vehicle(2, 2) = PC_Iv; // Inertia of the vehicle about the z axis

	return lambda_vehicle;
}

Eigen::Vector3d Vehicle::calcQ_dotVector(int caster_no)
{
	Eigen::Vector3d Q_dot;

	double steer_d = _qd(2*caster_no);
	double roll_d = _qd(2*caster_no+1);
	double gth_d = _gxd(2);
	Q_dot << steer_d, roll_d, gth_d;

	return Q_dot;
}

Eigen::Vector3d Vehicle::calcMuPumpkinVector(int caster_no)
{
	Eigen::Vector3d mu_pumpkin;
	double gth_d = _gxd(2);

	// Caster position values
	double hx = casterPosXSign(caster_no+1) * DIST_TO_CASTER_X;
	double hy = casterPosYSign(caster_no+1) * DIST_TO_CASTER_Y;

	mu_pumpkin << -PC_Mp * hx * pow(gth_d, 2), -PC_Mp * hy * pow(gth_d, 2), 0.0;

	return mu_pumpkin;
}

/* Update Jq matrix. Transforms q_d's into p_d's (p_d = Jq*q_d). In documentation, Jq = C_q^{-1} */
void Vehicle::updateJqMatrix()
{
	_Jq = Eigen::MatrixXd::Zero(NUM_MOTORS, NUM_MOTORS);
	for(int i = 0; i < NUM_CASTERS; i++)
	{
		// Which rows to calculate
		int steerRow = 2*i;
		int rollRow = 2*i + 1;

		// Take sin/cos of steering angle
		double steerSin = sin(_q_steer(i));
		double steerCos = cos(_q_steer(i));

		// Compute steer row
		_Jq(steerRow,steerRow) = steerSin * PC_b;
		_Jq(steerRow,rollRow) = steerCos * PC_r;

		// Compute roll row
		_Jq(rollRow,steerRow) = -steerCos * PC_b;
		_Jq(rollRow,rollRow) = steerSin * PC_r;
	}
}

/* Update Cp matrix. Transforms x_d's into p_d's (p_d = C_p*x_d) */
void Vehicle::updateCpMatrix()
{
	for(int i = 0; i < NUM_CASTERS; i++)
	{
		// Which rows to calculate
		int steerRow = 2*i;
		int rollRow = 2*i + 1;

		// Take sin/cos of steering angle
		double steerSin = sin(_q_steer(i));
		double steerCos = cos(_q_steer(i));

		// Caster position values
		double hx = casterPosXSign(i+1) * DIST_TO_CASTER_X;
		double hy = casterPosYSign(i+1) * DIST_TO_CASTER_Y;

		// Cp = Jq*C, could calculate it this way instead
		// Compute steer row
		_Cp(steerRow,0) = 1.0;
		_Cp(steerRow,1) = 0.0;
		_Cp(steerRow,2) = -PC_b * steerSin - hy;

		// Compute roll row
		_Cp(rollRow,0) = 0.0;
		_Cp(rollRow,1) = 1.0;
		_Cp(rollRow,2) = PC_b * steerCos + hx;
	}
}

// Clamp the torque command to max values
void Vehicle::clampTorque(){
	// Clamp torque values to maximum allowed
	for(int i = 0; i < NUM_CASTERS; i++)
	{
		_tq_des(2*i)   = abs(_tq_des(2*i))   < MAX_STEER_TORQUE  ? _tq_des(2*i)   : copysign(MAX_STEER_TORQUE, _tq_des(2*i));
		_tq_des(2*i+1) = abs(_tq_des(2*i+1)) < MAX_ROLL_TORQUE   ? _tq_des(2*i+1) : copysign(MAX_ROLL_TORQUE,  _tq_des(2*i+1));
	}
}

/* ------------ Static functions-------------- */
// Determine sign of x position of caster based on number
static double casterPosXSign(int casterNum)
{
	switch(casterNum)
	{
		case 1:
		case 2:
			return 1.0;
		case 3:
		case 4:
			return -1.0;
		default:
			return 0;
	}
}

// Determine sign of y position of caster based on number
static double casterPosYSign(int casterNum)
{
	switch(casterNum)
	{
		case 2:
		case 3:
			return 1.0;
		case 1:
		case 4:
			return -1.0;
		default:
			return 0;
	}
}

/*  Saturation function
	Returns the input float value if its absolute value is less than or equal to 1.
	Otherwise, it returns 1.0 or -1.0 (depending on the sign of float x) */
static float saturate(float x) {
	x = abs(x) <= 1.0 ? x : copysign(1.0, x);
	return x;
}

// Function to clamp velocity command to max values
static Eigen::Vector3d clampVelocity(Eigen::Vector3d xd_com_in){
	// Clamp velocity values to maximum allowed
	xd_com_in(0) = abs(xd_com_in(0)) < MAX_VEL_TRANS ? xd_com_in(0) : copysign(MAX_VEL_TRANS, xd_com_in(0));
	xd_com_in(1) = abs(xd_com_in(1)) < MAX_VEL_TRANS ? xd_com_in(1) : copysign(MAX_VEL_TRANS, xd_com_in(1));
	xd_com_in(2) = abs(xd_com_in(2)) < MAX_VEL_ROT ? xd_com_in(2) : copysign(MAX_VEL_ROT, xd_com_in(2));

	return xd_com_in;
}

// Function to clamp acceleration command to max values (accel is change in xd_des for one control loop here)
static Eigen::Vector3d clampAcceleration(Eigen::Vector3d xdd_com_in){

	// Clamp acceleration values to maximum allowed

	// Check accel/decel for x
	if(xdd_com_in(0) >= 0){
		// Clamp trans accel
		xdd_com_in(0) = xdd_com_in(0) < MAX_VEL_TRANS_INC ? xdd_com_in(0) : MAX_VEL_TRANS_INC;
	}
	else{
		// Clamp trans decel
		xdd_com_in(0) = xdd_com_in(0) > -MAX_VEL_TRANS_DEC ? xdd_com_in(0) : -MAX_VEL_TRANS_DEC;
	}

	// Check accel/decel for y
	if(xdd_com_in(1) >= 0){
		// Clamp trans accel
		xdd_com_in(1) = xdd_com_in(1) < MAX_VEL_TRANS_INC ? xdd_com_in(1) : MAX_VEL_TRANS_INC;
	}
	else{
		// Clamp trans decel
		xdd_com_in(1) = xdd_com_in(1) > -MAX_VEL_TRANS_DEC ? xdd_com_in(1) : -MAX_VEL_TRANS_DEC;
	}

	// Check accel/decel for theta
	if(xdd_com_in(2) >= 0){
		// Clamp trans accel
		xdd_com_in(2) = xdd_com_in(2) < MAX_VEL_ROT_INC ? xdd_com_in(2) : MAX_VEL_ROT_INC;
	}
	else{
		// Clamp trans decel
		xdd_com_in(2) = xdd_com_in(2) > -MAX_VEL_ROT_DEC ? xdd_com_in(2) : -MAX_VEL_ROT_DEC;
	}

	return xdd_com_in;
}

/*********************** TEST HARNESS *****************************************/

#ifdef TEST_VEHICLE

// Different types of tests to run
enum test_type
{
	TEST_INIT,
	TEST_CONSTRAINT_MATRIX,
	TEST_DYNAMICS
};

int main (void)
{
	cout << "Begin vehicle test harness" << endl;

	/* instantiate vehicle */
	Vehicle *vehicle = new Vehicle ();

	// Which test to run
	enum test_type thisTest = TEST_DYNAMICS;

	int s = create_can_socket (0xF, 0xF);

	// Switch on type
	switch(thisTest){

		case TEST_DYNAMICS:
		{
			cout << "Testing Vehicle Dynamics" << endl;

		}

		case TEST_INIT:
			cout << "Testing Vehicle Initialization" << endl;

			if (vehicle->init())
				cout << "Vehicle initialization failed" << endl;
			else
				cout << "Vehicle initialization success" << endl;

			getchar ();
			break;

		case TEST_CONSTRAINT_MATRIX:
			{
				cout << "Testing Vehicle constraint matrix" << endl;

				Eigen::Matrix<double, NUM_MOTORS, 3> C; 		 		// constraint matrix
				Eigen::Matrix<double, NUM_CASTERS, 5> _q_steer;
				Eigen::Vector3d xd1;
				Eigen::Vector3d xd2;
				xd1 << 1,0,0;
				xd2 << 0,1,0;

				_q_steer << 0,0,0,0,
								   M_PI/2, M_PI/2, M_PI/2, M_PI/2,
									 M_PI,M_PI,M_PI,M_PI,
									 3*M_PI/2,3*M_PI/2,3*M_PI/2,3*M_PI/2,
									 -M_PI/2, -M_PI/2, -M_PI/2, -M_PI/2;

				// Re-compute constraint matrix C
				for(int j = 0; j < 5; j++){
					cout << "q_steer = [";
					for(int i = 0; i < NUM_CASTERS; i++)
					{
						// Which rows to calculate
						int steerRow = 2*i;
						int rollRow = 2*i + 1;

						// Caster position values
						double hx = casterPosXSign(i+1) * DIST_TO_CASTER_X;
						double hy = casterPosYSign(i+1) * DIST_TO_CASTER_Y;

						// Take sin/cos of steeering angle
						double steerSin = sin(_q_steer(i,j));
						double steerCos = cos(_q_steer(i,j));

						// Compute steer row
						C(steerRow,0) = -steerSin / PC_b;
						C(steerRow,1) =  steerCos / PC_b;
						C(steerRow,2) = (hx * steerCos + hy * steerSin) / PC_b - 1.0;

								// Compute roll row
						C(rollRow,0) = steerCos / PC_r;
						C(rollRow,1) = steerSin / PC_r;
						C(rollRow,2) = (hx * steerSin - hy * steerCos) / PC_r;

						cout << _q_steer(i,j) << ", ";
					}

					cout << "]" << endl;
					cout << endl << "C = " << endl;
					cout << C << endl;
					cout << "xd1 = " << xd1.transpose() << endl;
					cout << "qd1 = " << (C*xd1).transpose() << endl;
					cout << "xd2 = " << xd2.transpose() << endl;
					cout << "qd2 = " << (C*xd2).transpose() << endl;

					getchar();
				}
			}
			break;

		default:
			break;
	}

	// Delete caster
	delete vehicle;

	getchar ();
	cout << "End vehicle test harness" << endl;
	return 0;
}

#endif


#include <boost/accumulators/statistics/weighted_tail_mean.hpp>


//==================================================================================================
/*!
  @file

  @copyright 2012-2015 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_CONSTANT_DEFINITION_INF_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_DEFINITION_INF_HPP_INCLUDED

#include <boost/simd/config.hpp>
#include <boost/simd/detail/brigand.hpp>
#include <boost/simd/detail/dispatch.hpp>
#include <boost/simd/detail/constant_traits.hpp>
#include <boost/simd/constant/definition/valmax.hpp>
#include <boost/dispatch/function/make_callable.hpp>
#include <boost/dispatch/hierarchy/functions.hpp>
#include <boost/dispatch/as.hpp>

namespace boost { namespace simd
{
  namespace tag
  {
    struct inf_ : boost::dispatch::constant_value_<inf_>
    {
      BOOST_DISPATCH_MAKE_CALLABLE(ext,inf_,boost::dispatch::constant_value_<inf_>);

      struct value_map
      {
        template<typename X>
        static auto value(X const& x) -> decltype(valmax_::value_map::value(x));

        template<typename X>
        static brigand::single_<0x7F800000U> value(boost::dispatch::single_<X> const&);

        template<typename X>
        static brigand::double_<0x7FF0000000000000ULL> value(boost::dispatch::double_<X> const&);
      };
    };
  }

  namespace ext
  {
    BOOST_DISPATCH_FUNCTION_DECLARATION(tag,inf_);
  }

  namespace detail
  {
    BOOST_DISPATCH_CALLABLE_DEFINITION(tag::inf_,inf);
  }

  template<typename T> BOOST_FORCEINLINE auto Inf()
  -> decltype(detail::inf(boost::dispatch::as_<T>{}))
  {
    return detail::inf( boost::dispatch::as_<T>{} );
  }
} }

#endif