//Copyright (c) 2013 Singapore-MIT Alliance for Research and Technology
//Licensed under the terms of the MIT License, as described in the file:
//   license.txt   (http://opensource.org/licenses/MIT)

#include <boost/algorithm/string.hpp>
#include <boost/algorithm/string/trim.hpp>
#include <boost/nondet_random.hpp>
#include <boost/random.hpp>
#include <limits>
#include <cmath>

#include "Driver.hpp"
#include "entities/roles/driver/models/CarFollowModel.hpp"
#include "entities/vehicle/Vehicle.hpp"
#include "util/Math.hpp"
#include "util/Utils.hpp"

using std::numeric_limits;
using namespace sim_mob;
using namespace std;

namespace
{

double feet2Unit(double feet)
{
    return feet * 0.158;
}

double millisecondToSecond(double milliSec)
{
    return milliSec / 1000.0;
}

double calculateHeadway(double space, double speed, double elapsedSeconds, double maxAcceleration)
{
    if (speed == 0)
    {
        return -1;
    }
    else
    {
        return 2 * space / (speed + speed + elapsedSeconds * maxAcceleration);
    }
}

}

MITSIM_CF_Model::MITSIM_CF_Model(DriverUpdateParams &params, DriverPathMover *pathMover) : CarFollowingModel(pathMover)
{
    modelName = "general_driver_model";
    splitDelimiter = " ,";

    readDriverParameters(params);
}

MITSIM_CF_Model::~MITSIM_CF_Model()
{
}

void MITSIM_CF_Model::readDriverParameters(DriverUpdateParams &params)
{
    string speedScalarStr, maxAccStr, decelerationStr, maxAccScaleStr, normalDecScaleStr, maxDecScaleStr;
    string addOn, hBufferUpperStr;
    bool isAMOD = false;

    if (params.driver->getParent()->amodId != "-1")
    {
        isAMOD = true;
    }

    ParameterManager *parameterMgr = ParameterManager::Instance(isAMOD);

    parameterMgr->param(modelName, "speed_scaler", speedScalarStr, string("5 20 20"));
    parameterMgr->param(modelName, "max_acc_car1", maxAccStr, string("10.00  7.90  5.60  4.00  4.00"));
    createSpeedIndices(Vehicle::CAR, speedScalarStr, maxAccStr, maxAccelerationIndex, maxAccUpperBound);

    parameterMgr->param(modelName, "max_acceleration_scale", maxAccScaleStr, string("0.6 0.7 0.8 0.9 1.0 1.1 1.2 1.3 1.4 1.5"));
    createScaleIndices(maxAccScaleStr, maxAccelerationScale);

    parameterMgr->param(modelName, "normal_deceleration_car1", decelerationStr, string("7.8     6.7     4.8     4.8     4.8"));
    createSpeedIndices(Vehicle::CAR, speedScalarStr, decelerationStr, normalDecelerationIndex, normalDecelerationUpperBound);

    parameterMgr->param(modelName, "normal_deceleration_scale", normalDecScaleStr, string("1.0 1.0 1.0 1.0 1.0 1.0 1.0 1.0 1.0 1.0"));
    createScaleIndices(maxAccScaleStr, normalDecelerationScale);

    parameterMgr->param(modelName, "speed_limit_add_on", addOn, string("-0.1911 -0.0708 -0.0082 0.0397 0.0810 0.1248 0.1661 0.2180 0.2745 0.3657"));
    createScaleIndices(addOn, speedLimitAddon);

    parameterMgr->param(modelName, "Car_following_acceleration_add_on", addOn, string("-1.3564 -0.8547 -0.5562 -0.3178 -0.1036 0.1036 0.3178 0.5562 0.8547 1.3564"));
    Utils::convertStringToArray(addOn, accelerationAddon);

    parameterMgr->param(modelName, "Car_following_deceleration_add_on", addOn, string("-1.3187 -0.8309 -0.5407 -0.3089 -0.1007 0.1007 0.3089 0.5407 0.8309 1.3187"));
    Utils::convertStringToArray(addOn, decelerationAddon);

    parameterMgr->param(modelName, "max_deceleration_car1", decelerationStr, string("-16.0   -14.5   -13.0   -11.0   -9.0"));
    createSpeedIndices(Vehicle::CAR, speedScalarStr, decelerationStr, maxDecelerationIndex, maxDecelerationUpperBound);

    parameterMgr->param(modelName, "max_deceleration_scale", maxDecScaleStr, string("1.0 1.0 1.0 1.0 1.0 1.0 1.0 1.0 1.0 1.0"));
    createScaleIndices(maxDecScaleStr, maxDecelerationScale);

    parameterMgr->param(modelName, "acceleration_grade_factor", accGradeFactor, 0.305);
    parameterMgr->param(modelName, "tmp_all_grades", allGrades, 0.0);

    parameterMgr->param(modelName, "min_speed", minSpeed, 0.1);
    parameterMgr->param(modelName, "min_response_distance", minResponseDistance, 5.0);

    parameterMgr->param(modelName, "yellow_stop_headway", maxYellowLightHeadway, 1.0);
    parameterMgr->param(modelName, "min_speed_yellow", minYellowLightSpeed, 2.2352);

    parameterMgr->param(modelName, "hbuffer_lower", hBufferLower, 0.8);
    parameterMgr->param(modelName, "hbuffer_Upper", hBufferUpperStr, string("1.7498 2.2737 2.5871 2.8379 3.0633 3.2814 3.5068 3.7578 4.0718 4.5979"));
    createScaleIndices(hBufferUpperStr, hBufferUpperScale);
    hBufferUpper = getH_BufferUpperBound();

    string cfParamStr;
    parameterMgr->param(modelName, "CF_parameters_1", cfParamStr, string("0.0400, 0.7220, 0.2420, 0.6820, 0.6000, 0.8250"));
    createCF_Params(cfParamStr, CF_parameters[0]);

    parameterMgr->param(modelName, "CF_parameters_2", cfParamStr, string("-0.0418 0.0000 0.1510 0.6840 0.6800 0.8020"));
    createCF_Params(cfParamStr, CF_parameters[1]);

    string targetGapAccParmStr;
    parameterMgr->param(modelName, "target_gap_acc_parm", targetGapAccParmStr, string("0.604, 0.385, 0.323, 0.0678, 0.217,0.583, -0.596, -0.219, 0.0832, -0.170, 1.478, 0.131, 0.300"));
    createScaleIndices(targetGapAccParmStr, targetGapAccParm);

    string updateStepSizeStr;
    parameterMgr->param(modelName, "dec_update_step_size", updateStepSizeStr, string("0.5 0.0 0.5 0.5 0.5"));
    createUpdateSizeParams(updateStepSizeStr, decUpdateStepSize);

    parameterMgr->param(modelName, "speed_factor", speedFactor, 1.0);

    parameterMgr->param(modelName, "acc_update_step_size", updateStepSizeStr, string("1.0 0.0 1.0 1.0 0.5"));
    createUpdateSizeParams(updateStepSizeStr, accUpdateStepSize);

    parameterMgr->param(modelName, "uniform_speed_update_step_size", updateStepSizeStr, string("1.0 0.0 1.0 1.0 0.5"));
    createUpdateSizeParams(updateStepSizeStr, uniformSpeedUpdateStepSize);

    parameterMgr->param(modelName, "stopped_vehicle_update_step_size", updateStepSizeStr, string("0.5 0.0 0.5 0.5 0.5"));
    createUpdateSizeParams(updateStepSizeStr, stoppedUpdateStepSize);

    parameterMgr->param(modelName, "visibility_distance", visibilityDistance, 10.0);

    parameterMgr->param(modelName, "FF_Acc_Params_b2", params.FFAccParamsBeta, 0.3091);

    parameterMgr->param(modelName, "driver_signal_perception_distance", signalVisibilityDist, 75.0);

    boost::random_device seed_gen;
    long int seed = seed_gen();
    updateSizeRNG = boost::mt19937(seed);

    calcUpdateStepSizes();

    //Initialise step size, i = 3 is for stopped vehicle
    params.nextStepSize = updateStepSize[3];

    if (params.nextStepSize == 0)
    {
        params.nextStepSize = params.elapsedSeconds;
    }

    nextPerceptionSize = perceptionSize[3];
}

void MITSIM_CF_Model::createCF_Params(string &strParams, CarFollowingParams &cfParams)
{
    std::vector<std::string> arrayStr;
    vector<double> params;

    boost::trim(strParams);
    boost::split(arrayStr, strParams, boost::is_any_of(splitDelimiter), boost::token_compress_on);

    for (int i = 0; i < arrayStr.size(); ++i)
    {
        double res = 0;

        try
        {
            res = boost::lexical_cast<double>(arrayStr[i].c_str());
        }
        catch (boost::bad_lexical_cast&)
        {
            std::stringstream str;
            str << __func__ << ": Cannot convert " << strParams << " to type double";
            throw std::runtime_error(str.str());
        }

        params.push_back(res);
    }

    cfParams.alpha = params[0];
    cfParams.beta = params[1];
    cfParams.gama = params[2];
    cfParams.lambda = params[3];
    cfParams.rho = params[4];
    cfParams.stddev = params[5];
}

void MITSIM_CF_Model::createUpdateSizeParams(string &strParams, UpdateStepSizeParam &stepSizeParams)
{
    std::vector<std::string> arrayStr;
    vector<double> params;

    boost::trim(strParams);
    boost::split(arrayStr, strParams, boost::is_any_of(splitDelimiter), boost::token_compress_on);

    for (int i = 0; i < arrayStr.size(); ++i)
    {
        double res = 0;

        try
        {
            res = boost::lexical_cast<double>(arrayStr[i].c_str());
        }
        catch (boost::bad_lexical_cast&)
        {
            std::stringstream str;
            str << __func__ << ": Cannot convert " << strParams << " to type double";
            throw std::runtime_error(str.str());
        }

        params.push_back(res);
    }

    stepSizeParams.mean = params[0];
    stepSizeParams.stdev = params[1];
    stepSizeParams.lower = params[2];
    stepSizeParams.upper = params[3];
    stepSizeParams.perception = params[4];
}

void MITSIM_CF_Model::createScaleIndices(string &data, vector<double> &container)
{
    std::vector<std::string> arrayStr;

    boost::trim(data);
    boost::split(arrayStr, data, boost::is_any_of(splitDelimiter), boost::token_compress_on);

    for (int i = 0; i < arrayStr.size(); ++i)
    {
        double res = 0;

        try
        {
            res = boost::lexical_cast<double>(arrayStr[i].c_str());
        }
        catch (boost::bad_lexical_cast&)
        {
            std::stringstream str;
            str << __func__ << ": Cannot convert " << data << " to type double";
            throw std::runtime_error(str.str());
        }

        container.push_back(res);
    }
}

void MITSIM_CF_Model::createSpeedIndices(VehicleBase::VehicleType vhType, string &speedScalerStr, string &cstr,
                                         map<VehicleBase::VehicleType, map<int, double> > &idx, int &upperBound)
{
    //For example
    //speedScalerStr "5 20 20" ft/sec
    //maxAccStr      "10.00  7.90  5.60  4.00  4.00" ft/(s^2)
    std::vector<std::string> arrayStr;

    boost::trim(speedScalerStr);
    boost::split(arrayStr, speedScalerStr, boost::is_any_of(splitDelimiter), boost::token_compress_on);

    std::vector<double> speedScalerArrayDouble;

    for (int i = 0; i < arrayStr.size(); ++i)
    {
        double res = 0;
        try
        {
            res = boost::lexical_cast<double>(arrayStr[i].c_str());
        }
        catch (boost::bad_lexical_cast&)
        {
            std::stringstream str;
            str << __func__ << ": Cannot convert " << speedScalerStr << " to type double.";
            throw std::runtime_error(str.str());
        }

        speedScalerArrayDouble.push_back(res);
    }

    arrayStr.clear();

    boost::algorithm::trim(cstr);
    boost::split(arrayStr, cstr, boost::is_any_of(splitDelimiter), boost::token_compress_on);

    std::vector<double> cArrayDouble;

    for (int i = 0; i < arrayStr.size(); ++i)
    {
        double res = 0;
        try
        {
            res = boost::lexical_cast<double>(arrayStr[i].c_str());
        }
        catch (boost::bad_lexical_cast&)
        {
            std::stringstream str;
            str << __func__ << ": Cannot convert " << cstr << " to type double.";
            throw std::runtime_error(str.str());
        }

        cArrayDouble.push_back(res);
    }

    upperBound = round(speedScalerArrayDouble[1] * (speedScalerArrayDouble[0] - 1));

    map<int, double> cIdx;

    for (int speed = 0; speed <= upperBound; ++speed)
    {
        // Convert speed value to a table index.
        int j = speed / speedScalerArrayDouble[1];
        double maxAcc = 0;

        if (j >= (speedScalerArrayDouble[0] - 1))
        {
            maxAcc = cArrayDouble[speedScalerArrayDouble[0] - 1];
        }
        else
        {
            maxAcc = cArrayDouble[j];
        }

        cIdx.insert(std::make_pair(speed, maxAcc));
    }

    idx[vhType] = cIdx;
}

double MITSIM_CF_Model::getMaxAcceleration(DriverUpdateParams &params, VehicleBase::VehicleType vhType)
{
    int speed = params.perceivedFwdVelocity;

    if (speed < 0)
    {
        speed = 0;
    }

    if (speed > maxAccUpperBound)
    {
        speed = maxAccUpperBound;
    }

    double maxTableAcc = maxAccelerationIndex[vhType][speed];

    double maxAcc = (maxTableAcc - allGrades * accGradeFactor) * getMaxAccScalar();

    return maxAcc;
}

double MITSIM_CF_Model::getNormalDeceleration(DriverUpdateParams &params, VehicleBase::VehicleType vhType)
{
    int speed = params.perceivedFwdVelocity;

    if (speed < 0)
    {
        speed = 0;
    }

    if (speed > normalDecelerationUpperBound)
    {
        speed = normalDecelerationUpperBound;
    }

    double normalDec = normalDecelerationIndex[vhType][speed];

    double dec = (normalDec - allGrades * accGradeFactor) * getNormalDecScalar();

    return dec;
}

double MITSIM_CF_Model::getMaxDeceleration(DriverUpdateParams &params, VehicleBase::VehicleType vhType)
{
    int speed = params.perceivedFwdVelocity;

    if (speed < 0)
    {
        speed = 0;
    }

    if (speed > maxDecelerationUpperBound)
    {
        speed = maxDecelerationUpperBound;
    }

    double maxDec = maxDecelerationIndex[vhType][speed];

    double dec = (maxDec - allGrades * accGradeFactor) * getMaxDecScalar();

    return dec;
}

double MITSIM_CF_Model::getMaxAccScalar()
{
    int scaleNo = Utils::generateInt(1, maxAccelerationScale.size() - 1);
    double res = Utils::generateFloat(maxAccelerationScale[scaleNo - 1], maxAccelerationScale[scaleNo]);

    return res;
}

double MITSIM_CF_Model::getNormalDecScalar()
{
    int scaleNo = Utils::generateInt(1, normalDecelerationScale.size() - 1);
    double res = Utils::generateFloat(normalDecelerationScale[scaleNo - 1], normalDecelerationScale[scaleNo]);

    return res;
}

double MITSIM_CF_Model::getMaxDecScalar()
{
    int scaleNo = Utils::generateInt(1, maxDecelerationScale.size() - 1);
    double res = Utils::generateFloat(normalDecelerationScale[scaleNo - 1], normalDecelerationScale[scaleNo]);

    return res;
}

double MITSIM_CF_Model::getSpeedLimitAddon()
{
    int scaleNo = Utils::generateInt(1, speedLimitAddon.size() - 1);
    double res = Utils::generateFloat(speedLimitAddon[scaleNo - 1], speedLimitAddon[scaleNo]);

    return res;
}

double MITSIM_CF_Model::getAccelerationAddon()
{
    int scaleNo = Utils::generateInt(1, accelerationAddon.size() - 1);
    double res = Utils::generateFloat(accelerationAddon[scaleNo - 1], accelerationAddon[scaleNo]);

    return res;
}

double MITSIM_CF_Model::getDecelerationAddon()
{
    int scaleNo = Utils::generateInt(1, decelerationAddon.size() - 1);
    double res = Utils::generateFloat(decelerationAddon[scaleNo - 1], decelerationAddon[scaleNo]);

    return res;
}

double MITSIM_CF_Model::getH_BufferUpperBound()
{
    int scaleNo = Utils::generateInt(1, hBufferUpperScale.size() - 1);
    double res = Utils::generateFloat(hBufferUpperScale[scaleNo - 1], hBufferUpperScale[scaleNo]);

    return res;
}

double MITSIM_CF_Model::getHeadwayBuffer()
{
    return Utils::generateFloat(hBufferLower, hBufferUpper);
}

double MITSIM_CF_Model::makeAcceleratingDecision(DriverUpdateParams &params)
{
    params.cfDebugStr = "";

    //Calculate the state based variables for the current state
    calcStateBasedVariables(params);

    //Calculate the desired speed
    params.desiredSpeed = calcDesiredSpeed(params);

    //Select maximum acceleration by default
    double acceleration = params.maxAcceleration;
    params.accSelect = "max";

    //Calculate the different accelerations

    double aB = calcMergingAcc(params);
    double aC = calcTrafficSignalAcc(params);
    double aD = calcYieldingAcc(params);

    double aE = calcWaitForLaneExitAcc(params);
    //double aF = calcWaitForAllowedLaneAcc(params);

    //MISSING! > NOT YET IMPLEMENTED (@CLA_04/14)
    //double aG = calcLaneDropRate(p);

    double aH = params.maxAcceleration;
    std::string aHStr = "aH";

    //Check if we're in middle of performing lane change
    if (!params.getStatus(STATUS_LC_CHANGING))
    {
        if (params.getStatus(STATUS_ADJACENT))
        {
            aH = calcAdjacentGapRate(params);
            aHStr = "aHA";
        }
        else if (params.getStatus(STATUS_BACKWARD))
        {
            aH = calcBackwardGapAcc(params);
            aHStr = "aHB";
        }
        else if (params.getStatus(STATUS_FORWARD))
        {
            aH = calcForwardGapAcc(params);
            aHStr = "aHF";
        }
        else
        {
            aH = calcDesiredSpeedAcc(params);
            aHStr = "aHD";
        }
    }

    double aZ = calcCarFollowingAcc(params);
    double aSP = calcAccForStoppingPoint(params);

    //Choose the minimum acceleration

    if (acceleration > aB)
    {
        acceleration = aB;
        params.accSelect = "aB";
    }

    if (acceleration > aC)
    {
        acceleration = aC;
        params.accSelect = "aC";
    }

    if (acceleration > aD)
    {
        acceleration = aD;
        params.accSelect = "aD";
    }

    if (acceleration > aE)
    {
        acceleration = aE;
        params.accSelect = "aE";
    }

    if (acceleration > aH)
    {
        acceleration = aH;
        params.accSelect = aHStr;
    }

    if (acceleration > aC)
    {
        acceleration = aC;
        params.accSelect = "aC";
    }

    if (acceleration > aZ)
    {
        acceleration = aZ;
        params.accSelect = "aZ";
    }

    if (acceleration > aSP)
    {
        acceleration = aSP;
        params.accSelect = "aSP";
    }

    //SEVERAL CONDITONS MISSING! > NOT YET IMPLEMENTED (@CLA_04/14)

    //Update the selected the acceleration (for debugging)
    params.acc = acceleration;

    //If we are braking, alert the follower
    if (acceleration < -ACC_EPSILON)
    {
        //alert the vehicle behind if it is close
        if (params.nvBack.exists())
        {
            Driver *rearDriver = const_cast<Driver*> (params.nvBack.driver);
            DriverUpdateParams &rearDriverParams = rearDriver->getParams();

            if (params.nvBack.distance < visibilityDistance && !(rearDriver->IsBusDriver() && rearDriverParams.getStatus(STATUS_STOPPED)))
            {
                float alert = CF_CRITICAL_TIMER_RATIO * updateStepSize[0];
                rearDriverParams.reactionTimeCounter = std::min<double>(alert, rearDriverParams.reactionTimeCounter);
            }
        }
    }

    //Calculate the next step size
    params.reactionTimeCounter = calcNextStepSize(params);

    //If we're in emergency regime, reduce the reactionTimeCounter
    if (params.getStatus(STATUS_REGIME_EMERGENCY))
    {
        params.reactionTimeCounter = params.getNextStepSize() * CF_CRITICAL_TIMER_RATIO;
    }
    else
    {
        params.reactionTimeCounter = params.getNextStepSize();
    }

    return acceleration;
}

double MITSIM_CF_Model::calcCarFollowingAcc(DriverUpdateParams &params, NearestVehicle &nearestVehicle)
{
    double res = 0;
    std::stringstream debugStr;

    //Unset status
    params.unsetStatus(STATUS_REGIME_EMERGENCY);
    params.gapBetnVehicles = nearestVehicle.distance;

    debugStr << "t" << params.now.frame();

    params.headway = 99;
    if (nearestVehicle.distance == DBL_MAX)
    {
        res = calcFreeFlowingAcc(params, params.desiredSpeed);
        debugStr << "DEF;" << res;
    }
    else
    {
        debugStr << "ELSE;";

        params.velocityLeadVehicle = nearestVehicle.driver->getFwdVelocity();
        params.accLeadVehicle = params.driver->getFwdAcceleration();

        double dt = params.nextStepSize;
        float auxspeed = params.perceivedFwdVelocity == 0 ? 0.00001 : params.perceivedFwdVelocity;

        float headway = 2.0 * params.gapBetnVehicles / (auxspeed + params.perceivedFwdVelocity);

        debugStr << "+" << headway << "+" << params.gapBetnVehicles << "+" << auxspeed << "+" << params.perceivedFwdVelocity << ";";

        double emergSpace = nearestVehicle.distance;

        debugStr << emergSpace << ";";

        //If we have no space left to move, immediately cut off acceleration.
        if (emergSpace <= params.driver->getVehicleLength())
        {
            double speed = params.perceivedFwdVelocity;
            double emergHeadway = calculateHeadway(emergSpace, speed, params.elapsedSeconds, params.maxAcceleration);

            if (emergHeadway < hBufferLower)
            {
                //We need to brake. Override.
                params.gapBetnVehicles = emergSpace;
                headway = emergHeadway;
            }

            debugStr << "EM;" << emergHeadway << ";";
        }

        float v = params.velocityLeadVehicle + params.accLeadVehicle * dt;
        params.spaceStar = params.gapBetnVehicles + 0.5 * (params.velocityLeadVehicle + v) * dt;

        if (headway < hBufferLower)
        {
            res = calcEmergencyDeceleration(params);
            params.setStatus(STATUS_REGIME_EMERGENCY);
            debugStr << "LO;";
        }

        hBufferUpper = getH_BufferUpperBound();

        if (headway > hBufferUpper)
        {
            res = accOfMixOfCFandFF(params, params.desiredSpeed);
            debugStr << "UP;";
        }

        if (headway <= hBufferUpper && headway >= hBufferLower)
        {
            res = calcAccOfCarFollowing(params);
            debugStr << "LOUP;";
        }

        params.headway = headway;
    }

    params.cfDebugStr += debugStr.str();

    return res;
}

double MITSIM_CF_Model::calcCarFollowingAcc(DriverUpdateParams &params)
{
    double aZ1 = calcCarFollowingAcc(params, params.nvFwd);
    double aZ2 = calcCarFollowingAcc(params, params.nvFwdNextLink);

    return std::min(aZ1, aZ2);
}

double MITSIM_CF_Model::calcMergingAcc(DriverUpdateParams &params)
{
    double acc = params.maxAcceleration;

    //Vehicles from freeways and on ramps have different priority.
    //Separate procedures are applied. (MITSIM TS_CFModels.cc)

    if (!fwdDriverMovement->isInIntersection())
    {
        if (fwdDriverMovement->getCurrLink()->getLinkType() == LINK_TYPE_EXPRESSWAY)
        {
            if (params.nvLeadFreeway.exists())
            {
                double headway = getHeadwayBuffer();

                if (params.nvLeadFreeway.distance < headway)
                {
                    //MITSIM TS_CFModels.cc
                    acc = params.nvLeadFreeway.driver->getFwdAcceleration()
                            + calcTargetSpeedAcc(params, params.nvLeadFreeway.distance, params.nvLeadFreeway.driver->getFwdVelocity());
                }
            }
        }
        else if (fwdDriverMovement->getCurrLink()->getLinkType() == LINK_TYPE_RAMP)
        {
            if (params.nvLagFreeway.exists())
            {
                if (!isGapAcceptable(params, params.nvLagFreeway))
                {
                    // The gap is not acceptable. Prepare to stop.
                    return calcBrakeToStopAcc(params, params.distToStop);
                }
            }
            else
            {
                if (params.nvLeadFreeway.exists())
                {
                    //Lead vehicle exists, brake to target speed
                    acc = calcTargetSpeedAcc(params, params.nvLeadFreeway.distance, params.nvLeadFreeway.driver->getFwdVelocity());
                }
            }
        }
    }

    return acc;
}

bool MITSIM_CF_Model::isGapAcceptable(DriverUpdateParams &params, NearestVehicle &nearestVehicle)
{
    float acceleration = params.maxAcceleration;
    float speedOfOtherVehicle = nearestVehicle.driver->getFwdVelocity();

    Driver *otherDriver = const_cast<Driver *> (nearestVehicle.driver);
    DriverUpdateParams &otherDriverParams = otherDriver->getParams();

    float maxAcc = otherDriverParams.maxAcceleration;
    float speed;

    double dt = params.nextStepSize;

    double distance = params.distToStop;
    double currentSpeed = params.driver->getFwdVelocity();

    if (distance > Math::DOUBLE_EPSILON)
    {
        //maximum speed can be reached at the end of the lane.
        speed = currentSpeed * currentSpeed + 2.0 * acceleration * distance;
        speed = (speed > Math::DOUBLE_EPSILON) ? sqrt(speed) : 0.0;

        //Shortest time required to arrive at the end of the lane
        dt = (speed - currentSpeed) / acceleration;
    }
    else
    {
        speed = currentSpeed;
        dt = 0;
    }

    float gap = nearestVehicle.distance;

    // Speed at the predicted position
    speedOfOtherVehicle += maxAcc * dt;

    float sd = (speedOfOtherVehicle - speed) * getHeadwayBuffer();
    float threshold = (sd > 0.0) ? sd : 0.0;

    //Check if the gap is acceptable
    if (gap > threshold)
    {
        return true;
    }
    else
    {
        return false;
    }
}

double MITSIM_CF_Model::calcTrafficSignalAcc(DriverUpdateParams &p)
{
    double minAcc = p.maxAcceleration;
    TrafficColor color = p.perceivedTrafficColor;
    double distanceToTrafficSignal = p.perceivedDistToTrafficSignal;

    //The check for current lane ensures that we pass through the intersection without getting stuck if the driver reacts a little late
    //for the traffic light causing the vehicle to enter the intersection slightly
    if (distanceToTrafficSignal < signalVisibilityDist && p.currLane)
    {
        if (color == TRAFFIC_COLOUR_RED)
        {
            minAcc = std::min(calcBrakeToStopAcc(p, distanceToTrafficSignal), minAcc);
        }
        else if (color == TRAFFIC_COLOUR_AMBER)
        {
            double maxSpeed = (p.perceivedFwdVelocity > minYellowLightSpeed) ? p.perceivedFwdVelocity : minYellowLightSpeed;

            if (distanceToTrafficSignal / maxSpeed > maxYellowLightHeadway)
            {
                minAcc = std::min(calcBrakeToStopAcc(p, distanceToTrafficSignal), minAcc);
            }
        }
        else if (color == TRAFFIC_COLOUR_GREEN)
        {
            minAcc = p.maxAcceleration;
        }
    }

    return minAcc;
}

double MITSIM_CF_Model::calcYieldingAcc(DriverUpdateParams &params)
{
    if (!fwdDriverMovement->isInIntersection())
    {
        float acc = 0;
        params.lcDebugStr << ";---CYR";

        if (params.flag(FLAG_YIELDING))
        {
            // This vehicle is yielding to another vehicle

            params.lcDebugStr << ";DING" << params.driver->getYieldingToDriver()->getParent()->getId();

            uint32_t dt_sec = millisecondToSecond(params.now.ms() - params.yieldTime.ms());

            params.lcDebugStr << ";dt" << dt_sec;

            //Make sure a vehicle will not yield infinitely.
            if (dt_sec > params.lcMaxYieldingTime)
            {
                params.driver->setYieldingToDriver(NULL);
                params.unsetFlag(FLAG_YIELDING);
                params.lcDebugStr << ";yd1";
                return params.maxAcceleration;
            }

            bool rightFwdVhFlag = false;

            if (params.nvRightFwd.exists())
            {
                params.lcDebugStr << ";yd2";

                Driver* dRF = const_cast<Driver*> (params.nvRightFwd.driver);
                DriverUpdateParams& pRF = dRF->getParams();

                if (pRF.flag(FLAG_NOSING_LEFT))
                {
                    rightFwdVhFlag = true;
                    params.lcDebugStr << ";yd3";
                }
            }

            bool leftFwdVhFlag = false;

            if (params.nvLeftFwd.exists())
            {
                params.lcDebugStr << ";yd4";

                Driver* d = const_cast<Driver*> (params.nvLeftFwd.driver);
                DriverUpdateParams& p = d->getParams();

                if (p.flag(FLAG_NOSING_RIGHT))
                {
                    leftFwdVhFlag = true;
                    p.lcDebugStr << ";yd5";
                }
            }

            if (params.flag(FLAG_YIELDING_RIGHT))
            {
                params.lcDebugStr << ";yd6";

                //Check if the right front vehicle is nosing
                if ((params.rightLane) && (params.nvRightFwd.exists()) && params.nvRightFwd.driver == params.driver->getYieldingToDriver() && rightFwdVhFlag)
                {
                    params.lcDebugStr << ";yd7";

                    acc = calcCarFollowingAcc(params, params.nvRightFwd);

                    if (acc < params.normalDeceleration)
                    {
                        acc = params.normalDeceleration;
                    }
                    else if (acc > 0)
                    {
                        acc = 0.0;
                    }

                    params.lcDebugStr << ";acc" << acc;

                    return acc;
                }
            }
            else if (params.flag(FLAG_YIELDING_LEFT))
            {
                params.lcDebugStr << ";yd8";

                //Check if the left front vehicle is nosing
                if ((params.leftLane) && (params.nvLeftFwd.exists()) && params.nvLeftFwd.driver == params.driver->getYieldingToDriver() && leftFwdVhFlag)
                {
                    params.lcDebugStr << ";yd9";

                    acc = calcCarFollowingAcc(params, params.nvLeftFwd);

                    if (acc < params.normalDeceleration)
                    {
                        acc = params.normalDeceleration;
                    }
                    else if (acc > 0)
                    {
                        acc = 0.0;
                    }

                    params.lcDebugStr << ";acc" << acc;

                    return acc;
                }
            }

            params.driver->setYieldingToDriver(NULL);
            params.unsetFlag(FLAG_YIELDING);

            params.lcDebugStr << ";yd10" << acc;

            return params.maxAcceleration;
        }
        else if (params.flag(FLAG_NOSING))
        {
            // This vehicle is nosing

            params.lcDebugStr << ";SING";

            bool rightBackVhFlag = false;

            if (params.nvRightBack.exists())
            {
                Driver* d = const_cast<Driver*> (params.nvRightBack.driver);
                DriverUpdateParams& pd = d->getParams();

                if (pd.flag(FLAG_YIELDING_LEFT) || pd.flag(FLAG_NOSING))
                {
                    rightBackVhFlag = true;
                }
            }

            bool leftBackVhFlag = false;

            if (params.nvLeftBack.exists())
            {
                Driver* d = const_cast<Driver*> (params.nvLeftBack.driver);
                DriverUpdateParams& pd = d->getParams();

                if (pd.flag(FLAG_YIELDING_RIGHT) || pd.flag(FLAG_NOSING))
                {
                    leftBackVhFlag = true;
                }
            }

            if (params.flag(FLAG_NOSING_RIGHT))
            {
                params.lcDebugStr << ";RT";
                params.lcDebugStr << ";RBD" << params.nvRightBack.distance;
                params.lcDebugStr << ";RFD" << params.nvRightFwd.distance;

                //Check if the right rear vehicle is yielding to the left
                if ((params.rightLane) && (params.nvRightBack.exists()) && rightBackVhFlag)
                {
                    acc = calcAccToCreateGap(params, params.nvRightFwd, params.lcMinGap(2) + Math::DOUBLE_EPSILON);
                    double res = std::max<double>(params.maxDeceleration, acc);

                    params.lcDebugStr << ";acc" << acc;

                    return res;
                }
            }
            else if (params.flag(FLAG_NOSING_LEFT))
            {
                params.lcDebugStr << ";LT";

                if ((params.leftLane) && (params.nvLeftBack.exists()) && leftBackVhFlag)
                {
                    acc = calcAccToCreateGap(params, params.nvLeftFwd, params.lcMinGap(2) + Math::DOUBLE_EPSILON);
                    return std::max<double>(params.maxDeceleration, acc);
                }
            }

            if (params.getStatus(STATUS_CHANGING) && params.flag(FLAG_LC_FAILED_LEAD))
            {
                return params.normalDeceleration;
            }
            else
            {
                return params.maxAcceleration;
            }

        }
        else
        {
            //Currently this vehicle is neither yielding, nor nosing.

            params.lcDebugStr << ";NTH";

            return params.maxAcceleration;
        }
    }
    else
    {
        return params.maxAcceleration;
    }
}

double MITSIM_CF_Model::calcAccToCreateGap(DriverUpdateParams &params, NearestVehicle &nearestVeh, float gap)
{
    if (!nearestVeh.exists())
    {
        //No vehicle ahead, this constraint does not apply
        return params.maxAcceleration;
    }

    //freedom left
    float dx = nearestVeh.distance - gap;
    float dv = params.currSpeed - nearestVeh.driver->getFwdVelocity();
    float dt = params.nextStepSize;

    if (dt <= 0.0)
    {
        return params.maxAcceleration;
    }

    if (dx < 0.01 || dv < 0.0)
    {
        //insufficient gap or my speed is slower than the leader
        //front->accRate_ + 2.0 * (dx - dv * dt) / (dt * dt);
        double res = nearestVeh.driver->getFwdAcceleration() + 2.0 * (dx - dv * dt) / (dt * dt);
        return res;
    }
    else
    {
        //gap is ok and my speed is higher.
        //front->accRate_ - 0.5 * dv * dv / dx;
        double res = nearestVeh.driver->getFwdAcceleration() - 0.5 * dv * dv / dx;
        return res;
    }
}

double MITSIM_CF_Model::calcWaitForLaneExitAcc(DriverUpdateParams &params)
{
    double acceleration = params.maxAcceleration;

    if (!fwdDriverMovement->isInIntersection() && params.flag(FLAG_ESCAPE)
            || (params.flag(FLAG_NOSING) && !params.flag(FLAG_NOSING_FEASIBLE)))
    {
        double distance = params.distToStop;
        acceleration = calcBrakeToStopAcc(params, distance);
    }

    return acceleration;
}

double MITSIM_CF_Model::calcWaitForAllowedLaneAcc(DriverUpdateParams &params)
{
    if (params.distToStop < params.distanceToNormalStop && !params.isTargetLane)
    {
        if (params.flag(FLAG_NOSING))
        {
            return params.maxAcceleration;
        }

        return calcBrakeToStopAcc(params, params.distanceToNormalStop);
    }

    return params.maxAcceleration;
}

double MITSIM_CF_Model::calcDesiredSpeed(DriverUpdateParams &params)
{
    double speedOnSign = 0;
    double desiredSpeed = 0;

    if (params.speedLimit)
    {

        speedOnSign = params.speedLimit;
    }
    else
    {
        if(params.currLane)
        {
            speedOnSign = params.currLane->getParentSegment()->getMaxSpeed();
        }
        else
        {
            speedOnSign = params.maxLaneSpeed;
        }

    }

    float desired = speedFactor * speedOnSign;

    desired = desired * (1 + getSpeedLimitAddon());

    if(params.currLane)
    {
        desiredSpeed = std::min<double>(desired, params.currLane->getParentSegment()->getMaxSpeed());
    }
    else
    {
        desiredSpeed  = std::min<double>(desired, params.maxLaneSpeed);
    }

    return desiredSpeed;
}

double MITSIM_CF_Model::calcForwardGapAcc(DriverUpdateParams &params)
{
    //Adjacent lead vehicle
    const NearestVehicle *adjVehicle = NULL;

    if (params.getStatus(STATUS_LEFT))
    {
        adjVehicle = &params.nvLeftFwd;
    }
    else if (params.getStatus(STATUS_RIGHT))
    {
        adjVehicle = &params.nvRightFwd;
    }
    else
    {
        // No request for lane change
        return params.maxAcceleration;
    }

    const std::vector<double> &gapAcceptanceParams = targetGapAccParm;
    double distance = 0;
    double dv = 0;

    if (adjVehicle->exists())
    {
        distance = adjVehicle->distance + gapAcceptanceParams[0];
        Driver *adjDriver = const_cast<Driver*> (adjVehicle->driver);
        dv = adjDriver->getFwdVelocity() - params.driver->getFwdVelocity();
    }
    else
    {
        distance = adjVehicle->distance + gapAcceptanceParams[0];
        dv = 0 - params.driver->getFwdVelocity();
    }

    float acc = gapAcceptanceParams[1] * pow(distance, gapAcceptanceParams[2]);

    if (dv > 0)
    {
        acc *= pow(dv, gapAcceptanceParams[3]);
    }
    else if (dv < 0)
    {
        acc *= pow(-dv, gapAcceptanceParams[4]);
    }

    acc += getAccelerationAddon() * gapAcceptanceParams[5] / 0.824;
    return acc;
}

double MITSIM_CF_Model::calcBackwardGapAcc(DriverUpdateParams &params)
{
    //Rear vehicle
    const NearestVehicle *rearVehicle = NULL;

    if (params.getStatus(STATUS_LEFT))
    {
        rearVehicle = &params.nvLeftBack;
    }
    else if (params.getStatus(STATUS_RIGHT))
    {
        rearVehicle = &params.nvRightBack;
    }
    else
    {
        // No request for lane change
        return params.maxAcceleration;
    }

    const std::vector<double> &gapAcceptanceParams = targetGapAccParm;

    double distance = 0;
    double dv = 0;

    if (rearVehicle->exists())
    {
        Driver *rearDriver = const_cast<Driver*> (rearVehicle->driver);

        distance = rearVehicle->distance + gapAcceptanceParams[0];
        dv = rearDriver->getFwdVelocity() - params.driver->getFwdVelocity();
    }
    else
    {
        distance = rearVehicle->distance + gapAcceptanceParams[0];
        dv = 0 - params.driver->getFwdVelocity();
    }

    float acc = gapAcceptanceParams[6] * pow(distance, gapAcceptanceParams[7]);

    if (dv > 0)
    {
        acc *= pow(dv, gapAcceptanceParams[8]);
    }
    else if (dv < 0)
    {
        acc *= pow(-dv, gapAcceptanceParams[9]);
    }

    acc += getAccelerationAddon() * gapAcceptanceParams[10] / 0.824;
    return acc;
}

double MITSIM_CF_Model::calcAdjacentGapRate(DriverUpdateParams& p)
{
    //Adjacent vehicle
    const NearestVehicle *adjVehicle = NULL;

    //Vehicle behind the adjacent vehicle
    const NearestVehicle * adjRearVehicle = NULL;

    if (p.getStatus(STATUS_LEFT))
    {
        adjVehicle = &p.nvLeftFwd;
        adjRearVehicle = &p.nvLeftBack;
    }
    else if (p.getStatus(STATUS_RIGHT))
    {
        adjVehicle = &p.nvRightFwd;
        adjRearVehicle = &p.nvRightBack;
    }
    else
    {
        // No request for lane change
        return p.maxAcceleration;
    }

    const std::vector<double> &gapAcceptanceParams = targetGapAccParm;

    if (!adjVehicle->exists())
    {
        return p.maxAcceleration;
    }

    if (!adjRearVehicle->exists())
    {
        return p.maxAcceleration;
    }

    Driver *adjDriver = const_cast<Driver*> (adjVehicle->driver);
    Driver *adjRearDriver = const_cast<Driver*> (adjRearVehicle->driver);

    float gap = adjRearDriver->gapDistance(adjDriver);
    float position = adjRearDriver->gapDistance(p.driver) + p.driver->getVehicleLength();
    float acc = gapAcceptanceParams[11] * (gapAcceptanceParams[0] * gap - position);

    acc += getAccelerationAddon() * gapAcceptanceParams[12] / 0.824;
    p.lcDebugStr << "+++acc+++" << acc;

    return acc;
}

double MITSIM_CF_Model::calcAccForStoppingPoint(DriverUpdateParams &params)
{
    std::stringstream debugStr;
    //debugStr << ";SS-Dist" << params.distanceToStoppingPt << ";" << params.stopPointState << ";" << params.perceivedFwdVelocity;

    double acc = params.maxAcceleration;

    if (!params.getStatus(STATUS_CHANGING))
    {
        if (params.stopPointState == DriverUpdateParams::ARRIVING_AT_STOP_POINT)
        {
            acc = calcBrakeToStopAcc(params, params.distToStop);
    //      debugStr << "SP-Arriving;";
        }
        if (params.stopPointState == DriverUpdateParams::ARRIVED_AT_STOP_POINT || params.stopPointState == DriverUpdateParams::WAITING_AT_STOP_POINT)
        {
    //      debugStr << "SP-Arrived;";
            acc = params.maxDeceleration;
        }
    }

    if (params.stopPointState == DriverUpdateParams::ARRIVED_AT_STOP_POINT && params.perceivedFwdVelocity < 0.1)
    {
        debugStr << "SP-Arrive0;";
        acc = params.maxDeceleration;
        params.stopPointState = DriverUpdateParams::WAITING_AT_STOP_POINT;
        params.stopTimeTimer = params.now.ms();
    }

    if (params.stopPointState == DriverUpdateParams::WAITING_AT_STOP_POINT)
    {
        debugStr << "SP-Waiting;";
        double currentTime = params.now.ms();
        double waitTime = millisecondToSecond(currentTime - params.stopTimeTimer);

        debugStr << "SP-wt;" << waitTime;

        if (waitTime > params.currentStopPoint.dwellTime)
        {
            params.stopPointState = DriverUpdateParams::LEAVING_STOP_POINT;
        }
    }

    params.cfDebugStr += debugStr.str();
    return acc;
}

double MITSIM_CF_Model::calcBrakeToStopAcc(DriverUpdateParams &params, double distance)
{
    if (distance > Math::DOUBLE_EPSILON)
    {
        double u2 = (params.perceivedFwdVelocity) * (params.perceivedFwdVelocity);
        double acc = -u2 / distance * 0.5;

        if (acc <= params.normalDeceleration)
        {
            return acc;
        }

        double dt = params.nextStepSize;
        double vt = params.perceivedFwdVelocity * dt;
        double a = dt * dt;
        double b = 2.0 * vt - params.normalDeceleration * a;
        double c = u2 + 2.0 * params.normalDeceleration * (distance - vt);
        double d = b * b - 4.0 * a * c;

        if (d < 0 || a <= 0.0)
        {
            return acc;
        }

        return (sqrt(d) - b) / a * 0.5;
    }
    else
    {
        double dt = params.nextStepSize;
        return (dt > 0.0) ? -params.perceivedFwdVelocity / dt : params.maxDeceleration;
    }
}

double MITSIM_CF_Model::calcDesiredSpeedAcc(DriverUpdateParams &params)
{
    float maxspd = 0;

    if (params.currLane)
    {
        maxspd = params.currLane->getParentSegment()->getMaxSpeed();
    }
    else
    {
        maxspd = params.maxLaneSpeed;
    }

    double epsilon_v = Math::DOUBLE_EPSILON;

    if (params.perceivedFwdVelocity < maxspd - epsilon_v)
    {
        // Use maximum acceleration
        return params.maxAcceleration;
    }
    else if (params.perceivedFwdVelocity > maxspd + epsilon_v)
    {
        // Decelerate
        return params.normalDeceleration;
    }
    else
    {
        // Keep current speed.
        return 0.0;
    }
}

double MITSIM_CF_Model::calcTargetSpeedAcc(DriverUpdateParams &params, double distance, double velocity)
{
    double dt = params.nextStepSize;
    double currentSpeed = params.perceivedFwdVelocity;

    if (distance > Math::DOUBLE_EPSILON)
    {
        float v2 = velocity * velocity;
        float u2 = currentSpeed * currentSpeed;
        float acc = (v2 - u2) / distance * 0.5;

        return acc;
    }
    else
    {
        return (velocity - currentSpeed) / dt;
    }
}

double MITSIM_CF_Model::calcEmergencyDeceleration(DriverUpdateParams &params)
{
    double velocity = params.perceivedFwdVelocity;
    double dv = velocity - params.velocityLeadVehicle;
    double epsilon_v = Math::DOUBLE_EPSILON;

    if (velocity < epsilon_v)
    {
        return 0;
    }

    double aNormalDec = params.normalDeceleration;
    double a = 0;

    if (dv < epsilon_v)
    {
        a = params.accLeadVehicle + 0.25 * aNormalDec;
    }
    else if (params.gapBetnVehicles > 0.01)
    {
        a = params.accLeadVehicle - dv * dv / 2 / params.gapBetnVehicles;
    }
    else
    {
        double dt = params.nextStepSize;
        double s = params.spaceStar;
        double v = params.velocityLeadVehicle + params.accLeadVehicle * dt;
        a = calcTargetSpeedAcc(params, s, v);
    }

    return min(params.normalDeceleration, a);
}

double MITSIM_CF_Model::calcAccOfCarFollowing(DriverUpdateParams &params)
{
    double density = params.density;
    double velocity = params.perceivedFwdVelocity;
    int i = (velocity > params.velocityLeadVehicle) ? 1 : 0;

    double dv = (velocity > params.velocityLeadVehicle) ? (velocity - params.velocityLeadVehicle) : (params.velocityLeadVehicle - velocity);

    double res = CF_parameters[i].alpha * pow(velocity, CF_parameters[i].beta) / pow(params.nvFwd.distance, CF_parameters[i].gama);
    res *= pow(dv, CF_parameters[i].lambda) * pow(density, CF_parameters[i].rho);
    res += feet2Unit(Utils::nRandom(0, CF_parameters[i].stddev));

    return res;
}

double MITSIM_CF_Model::calcFreeFlowingAcc(DriverUpdateParams &params, double targetSpeed)
{
    double velocity = params.perceivedFwdVelocity;

    if (velocity < targetSpeed - minSpeed)
    {
        double acc = params.FFAccParamsBeta * (targetSpeed - velocity);
        return acc;
    }
    else if (velocity > targetSpeed + minSpeed)
    {
        return params.normalDeceleration;
    }

    return 0;
}

double MITSIM_CF_Model::accOfMixOfCFandFF(DriverUpdateParams &params, double targetSpeed)
{
    if (params.gapBetnVehicles > params.distanceToNormalStop)
    {
        return calcFreeFlowingAcc(params, targetSpeed);
    }
    else
    {
        double dt = params.nextStepSize;
        double s = params.spaceStar;
        double v = params.velocityLeadVehicle + params.accLeadVehicle * dt;
        return calcTargetSpeedAcc(params, s, v);
    }
}

void MITSIM_CF_Model::calcDistanceForNormalStop(DriverUpdateParams &params)
{
    if (params.perceivedFwdVelocity > minSpeed)
    {
        params.distanceToNormalStop = Math::DOUBLE_EPSILON - 0.5 * params.perceivedFwdVelocity * params.perceivedFwdVelocity / params.normalDeceleration;

        if (params.distanceToNormalStop < minResponseDistance)
        {
            params.distanceToNormalStop = minResponseDistance;
        }
    }
    else
    {
        params.distanceToNormalStop = minResponseDistance;
    }
}

void MITSIM_CF_Model::calcStateBasedVariables(DriverUpdateParams &params)
{
    calcDistanceForNormalStop(params);

    // Acceleration rate for a vehicle (a function of vehicle type,
    // facility type, segment grade, current speed).
    params.maxAcceleration = getMaxAcceleration(params);

    // Deceleration rate for a vehicle (a function of vehicle type, and
    // segment grade).
    params.normalDeceleration = getNormalDeceleration(params);

    // Maximum deceleration is function of speed and vehicle class
    params.maxDeceleration = getMaxDeceleration(params);

    // unsetStatus;
    params.unsetStatus(STATUS_REGIME);
}

void MITSIM_CF_Model::calcUpdateStepSizes()
{
    //Deceleration
    double totalReactionTime = sampleFromNormalDistribution(decUpdateStepSize);

    //Perception time  = reaction time * perception percentage
    double perceptionTime = totalReactionTime * decUpdateStepSize.perception;

    updateStepSize.push_back(totalReactionTime);
    perceptionSize.push_back(perceptionTime);

    //Acceleration
    totalReactionTime = sampleFromNormalDistribution(accUpdateStepSize);

    //Perception time  = reaction time * perception percentage
    perceptionTime = totalReactionTime * accUpdateStepSize.perception;

    updateStepSize.push_back(totalReactionTime);
    perceptionSize.push_back(perceptionTime);

    //Uniform Speed
    totalReactionTime = sampleFromNormalDistribution(uniformSpeedUpdateStepSize);

    //Perception time  = reaction time * perception percentage
    perceptionTime = totalReactionTime * uniformSpeedUpdateStepSize.perception;

    updateStepSize.push_back(totalReactionTime);
    perceptionSize.push_back(perceptionTime);

    //Stopped vehicle
    totalReactionTime = sampleFromNormalDistribution(stoppedUpdateStepSize);

    //Perception time  = reaction time * perception percentage
    perceptionTime = totalReactionTime * stoppedUpdateStepSize.perception;

    updateStepSize.push_back(totalReactionTime);
    perceptionSize.push_back(perceptionTime);
}

double MITSIM_CF_Model::sampleFromNormalDistribution(UpdateStepSizeParam &stepSizeParams)
{

    if (stepSizeParams.mean == 0)
    {
        return 0;
    }

    boost::lognormal_distribution<double> nor(stepSizeParams.mean, stepSizeParams.stdev);
    boost::variate_generator<boost::mt19937, boost::lognormal_distribution<double> > dice(updateSizeRNG, nor);
    double v = dice();

    if (v < stepSizeParams.lower)
    {
        return stepSizeParams.lower;
    }
    if (v > stepSizeParams.upper)
    {
        return stepSizeParams.upper;
    }

    return v;
}

double CarFollowingModel::calcNextStepSize(DriverUpdateParams &params)
{
    double currAcc = params.driver->getFwdAcceleration();
    double currentSpeed = params.driver->getFwdVelocity();

    int i = 0;
    if (currentSpeed < minSpeed)
    {
        i = 3;
    }
    else
    {
        if (currAcc < -Math::DOUBLE_EPSILON)
        {
            i = 0;
        }
        else if (currAcc > Math::DOUBLE_EPSILON)
        {
            i = 1;
        }
        else if (currentSpeed > Math::DOUBLE_EPSILON)
        {
            i = 2;
        }
        else
        {
            i = 3;
        }
    }

    params.nextStepSize = updateStepSize[i];

    if (params.nextStepSize == 0)
    {
        params.nextStepSize = params.elapsedSeconds;
    }

    nextPerceptionSize = perceptionSize[i];
    params.driver->resetReactionTime(nextPerceptionSize * 1000);

    return params.nextStepSize;
}


#include <iostream>
#include <string>
#include <sstream>
#include <vector>
#include <set>
#include <boost/algorithm/string.hpp>

const std::string input = "L5, R1, R4, L5, L4, R3, R1, L1, R4, R5, L1, L3, R4, L2, L4, R2, L4, L1, R3, R1, R1, L1, R1, L5, R5, R2, L5, R2, R1, L2, L4, L4, R191, R2, R5, R1, L1, L2, R5, L2, L3, R4, L1, L1, R1, R50, L1, R1, R76, R5, R4, R2, L5, L3, L5, R2, R1, L1, R2, L3, R4, R2, L1, L1, R4, L1, L1, R185, R1, L5, L4, L5, L3, R2, R3, R1, L5, R1, L3, L2, L2, R5, L1, L1, L3, R1, R4, L2, L1, L1, L3, L4, R5, L2, R3, R5, R1, L4, R5, L3, R3, R3, R1, R1, R5, R2, L2, R5, L5, L4, R4, R3, R5, R1, L3, R1, L2, L2, R3, R4, L1, R4, L1, R4, R3, L1, L4, L1, L5, L2, R2, L1, R1, L5, L3, R4, L1, R5, L5, L5, L1, L3, R1, R5, L2, L4, L5, L1, L1, L2, R5, R5, L4, R3, L2, L1, L3, L4, L5, L5, L2, R4, R3, L5, R4, R2, R1, L5";

using IntPair = std::pair<int, int>;

IntPair turn(char ch, IntPair dir)
{
    int angle_sine = 0;

    if (ch == 'L')
        angle_sine = 1;
    else
        angle_sine = -1;

    return std::make_pair(-dir.second * angle_sine, dir.first * angle_sine);
}

IntPair move(IntPair from, IntPair direction, int distance)
{
    return std::make_pair(
        from.first + distance * direction.first,
        from.second + distance * direction.second);
}

int calc_distance(IntPair pos)
{
    return std::abs(pos.first) + std::abs(pos.second);
}

std::string to_string(IntPair pair)
{
    std::stringstream stream;
    stream << '[' << pair.first << ',' << pair.second << ']';
    return stream.str();
}

int main(int argc, char *argv[])
{
    std::vector<std::string> instructions;
    boost::split(instructions, input, boost::is_any_of(", "), boost::token_compress_on);

    IntPair pos{ 0, 0 };
    IntPair direction{ 0, 1 };

    std::set<IntPair> visited;
    visited.insert(pos);
    int visited_twice = false;
    int distance_to_first_visited_twice = 0;

    for (const std::string &instruction : instructions)
    {
        int distance = atoi(instruction.substr(1).c_str());

        IntPair new_dir = turn(instruction[0], direction);

        IntPair new_pos = pos;
        for (int i = 0; i < distance; ++i)
        {
            new_pos = move(new_pos, new_dir, 1);

            if (!visited.insert(new_pos).second
                && !visited_twice)
            {
                distance_to_first_visited_twice = calc_distance(new_pos);
                visited_twice = true;
            }
        }

        //std::cout << instruction << " : "
        //    << to_string(pos) << " by " << to_string(direction) << " dis " << distance
        //    << " => " << to_string(new_pos) << " by " << to_string(new_dir)
        //    << std::endl;

        pos = new_pos;
        direction = new_dir;
    }

    std::cout << "Answer #1 : " << calc_distance(pos) << std::endl;
    std::cout << "Answer #2 : " << distance_to_first_visited_twice << std::endl;

    return 0;
}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2008-2015 Bruno Lalande, Paris, France.
// Copyright (c) 2008-2015 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2009-2015 Mateusz Loskot, London, UK.

// This file was modified by Oracle on 2014-2017.
// Modifications copyright (c) 2014-2017, Oracle and/or its affiliates.

// Contributed and/or modified by Vissarion Fysikopoulos, on behalf of Oracle
// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_STRATEGIES_SPHERICAL_DISTANCE_CROSS_TRACK_POINT_BOX_HPP
#define BOOST_GEOMETRY_STRATEGIES_SPHERICAL_DISTANCE_CROSS_TRACK_POINT_BOX_HPP

#include <boost/config.hpp>
#include <boost/concept_check.hpp>
#include <boost/mpl/if.hpp>
#include <boost/type_traits/is_void.hpp>

#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/assert.hpp>
#include <boost/geometry/core/point_type.hpp>
#include <boost/geometry/core/radian_access.hpp>
#include <boost/geometry/core/tags.hpp>

#include <boost/geometry/strategies/distance.hpp>
#include <boost/geometry/strategies/concepts/distance_concept.hpp>
#include <boost/geometry/strategies/spherical/distance_cross_track.hpp>

#include <boost/geometry/util/math.hpp>
#include <boost/geometry/algorithms/detail/assign_box_corners.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace distance
{

namespace details
{

template <typename ReturnType>
class cross_track_point_box_generic
{
public :

    template
    <
            typename Point,
            typename Box,
            typename Strategy
    >
    ReturnType static inline apply (Point const& point,
                                    Box const& box,
                                    Strategy ps_strategy)
    {
        // this method assumes that the coordinates of the point and
        // the box are normalized

        typedef typename point_type<Box>::type box_point_type;

        box_point_type bottom_left, bottom_right, top_left, top_right;
        geometry::detail::assign_box_corners(box,
                                             bottom_left, bottom_right,
                                             top_left, top_right);

        ReturnType const plon = geometry::get_as_radian<0>(point);
        ReturnType const plat = geometry::get_as_radian<1>(point);

        ReturnType const lon_min = geometry::get_as_radian<0>(bottom_left);
        ReturnType const lat_min = geometry::get_as_radian<1>(bottom_left);
        ReturnType const lon_max = geometry::get_as_radian<0>(top_right);
        ReturnType const lat_max = geometry::get_as_radian<1>(top_right);

        ReturnType const pi = math::pi<ReturnType>();
        ReturnType const two_pi = math::two_pi<ReturnType>();

        typedef typename point_type<Box>::type box_point_type;

        // First check if the point is within the band defined by the
        // minimum and maximum longitude of the box; if yes, determine
        // if the point is above, below or inside the box and compute
        // the distance (easy in this case)
        //
        // Notice that the point may not be inside the longitude range
        // of the box, but the shifted point may be inside the
        // longitude range of the box; in this case the point is still
        // considered as inside the longitude range band of the box
        if ((plon >= lon_min && plon <= lon_max) || plon + two_pi <= lon_max)
        {
            if (plat > lat_max)
            {
                return geometry::strategy::distance::services::result_from_distance
                        <
                            Strategy, Point, box_point_type
                        >::apply(ps_strategy, ps_strategy.get_distance_strategy().meridian(plat, lat_max));
            }
            else if (plat < lat_min)
            {
                return geometry::strategy::distance::services::result_from_distance
                        <
                            Strategy, Point, box_point_type
                        >::apply(ps_strategy, ps_strategy.get_distance_strategy().meridian(lat_min, plat));
            }
            else
            {
                BOOST_GEOMETRY_ASSERT(plat >= lat_min && plat <= lat_max);
                return ReturnType(0);
            }
        }

        // Otherwise determine which among the two medirian segments of the
        // box the point is closest to, and compute the distance of
        // the point to this closest segment

        // Below lon_midway is the longitude of the meridian that:
        // (1) is midway between the meridians of the left and right
        //     meridians of the box, and
        // (2) does not intersect the box
        ReturnType const two = 2.0;
        bool use_left_segment;
        if (lon_max > pi)
        {
            // the box crosses the antimeridian

            // midway longitude = lon_min - (lon_min + (lon_max - 2 * pi)) / 2;
            ReturnType const lon_midway = (lon_min - lon_max) / two + pi;
            BOOST_GEOMETRY_ASSERT(lon_midway >= -pi && lon_midway <= pi);

            use_left_segment = plon > lon_midway;
        }
        else
        {
            // the box does not cross the antimeridian

            ReturnType const lon_sum = lon_min + lon_max;
            if (math::equals(lon_sum, ReturnType(0)))
            {
                // special case: the box is symmetric with respect to
                // the prime meridian; the midway meridian is the antimeridian

                use_left_segment = plon < lon_min;
            }
            else
            {
                // midway long. = lon_min - (2 * pi - (lon_max - lon_min)) / 2;
                ReturnType lon_midway = (lon_min + lon_max) / two - pi;

                // normalize the midway longitude
                if (lon_midway > pi)
                {
                    lon_midway -= two_pi;
                }
                else if (lon_midway < -pi)
                {
                    lon_midway += two_pi;
                }
                BOOST_GEOMETRY_ASSERT(lon_midway >= -pi && lon_midway <= pi);

                // if lon_sum is positive the midway meridian is left
                // of the box, or right of the box otherwise
                use_left_segment = lon_sum > 0
                        ? (plon < lon_min && plon >= lon_midway)
                        : (plon <= lon_max || plon > lon_midway);
            }
        }

        return use_left_segment
                ? ps_strategy.apply(point, bottom_left, top_left)
                : ps_strategy.apply(point, bottom_right, top_right);
    }
};

}  //namespace details

/*!
\brief Strategy functor for distance point to box calculation
\ingroup strategies
\details Class which calculates the distance of a point to a box, for
points and boxes on a sphere or globe
\tparam CalculationType \tparam_calculation
\tparam Strategy underlying point-segment distance strategy, defaults
to cross track
\qbk{
[heading See also]
[link geometry.reference.algorithms.distance.distance_3_with_strategy distance (with strategy)]
}
*/
template
<
    typename CalculationType = void,
    typename Strategy = cross_track<CalculationType>
>
class cross_track_point_box
{
public:
    template <typename Point, typename Box>
    struct return_type
        : services::return_type<Strategy, Point, typename point_type<Box>::type>
    {};

    typedef typename Strategy::radius_type radius_type;

    inline cross_track_point_box()
    {}

    explicit inline cross_track_point_box(typename Strategy::radius_type const& r)
        : m_ps_strategy(r)
    {}

    inline cross_track_point_box(Strategy const& s)
        : m_ps_strategy(s)
    {}

    // It might be useful in the future
    // to overload constructor with strategy info.
    // crosstrack(...) {}

    template <typename Point, typename Box>
    inline typename return_type<Point, Box>::type
    apply(Point const& point, Box const& box) const
    {
#if !defined(BOOST_MSVC)
        BOOST_CONCEPT_ASSERT
            (
                (concepts::PointSegmentDistanceStrategy
                    <
                        Strategy, Point, typename point_type<Box>::type
                    >)
            );
#endif
        typedef typename return_type<Point, Box>::type return_type;
        return details::cross_track_point_box_generic
                               <return_type>::apply(point, box, m_ps_strategy);
    }

    inline typename Strategy::radius_type radius() const
    {
        return m_ps_strategy.radius();
    }

private:
    Strategy m_ps_strategy;
};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS
namespace services
{

template <typename CalculationType, typename Strategy>
struct tag<cross_track_point_box<CalculationType, Strategy> >
{
    typedef strategy_tag_distance_point_box type;
};

template <typename CalculationType, typename Strategy, typename P, typename Box>
struct return_type<cross_track_point_box<CalculationType, Strategy>, P, Box>
    : cross_track_point_box
        <
            CalculationType, Strategy
        >::template return_type<P, Box>
{};

template <typename CalculationType, typename Strategy>
struct comparable_type<cross_track_point_box<CalculationType, Strategy> >
{
    typedef cross_track_point_box
        <
            CalculationType, typename comparable_type<Strategy>::type
        > type;
};

template <typename CalculationType, typename Strategy>
struct get_comparable<cross_track_point_box<CalculationType, Strategy> >
{
    typedef cross_track_point_box<CalculationType, Strategy> this_strategy;
    typedef typename comparable_type<this_strategy>::type comparable_type;

public:
    static inline comparable_type apply(this_strategy const& strategy)
    {
        return comparable_type(strategy.radius());
    }
};

template <typename CalculationType, typename Strategy, typename P, typename Box>
struct result_from_distance
    <
        cross_track_point_box<CalculationType, Strategy>, P, Box
    >
{
private:
    typedef cross_track_point_box<CalculationType, Strategy> this_strategy;

    typedef typename this_strategy::template return_type
        <
            P, Box
        >::type return_type;

public:
    template <typename T>
    static inline return_type apply(this_strategy const& strategy,
                                    T const& distance)
    {
        Strategy s(strategy.radius());

        return result_from_distance
            <
                Strategy, P, typename point_type<Box>::type
            >::apply(s, distance);
    }
};

// define cross_track_point_box<default_point_segment_strategy> as
// default point-box strategy for the spherical equatorial coordinate system
template <typename Point, typename Box, typename Strategy>
struct default_strategy
    <
        point_tag, box_tag, Point, Box,
        spherical_equatorial_tag, spherical_equatorial_tag,
        Strategy
    >
{
    typedef cross_track_point_box
        <
            void,
            typename boost::mpl::if_
                <
                    boost::is_void<Strategy>,
                    typename default_strategy
                        <
                            point_tag, segment_tag,
                            Point, typename point_type<Box>::type,
                            spherical_equatorial_tag, spherical_equatorial_tag
                        >::type,
                    Strategy
                >::type
        > type;
};

template <typename Box, typename Point, typename Strategy>
struct default_strategy
    <
        box_tag, point_tag, Box, Point,
        spherical_equatorial_tag, spherical_equatorial_tag,
        Strategy
    >
{
    typedef typename default_strategy
        <
            point_tag, box_tag, Point, Box,
            spherical_equatorial_tag, spherical_equatorial_tag,
            Strategy
        >::type type;
};

} // namespace services
#endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

}} // namespace strategy::distance

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_SPHERICAL_DISTANCE_CROSS_TRACK_POINT_BOX_HPP


#ifndef BOOST_TEST_DYN_LINK
#define BOOST_TEST_DYN_LINK
#endif

#include <boost/test/unit_test.hpp>

#include <set>
#include <algorithm>

#include "../simulation/shapeGenerator.hpp"
#include "../simulation/point.hpp"

namespace ShapeGeneratorTest
{

const float g_epsilon = 0.0001;

bool FindPoint(int x, int y, std::set<Point>& set)
{
    return std::find_if(set.begin(), set.end(), [x, y](Point p)->bool
        { return (p.posX() == x && p.posY() == y); }) != set.end();
}

BOOST_AUTO_TEST_CASE(test_generateLineThin_shouldStayInBoundries)
{
    const Point start(2, 2), end(2, 15);
    const unsigned thickness = 0;

    auto output = ShapeGenerator::GenerateLine(start, end, thickness);

    BOOST_CHECK(FindPoint(2, 2, output));
    BOOST_CHECK(FindPoint(2, 10, output));
    BOOST_CHECK(FindPoint(2, 15, output));
    BOOST_CHECK(!FindPoint(1, 10, output));
    BOOST_CHECK(!FindPoint(3, 10, output));
    BOOST_CHECK(!FindPoint(2, 1, output));
    BOOST_CHECK(!FindPoint(2, 16, output));
}

BOOST_AUTO_TEST_CASE(test_invalidParams_shouldThrow)
{
    const Point point(1, 1);
    const int thickness = -1, radius = -1;
    BOOST_CHECK_THROW(ShapeGenerator::GenerateLine(point, point, thickness),
        std::exception);
    BOOST_CHECK_THROW(ShapeGenerator::GenerateCircle(point, radius),
        std::exception);
}

BOOST_AUTO_TEST_CASE(test_generateLine_shouldHaveCorrectEnds)
{
    const Point start(2, 2), end(5, 15);
    const unsigned thickness = 0;

    auto output = ShapeGenerator::GenerateLine(start, end, thickness);

    BOOST_CHECK(FindPoint(2, 2, output));
    BOOST_CHECK(FindPoint(5, 15, output));
}

BOOST_AUTO_TEST_CASE(test_generateLine_shouldStayInBoundries)
{
    const Point start(2, 2), end(2, 15);
    const unsigned thickness = 3;

    auto output = ShapeGenerator::GenerateLine(start, end, thickness);

    BOOST_CHECK(FindPoint(2, 2, output));
    BOOST_CHECK(FindPoint(2, 10, output));
    BOOST_CHECK(FindPoint(2, 15, output));
    BOOST_CHECK(FindPoint(3, 10, output));
    BOOST_CHECK(!FindPoint(4, 10, output));
    BOOST_CHECK(FindPoint(2, 16, output));
    BOOST_CHECK(!FindPoint(2, 17, output));
}

BOOST_AUTO_TEST_CASE(test_lineReverseStartEnd_shouldBeSimilarToNotReversed)
{
    const Point start(2, 2), end(5, 15);
    const unsigned thickness = 1;

    auto output = ShapeGenerator::GenerateLine(start, end, thickness);
    auto output_reversed = ShapeGenerator::GenerateLine(end, start, thickness);

    BOOST_CHECK(FindPoint(2, 2, output));
    BOOST_CHECK(FindPoint(5, 15, output));
    BOOST_CHECK(FindPoint(2, 2, output_reversed));
    BOOST_CHECK(FindPoint(5, 15, output_reversed));
    BOOST_CHECK(output.size() == output_reversed.size());
}

BOOST_AUTO_TEST_CASE(test_generateOneSquare_shouldBeOnlyOneSquare)
{
    const Point centre(5, 5);
    const unsigned radius = 0;

    auto output = ShapeGenerator::GenerateCircle(centre, radius);

    BOOST_CHECK(FindPoint(5, 5, output));
    BOOST_CHECK(!FindPoint(4, 5, output));
    BOOST_CHECK(!FindPoint(3, 5, output));
    BOOST_CHECK(!FindPoint(2, 5, output));
    BOOST_CHECK(!FindPoint(1, 5, output));
}

BOOST_AUTO_TEST_CASE(test_generateCircle_shouldBeCorrectRadius)
{
    const Point centre(5, 5);
    const unsigned radius = 2;

    auto output = ShapeGenerator::GenerateCircle(centre, radius);

    BOOST_CHECK(FindPoint(5, 5, output));
    BOOST_CHECK(FindPoint(4, 5, output));
    BOOST_CHECK(FindPoint(3, 5, output));
    BOOST_CHECK(!FindPoint(2, 5, output));
    BOOST_CHECK(!FindPoint(1, 5, output));
}

} // namespace ShapeGeneratorTest


// test_compare.cc
#include "menon/utility.hh"
#include <boost/core/lightweight_test.hpp>

// lt関数のテストコード
void test_lt()
{
  BOOST_TEST(menon::lt(1, 2));
  BOOST_TEST(!menon::lt(2, 1));
  BOOST_TEST(!menon::lt(2, 2));
  BOOST_TEST(menon::lt(1L, 2));
  BOOST_TEST(!menon::lt(2, 1L));
  BOOST_TEST(!menon::lt(2, 2LL));
  BOOST_TEST(menon::lt(-2, 1));
  BOOST_TEST(menon::lt(-2, 1u));
  BOOST_TEST(menon::lt(123.45, 345.67));
  BOOST_TEST(!menon::lt(123.45, 123.45));
}

// gt関数のテストコード
void test_gt()
{
  BOOST_TEST(!menon::gt(1, 2));
  BOOST_TEST(menon::gt(2, 1));
  BOOST_TEST(!menon::gt(2, 2));
  BOOST_TEST(!menon::gt(1L, 2));
  BOOST_TEST(menon::gt(2, 1L));
  BOOST_TEST(!menon::gt(2, 2LL));
  BOOST_TEST(menon::gt(2, -1));
  BOOST_TEST(!menon::gt(-2, 1));
  BOOST_TEST(!menon::gt(-2, 1u));
  BOOST_TEST(!menon::gt(123.45, 345.67));
  BOOST_TEST(!menon::gt(123.45, 123.45));
}

// le関数のテストコード
void test_le()
{
  BOOST_TEST(menon::le(1, 2));
  BOOST_TEST(!menon::le(2, 1));
  BOOST_TEST(menon::le(2, 2));
  BOOST_TEST(menon::le(1L, 2));
  BOOST_TEST(!menon::le(2, 1L));
  BOOST_TEST(menon::le(2, 2LL));
  BOOST_TEST(menon::le(-2, 1));
  BOOST_TEST(menon::le(-2, 1u));
  BOOST_TEST(menon::le(123.45, 345.67));
  BOOST_TEST(menon::le(123.45, 123.45));
}

// ge関数のテストコード
void test_ge()
{
  BOOST_TEST(!menon::ge(1, 2));
  BOOST_TEST(menon::ge(2, 1));
  BOOST_TEST(menon::ge(2, 2));
  BOOST_TEST(!menon::ge(1L, 2));
  BOOST_TEST(menon::ge(2, 1L));
  BOOST_TEST(menon::ge(2, 2LL));
  BOOST_TEST(!menon::ge(-2, 1));
  BOOST_TEST(!menon::ge(-2, 1u));
  BOOST_TEST(!menon::ge(123.45, 345.67));
  BOOST_TEST(menon::ge(123.45, 123.45));
}

// eq関数のテストコード
void test_eq()
{
  BOOST_TEST(!menon::eq(1, 2));
  BOOST_TEST(!menon::eq(2, 1));
  BOOST_TEST(menon::eq(2, 2));
  BOOST_TEST(!menon::eq(1L, 2));
  BOOST_TEST(!menon::eq(2, 1L));
  BOOST_TEST(menon::eq(2, 2LL));
  BOOST_TEST(!menon::eq(-2, 1));
  BOOST_TEST(!menon::eq(-2, 1u));
  BOOST_TEST(!menon::eq(123.45, 345.67));
  BOOST_TEST(menon::eq(123.45, 123.45));
}

// ne関数のテストコード
void test_ne()
{
  BOOST_TEST(menon::ne(1, 2));
  BOOST_TEST(menon::ne(2, 1));
  BOOST_TEST(!menon::ne(2, 2));
  BOOST_TEST(menon::ne(1L, 2));
  BOOST_TEST(menon::ne(2, 1L));
  BOOST_TEST(!menon::ne(2, 2LL));
  BOOST_TEST(menon::ne(-2, 1));
  BOOST_TEST(menon::ne(-2, 1u));
  BOOST_TEST(menon::ne(-1, ~0u));
  BOOST_TEST(menon::ne(123.45, 345.67));
  BOOST_TEST(!menon::ne(123.45, 123.45));
}

int main()
{
  test_lt();
  test_gt();
  test_le();
  test_ge();
  test_eq();
  test_ne();
  return boost::report_errors();
}


#include "get_fitzgibbon_cvpr_2001.hpp"
#include "get_kukelova_cvpr_2015.hpp"
#include "get_valtonenornhag_arxiv_2020a.hpp"
#include "get_valtonenornhag_arxiv_2020b.hpp"
#include <pybind11/pybind11.h>
#include <pybind11/stl.h>
#include <pybind11/numpy.h>
#include <pybind11/eigen.h>
#include <Eigen/Dense>
#include <stdexcept>
#include <vector>

namespace py = pybind11;

py::dict get_fitzgibbon_cvpr_2001_wrapper(const Eigen::Ref<const Eigen::MatrixXd> &x1,
                                          const Eigen::Ref<const Eigen::MatrixXd> &x2)
{

    if (x1.rows() != 2 || x1.cols() != 5) {
        throw std::invalid_argument("First argument should be of size 2x5");
    }
    if (x2.rows() != 2 || x2.cols() != 5) {
        throw std::invalid_argument("Second argument should be of size 2x5");
    }

    HomLib::PoseData posedata = HomLib::FitzgibbonCVPR2001::get(x1, x2);

    py::dict d;
    d["H"] = posedata.homography;
    d["lam"] = posedata.distortion_parameter;

    return d;
}

py::list get_kukelova_cvpr_2015_wrapper(const Eigen::Ref<const Eigen::MatrixXd> &x1,
                                        const Eigen::Ref<const Eigen::MatrixXd> &x2)
{

    if (x1.rows() != 2 || x1.cols() != 5) {
        throw std::invalid_argument("First argument should be of size 2x5");
    }
    if (x2.rows() != 2 || x2.cols() != 5) {
        throw std::invalid_argument("Second argument should be of size 2x5");
    }

    std::vector<HomLib::PoseData> posedata = HomLib::KukelovaCVPR2015::get(x1, x2);

    py::list lst;
    for (int i = 0; i < posedata.size(); i++) {
        py::dict d;
        d["H"] = posedata[i].homography;
        d["lam1"] = posedata[i].distortion_parameter;
        d["lam2"] = posedata[i].distortion_parameter2;
        lst.append(d);
    }

    return lst;
}

py::dict get_valtonenornhag_arxiv_2020a_fHf_wrapper(const Eigen::Ref<const Eigen::MatrixXd> &x1,
                                                    const Eigen::Ref<const Eigen::MatrixXd> &x2,
                                                    const Eigen::Ref<const Eigen::MatrixXd> &R1,
                                                    const Eigen::Ref<const Eigen::MatrixXd> &R2)
{

    if (x1.rows() != 2 || x1.cols() != 3) {
        throw std::invalid_argument("First argument should be of size 2x3");
    }
    if (x2.rows() != 2 || x2.cols() != 3) {
        throw std::invalid_argument("Second argument should be of size 2x3");
    }
    if (R1.rows() != 3 || R1.cols() != 3) {
        throw std::invalid_argument("Third argument should be of size 3x3");
    }
    if (R2.rows() != 3 || R2.cols() != 3) {
        throw std::invalid_argument("Fourth argument should be of size 3x3");
    }

    HomLib::PoseData posedata = HomLib::ValtonenOrnhagArxiv2020A::get_fHf(x1, x2, R1, R2);

    py::dict d;
    d["H"] = posedata.homography;
    d["f"] = posedata.focal_length;

    return d;
}

py::list get_valtonenornhag_arxiv_2020b_fHf_wrapper(const Eigen::Ref<const Eigen::MatrixXd> &x1,
                                                    const Eigen::Ref<const Eigen::MatrixXd> &x2,
                                                    const Eigen::Ref<const Eigen::MatrixXd> &R1,
                                                    const Eigen::Ref<const Eigen::MatrixXd> &R2)
{

    if (x1.rows() != 2 || x1.cols() != 2) {
        throw std::invalid_argument("First argument should be of size 2x2");
    }
    if (x2.rows() != 2 || x2.cols() != 2) {
        throw std::invalid_argument("Second argument should be of size 2x2");
    }
    if (R1.rows() != 3 || R1.cols() != 3) {
        throw std::invalid_argument("Third argument should be of size 3x3");
    }
    if (R2.rows() != 3 || R2.cols() != 3) {
        throw std::invalid_argument("Fourth argument should be of size 3x3");
    }

    std::vector<HomLib::PoseData> posedata = HomLib::ValtonenOrnhagArxiv2020B::get_fHf(x1, x2, R1, R2);

    py::list lst;
    for (int i = 0; i < posedata.size(); i++) {
        py::dict d;
        d["H"] = posedata[i].homography;
        d["f"] = posedata[i].focal_length;
        lst.append(d);
    }

    return lst;
}

py::dict get_valtonenornhag_arxiv_2020b_frHfr_wrapper(const Eigen::Ref<const Eigen::MatrixXd> &x1,
                                                      const Eigen::Ref<const Eigen::MatrixXd> &x2,
                                                      const Eigen::Ref<const Eigen::MatrixXd> &R1,
                                                      const Eigen::Ref<const Eigen::MatrixXd> &R2)
{

    if (x1.rows() != 2 || x1.cols() != 3) {
        throw std::invalid_argument("First argument should be of size 2x3");
    }
    if (x2.rows() != 2 || x2.cols() != 3) {
        throw std::invalid_argument("Second argument should be of size 2x3");
    }
    if (R1.rows() != 3 || R1.cols() != 3) {
        throw std::invalid_argument("Third argument should be of size 3x3");
    }
    if (R2.rows() != 3 || R2.cols() != 3) {
        throw std::invalid_argument("Fourth argument should be of size 3x3");
    }

    HomLib::PoseData posedata = HomLib::ValtonenOrnhagArxiv2020B::get_frHfr(x1, x2, R1, R2);

    py::dict d;
    d["H"] = posedata.homography;
    d["f"] = posedata.focal_length;
    d["lam"] = posedata.distortion_parameter;

    return d;
}

PYBIND11_MODULE(homlib, m) {
  m.doc() = R"doc(
        Python module
        -----------------------
        .. currentmodule:: homlib
        .. autosummary::
           :toctree: _generate

           get_fitzgibbon_cvpr_2001
           get_kukelova_cvpr_2015
           get_valtonenornhag_arxiv_2020a_fHf
           get_valtonenornhag_arxiv_2020b_fHf
           get_valtonenornhag_arxiv_2020b_frHfr
    )doc";

  m.def("get_fitzgibbon_cvpr_2001", &get_fitzgibbon_cvpr_2001_wrapper, R"doc(
        Fitzgibbon (CVPR, 2001) 5-point radial distortion homography.

        More info to be added...
    )doc");

  m.def("get_kukelova_cvpr_2015", &get_kukelova_cvpr_2015_wrapper, R"doc(
        Kukelova et al. (CVPR, 2015) 5-point radial distortion homography.

        More info to be added...
    )doc");

  m.def("get_valtonenornhag_arxiv_2020a_fHf", &get_valtonenornhag_arxiv_2020a_fHf_wrapper, R"doc(
        Valtonen Ornhag et al. (ArXiV, 2020) 2.5-point homography using IMU data.

        More info to be added...
    )doc");

  m.def("get_valtonenornhag_arxiv_2020b_fHf", &get_valtonenornhag_arxiv_2020b_fHf_wrapper, R"doc(
        Valtonen Ornhag et al. (ArXiV, 2020) 2-point homography using IMU data.

        More info to be added...
    )doc");

  m.def("get_valtonenornhag_arxiv_2020b_frHfr", &get_valtonenornhag_arxiv_2020b_frHfr_wrapper, R"doc(
        Valtonen Ornhag et al. (ArXiV, 2020) 2.5-point radial distortion homography using IMU data.

        More info to be added...
    )doc");
}


#include <boost/mp11/mpl.hpp>
#include <string>
#include <tuple>
#include <type_traits>

template <int I>
using int_ = std::integral_constant<int, I>;

int main()
{
    using v1 = boost::mp11::mp_list<int*, double, std::string>;
    using r1_index = boost::mp11::mp_find<v1, std::string>;
    using v1_size = boost::mp11::mp_size<v1>;
    using in_v1 = boost::mp11::mp_less<r1_index, v1_size>;
    constexpr int r1{ boost::mp11::mp_if<in_v1, r1_index, int_<-1>>::value };
    static_assert(r1 == 2);

    using v2 = std::tuple<int*, double, bool>;
    using r2_index = boost::mp11::mp_find<v2, std::string>;
    using v2_size = boost::mp11::mp_size<v2>;
    using in_v2 = boost::mp11::mp_less<r2_index, v2_size>;
    constexpr int r2{ boost::mp11::mp_if<in_v2, r2_index, int_<-1>>::value };
    static_assert(r2 == -1);
}


//==================================================================================================
/*!

  Copyright 2016 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)
*/
//==================================================================================================
#include <boost/simd/function/scalar/sin.hpp>
#include <boost/simd/function/restricted.hpp>
#include <scalar_test.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/mzero.hpp>
#include <boost/simd/constant/pio_2.hpp>
#include <boost/simd/constant/pio_4.hpp>
#include <boost/simd/constant/sqrt_2o_2.hpp>
#include <boost/simd/function/is_negative.hpp>
#include <boost/simd/function/is_positive.hpp>

STF_CASE_TPL (" sin",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::sin;

  using r_t = decltype(sin(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, T);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  STF_ULP_EQUAL(sin(bs::Inf<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(sin(bs::Minf<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(sin(bs::Nan<T>()), bs::Nan<r_t>(), 0.5);
#endif
  STF_ULP_EQUAL(sin(-bs::Pio_2<T>()), bs::Mone<r_t>(), 0.5);
  STF_ULP_EQUAL(sin(-bs::Pio_4<T>()), -bs::Sqrt_2o_2<r_t>(), 0.5);
  STF_ULP_EQUAL(sin(bs::Pio_2<T>()), bs::One<r_t>(), 0.5);
  STF_ULP_EQUAL(sin(bs::Pio_4<T>()), bs::Sqrt_2o_2<r_t>(), 0.5);
  STF_ULP_EQUAL(sin(bs::Zero<T>()), bs::Zero<r_t>(), 0.5);
  STF_EXPECT(bs::is_negative(sin(bs::Mzero<T>())));
  STF_EXPECT(bs::is_positive(sin(bs::Zero<T>())));
}

STF_CASE_TPL (" sin restricted ",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::sin;
  using r_t = decltype(sin(T()));

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  STF_ULP_EQUAL(bs::restricted_(sin)(bs::Inf<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(bs::restricted_(sin)(bs::Minf<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(bs::restricted_(sin)(bs::Nan<T>()), bs::Nan<r_t>(), 0.5);
#endif
  STF_ULP_EQUAL(bs::restricted_(sin)(-bs::Pio_2<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(bs::restricted_(sin)(-bs::Pio_4<T>()), -bs::Sqrt_2o_2<r_t>(), 0.5);
  STF_ULP_EQUAL(bs::restricted_(sin)(bs::Pio_2<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(bs::restricted_(sin)(bs::Pio_4<T>()), bs::Sqrt_2o_2<r_t>(), 0.5);
  STF_ULP_EQUAL(bs::restricted_(sin)(bs::Zero<T>()), bs::Zero<r_t>(), 0.5);
  STF_EXPECT(bs::is_negative(bs::restricted_(sin)(bs::Mzero<T>())));
  STF_EXPECT(bs::is_positive(bs::restricted_(sin)(bs::Zero<T>())));
}

STF_CASE_TPL (" sin",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bst = bs::tag;
  namespace bd = boost::dispatch;
  using bs::sin;

  using r_t = decltype(sin(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, T);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  STF_ULP_EQUAL(sin(bs::Inf<T>(), bst::clipped_small_), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(sin(bs::Minf<T>(), bst::clipped_small_), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(sin(bs::Nan<T>(), bst::clipped_small_), bs::Nan<r_t>(), 0.5);
#endif
  STF_ULP_EQUAL(sin(-bs::Pio_2<T>(), bst::clipped_small_), bs::Mone<r_t>(), 0.5);
  STF_ULP_EQUAL(sin(-bs::Pio_4<T>(), bst::clipped_small_), -bs::Sqrt_2o_2<r_t>(), 0.5);
  STF_ULP_EQUAL(sin(bs::Pio_2<T>(), bst::clipped_small_), bs::One<r_t>(), 0.5);
  STF_ULP_EQUAL(sin(bs::Pio_4<T>(), bst::clipped_small_), bs::Sqrt_2o_2<r_t>(), 0.5);
  STF_ULP_EQUAL(sin(bs::Zero<T>(), bst::clipped_small_), bs::Zero<r_t>(), 0.5);
  STF_EXPECT(bs::is_negative(sin(bs::Mzero<T>(), bst::clipped_small_)));
  STF_EXPECT(bs::is_positive(sin(bs::Zero<T>(), bst::clipped_small_)));
}
STF_CASE_TPL (" sin",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bst = bs::tag;
  namespace bd = boost::dispatch;
  using bs::sin;

  using r_t = decltype(sin(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, T);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  STF_ULP_EQUAL(sin(bs::Inf<T>(), bst::clipped_medium_), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(sin(bs::Minf<T>(), bst::clipped_medium_), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(sin(bs::Nan<T>(), bst::clipped_medium_), bs::Nan<r_t>(), 0.5);
#endif
  STF_ULP_EQUAL(sin(-bs::Pio_2<T>(), bst::clipped_medium_), bs::Mone<r_t>(), 0.5);
  STF_ULP_EQUAL(sin(-bs::Pio_4<T>(), bst::clipped_medium_), -bs::Sqrt_2o_2<r_t>(), 0.5);
  STF_ULP_EQUAL(sin(bs::Pio_2<T>(), bst::clipped_medium_), bs::One<r_t>(), 0.5);
  STF_ULP_EQUAL(sin(bs::Pio_4<T>(), bst::clipped_medium_), bs::Sqrt_2o_2<r_t>(), 0.5);
  STF_ULP_EQUAL(sin(bs::Zero<T>(), bst::clipped_medium_), bs::Zero<r_t>(), 0.5);
  STF_EXPECT(bs::is_negative(sin(bs::Mzero<T>(), bst::clipped_medium_)));
  STF_EXPECT(bs::is_positive(sin(bs::Zero<T>(), bst::clipped_medium_)));
}