#include <boost/format.hpp>
#include "RangeMap.hxx"
#include <string>
#include <iostream>

int main()
{
  SoDa::Range<double> k1(1.0, 2.0);
  SoDa::RangeMap<double, std::string> rmap;

  rmap[k1] = std::string("One-to-two");

  rmap[SoDa::Range<double>(2.0, 3.0)] = std::string("two-to-three");
  rmap[SoDa::Range<double>(3.0, 4.0)] = std::string("three-to-four");

  std::cerr << rmap[2.99999] << std::endl;
}


//
// Copyright (c) 2002--2010
// Toon Knapen, Karl Meerbergen, Kresimir Fresl,
// Thomas Klimpel and Rutger ter Borg
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
// THIS FILE IS AUTOMATICALLY GENERATED
// PLEASE DO NOT EDIT!
//

#ifndef BOOST_NUMERIC_BINDINGS_LAPACK_COMPUTATIONAL_STERF_HPP
#define BOOST_NUMERIC_BINDINGS_LAPACK_COMPUTATIONAL_STERF_HPP

#include <boost/assert.hpp>
#include <boost/numeric/bindings/begin.hpp>
#include <boost/numeric/bindings/is_mutable.hpp>
#include <boost/numeric/bindings/remove_imaginary.hpp>
#include <boost/numeric/bindings/size.hpp>
#include <boost/numeric/bindings/stride.hpp>
#include <boost/numeric/bindings/value_type.hpp>
#include <boost/static_assert.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/type_traits/remove_const.hpp>

//
// The LAPACK-backend for sterf is the netlib-compatible backend.
//
#include <boost/numeric/bindings/lapack/detail/lapack.h>
#include <boost/numeric/bindings/lapack/detail/lapack_option.hpp>

namespace boost {
namespace numeric {
namespace bindings {
namespace lapack {

//
// The detail namespace contains value-type-overloaded functions that
// dispatch to the appropriate back-end LAPACK-routine.
//
namespace detail {

//
// Overloaded function for dispatching to
// * netlib-compatible LAPACK backend (the default), and
// * float value-type.
//
inline std::ptrdiff_t sterf( const fortran_int_t n, float* d, float* e ) {
    fortran_int_t info(0);
    LAPACK_SSTERF( &n, d, e, &info );
    return info;
}

//
// Overloaded function for dispatching to
// * netlib-compatible LAPACK backend (the default), and
// * double value-type.
//
inline std::ptrdiff_t sterf( const fortran_int_t n, double* d, double* e ) {
    fortran_int_t info(0);
    LAPACK_DSTERF( &n, d, e, &info );
    return info;
}

} // namespace detail

//
// Value-type based template class. Use this class if you need a type
// for dispatching to sterf.
//
template< typename Value >
struct sterf_impl {

    typedef Value value_type;
    typedef typename remove_imaginary< Value >::type real_type;

    //
    // Static member function, that
    // * Deduces the required arguments for dispatching to LAPACK, and
    // * Asserts that most arguments make sense.
    //
    template< typename VectorD, typename VectorE >
    static std::ptrdiff_t invoke( const fortran_int_t n, VectorD& d,
            VectorE& e ) {
        namespace bindings = ::boost::numeric::bindings;
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< VectorD >::type >::type,
                typename remove_const< typename bindings::value_type<
                VectorE >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< VectorD >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< VectorE >::value) );
        BOOST_ASSERT( bindings::size(e) >= n-1 );
        BOOST_ASSERT( n >= 0 );
        return detail::sterf( n, bindings::begin_value(d),
                bindings::begin_value(e) );
    }

};

//
// Functions for direct use. These functions are overloaded for temporaries,
// so that wrapped types can still be passed and used for write-access. In
// addition, if applicable, they are overloaded for user-defined workspaces.
// Calls to these functions are passed to the sterf_impl classes. In the
// documentation, most overloads are collapsed to avoid a large number of
// prototypes which are very similar.
//

//
// Overloaded function for sterf. Its overload differs for
//
template< typename VectorD, typename VectorE >
inline std::ptrdiff_t sterf( const fortran_int_t n, VectorD& d,
        VectorE& e ) {
    return sterf_impl< typename bindings::value_type<
            VectorD >::type >::invoke( n, d, e );
}

} // namespace lapack
} // namespace bindings
} // namespace numeric
} // namespace boost

#endif


#define NOMINMAX

#include <opencv2/text.hpp>
#include <opencv2/core/utility.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/imgproc.hpp>
#include <tesseract/baseapi.h>
#include <leptonica/allheaders.h>
#include <boost/format.hpp>
#include <boost/algorithm/string.hpp>
#include <iostream>
#include <fstream>
#include <vector>
#include <math.h>
#include <algorithm>
#include <array>
#include <random>
#include <vector>
#include <numeric>

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

struct TelemetryPoint {
    unsigned int velocity;
    unsigned int altitude;

    TelemetryPoint(unsigned int velocity, unsigned int altitude)
        : velocity(velocity), altitude(altitude)
    {}
};

struct Telemetry {
    unsigned int fps;
    unsigned long frames;
    std::vector<TelemetryPoint> data;

    Telemetry(unsigned int fps, unsigned long frames)
        : fps(fps), frames(frames)
    {
        data.reserve(frames);
    }

    void write_data(std::ostream& out) {
        for (size_t x = 0; x < data.size(); x++) {
            const TelemetryPoint& point = data[x];
            out << x/(double)fps << "," << point.velocity << "," << point.altitude << endl;
        }
    }
};

void onMouse(int event, int x, int y, int, void* userdata)
{
    if (event != CV_EVENT_LBUTTONDOWN)
        return;

    cv::Mat* frame = static_cast<cv::Mat*>(userdata);

    cout << "(" << x << ", " << y << "): " << (int)frame->at<uchar>(y, x) << endl;
}

int count_digits(int n) {
    int digits = 1;
    for (int x = 10; ; x *= 10) {
        if (n / x > 0) {
            digits++;
        } else {
            break;
        }
    }
    return digits;
}

class Trainer {
private:
    std::array<int, 10> counts;
    std::vector<cv::Mat> training_data;
    int width, height, count, total_count;

public:
    Trainer(int width, int height, int count)
        : width(width), height(height), count(count), total_count(count*10)
    {
        std::fill(counts.begin(), counts.end(), 0);
        counts[5] -= 3;
        counts[6] -= 3;
    }

    void seen_digit(int digit, const cv::Mat& mat)
    {
        if (counts[digit] < count) {
            cv::Mat copy;
            mat.copyTo(copy);

            training_data.push_back(std::move(copy));
            counts[digit]++;
        }
    }

    bool is_ready() const {
        return training_data.size() == total_count;
    }

    cv::Mat generate_training_image() const {

        int vertical_padding = 10;
        int horizontal_padding = 0;

        cv::Mat training_mat(cv::Size(width + (horizontal_padding*2), (height + vertical_padding) * total_count), CV_8UC1);
        training_mat.setTo(cv::Scalar(0));

        std::vector<int> order(training_data.size());
        std::iota(order.begin(), order.end(), 0);
        std::random_shuffle(order.begin(), order.end());

        cv::Rect next_place(horizontal_padding, vertical_padding, width, height);
        for (size_t x = 0; x < order.size(); x++) {
            const cv::Mat& mat = training_data[order[x]];
            mat.copyTo(training_mat(next_place));

            next_place.y += height + vertical_padding;
        }

        return training_mat;
    }
};

struct MinimumInt {
    int x;

    MinimumInt(int x)
        : x(x)
    {}

    bool operator==(const MinimumInt& other) {
        return other.x >= x;
    }
};

class SpaceXOCR {
private:
    tesseract::TessBaseAPI tess;
    cv::VideoCapture cap;
    cv::Mat frame;
    cv::Rect velocity_rect;
    cv::Rect altitude_rect;
    cv::Mat velocity_mat, altitude_mat;
    Trainer trainer;

    double fps;
    unsigned long frame_count;
    unsigned int width, height;

    void configure_for_velocity() {
        tess.SetVariable("tessedit_char_whitelist", "0123456789");
    }

    void configure_for_altitude() {
       tess.SetVariable("tessedit_char_whitelist", "0123456789.");
    }

    template <typename T> T as_number(const char* str);

    template <>
    int as_number(const char* str) {
        return atoi(str);
    }

    template <>
    double as_number(const char* str) {
        return atof(str);
    }

    template <typename T>
    T recognize(const cv::Mat& frame, const cv::Rect& roi, cv::Mat& mat)
    {
        resize(frame(roi), mat, roi.size() * 2, 0.0, 0.0, cv::INTER_CUBIC);
        cv::cvtColor(mat, mat, CV_BGR2GRAY);

        // accuracy is very dependent on this threshold value
        cv::threshold(mat, mat, 130, 255, cv::THRESH_BINARY);

        tess.SetImage((uchar*)mat.data, mat.size().width, mat.size().height, mat.channels(), mat.step1());
        tess.Recognize(0);

        return as_number<T>(tess.GetUTF8Text());
    }

    bool get_velocity(const cv::Rect& roi, cv::Mat& mat, char* result)
    {
        resize(frame(roi), mat, roi.size() * 2, 0.0, 0.0, cv::INTER_CUBIC);
        cv::cvtColor(mat, mat, CV_BGR2GRAY);

        // accuracy is very dependent on this threshold value
        cv::threshold(mat, mat, 130, 255, cv::THRESH_BINARY);

        tess.SetImage((uchar*)mat.data, mat.size().width, mat.size().height, mat.channels(), mat.step1());

        int boxes[] = { 7, 33, 60, 89, 114 };

        for (int i = 0; i < 5; i++) {
            int x = boxes[i] - 1;
            int y = 3;
            int w = 23 + 1;
            int h = 32;

            tess.SetRectangle(x, y, w, h);
            char c = tess.GetUTF8Text()[0];

            if (c < '0' || c > '9') {
                return false;
            }
            result[i] = c;
            int int_value = c - '0';

            //int conf = tess.MeanTextConf();
            //fprintf(stdout, "Box[%d]: x=%d, y=%d, w=%d, h=%d, confidence: %d, text: %d\n",
            //        i, x, y, w, h, conf, int_value);
        }

        result[5] = 0;
        return true;
        //for (int i = 0; i < 5; i++) {
        //    cv::line(mat, cv::Point(boxes[i]-1, 0), cv::Point(boxes[i]-1, 36), cv::Scalar(255, 255, 255));
        //}
    }

    void display(cv::Mat& velocity, cv::Rect& velocity_rect, cv::Mat& altitude, cv::Rect& altitude_rect) {
        cv::Mat velocity3, altitude3;

        cv::cvtColor(velocity, velocity3, CV_GRAY2BGR);
        velocity3.copyTo(frame(velocity_rect));

        cv::cvtColor(altitude, altitude3, CV_GRAY2BGR);
        altitude3.copyTo(frame(altitude_rect));

        imshow("JCSAT-14", frame);
        cv::waitKey();
    }

    bool skip_to_approx_telemetry_start(int frame_step)
    {
        unsigned long current_frame = cap.get(cv::CAP_PROP_POS_FRAMES);
        char velocity_str[6];

        cap >> frame;

        while (!frame.empty()) {
            imshow("JCSAT-14", frame);
            cv::waitKey(1);

            if (get_velocity(velocity_rect, velocity_mat, velocity_str)) {
                cap.set(cv::CAP_PROP_POS_FRAMES, std::max(0ul, current_frame - frame_step));
                return true;
            }

            current_frame += frame_step;
            cap.set(cv::CAP_PROP_POS_FRAMES, current_frame);

            cap >> frame;
        }

        return false;
    }

    template <typename Itr>
    bool skip_to_telemetry_start(const Itr expected_start, const Itr expected_end) {
        int frame_step = roundl(fps * 30);
        char velocity_str[6];

        if (skip_to_approx_telemetry_start(frame_step)) {
            auto itr = expected_start;
            while (itr != expected_end) {

                cap >> frame;

                if (frame.empty()) {
                    return false;
                }

                if (!get_velocity(velocity_rect, velocity_mat, velocity_str)) {
                    continue;
                }

                int velocity = atoi(velocity_str);

                if (*itr == velocity) {
                    ++itr;
                } else {
                    itr = expected_start;
                }
            }

            return true;
        } else {
            return false;
        }
    }

    void begin_processing_telemetry(Telemetry& result)
    {
        cv::Rect display_rect(541, 142, 144, 40);
        cv::Rect altitude_display_rect(541, 342, 144, 40);

        int prev_digits = 0;
        int prev_velocity = 0;

        bool do_show = false;
        int empty_count = 0;

        for (int x = 0; ; ++x)
        {
            cap >> frame;

            cv::Size size = frame.size();
            if (frame.empty())
                break;

            configure_for_velocity();

            char velocity_str[6];
            int boxes[] = { 7, 33, 60, 89, 114 };
            if (!get_velocity(velocity_rect, velocity_mat, velocity_str)) {

                ++empty_count;
                //imshow("JCSAT-14", frame);
                //cv::waitKey();

                if (empty_count >= 5) {
                    unsigned long current = cap.get(cv::CAP_PROP_POS_FRAMES);
                    std::array<MinimumInt, 3> expected_mins{ prev_velocity - 1000, prev_velocity - 1000, prev_velocity - 1000 };

                    if (skip_to_telemetry_start(expected_mins.begin(), expected_mins.end())) {
                        unsigned long skipped = cap.get(cv::CAP_PROP_POS_FRAMES) - current;
                        for (unsigned long i = 0; i < skipped; i++) {
                            result.data.emplace_back(0, 0);
                        }

                        empty_count = 0;
                    } else {
                        break;
                    }
                }
            } else {
                empty_count = 0;
            }

           /* for (int i = 0; i < 5; i++) {
                int x1 = boxes[i] - 1;
                int y1 = 3;
                int w = 23 + 1;
                int h = 32;

                trainer.seen_digit(velocity_str[i] - '0', velocity_mat(cv::Rect(x1, y1, w, h)));

                if (trainer.is_ready()) {
                    cv::imwrite("training1.tif", trainer.generate_training_image());
                    throw std::runtime_error("finish");
                    //imshow("JCSAT-14", trainer.generate_training_image());
                    //cv::waitKey();
                }
            }*/

            int ocr_velocity = atoi(velocity_str);
            //if (ocr_velocity >= 9177) {
            //    cout << "Break" << endl;
            //    do_show = true;
            //}

            configure_for_altitude();
            double ocr_altitude = recognize<double>(frame, altitude_rect, altitude_mat);

            printf("%.5f, v = %5d, d = %.2f\n", (x / (double)fps), ocr_velocity, ocr_altitude);

            if (ocr_velocity < prev_velocity && (ocr_velocity > 8135 || ocr_velocity < 8210)) {
               // display(velocity_mat, display_rect, altitude_mat, altitude_display_rect);
            }

            if (do_show) {
                display(velocity_mat, display_rect, altitude_mat, altitude_display_rect);
            }

            //find_component_wise(velocity_mat);
            //display(velocity_mat, display_rect, altitude_mat, altitude_display_rect);

            result.data.emplace_back(ocr_velocity, ocr_altitude * 1000);

            prev_velocity = ocr_velocity;

            //if (ocr_velocity >=27106) { // crs8
            //if (ocr_velocity >= 26980) { // ses9
            //if (ocr_velocity >= 26720) { // jcsat14
            //    break;
            //}
        }
    }

public:

    SpaceXOCR(const std::string& file)
        : tess(), cap(file), velocity_rect(1041, 142, 72, 20), altitude_rect(1178, 140, 72, 20),
          velocity_mat(velocity_rect.size() * 2, CV_BGR2GRAY), altitude_mat(altitude_rect.size() * 2, CV_BGR2GRAY),
          trainer(24, 32, 10)
    {
        if (!cap.isOpened()) {
            throw std::runtime_error("Cannot open file: " + file);
        }

        fps = cap.get(cv::CAP_PROP_FPS);
        frame_count = cap.get(cv::CAP_PROP_FRAME_COUNT);
        width = (int)cap.get(cv::CAP_PROP_FRAME_WIDTH);
        height = (int)cap.get(cv::CAP_PROP_FRAME_HEIGHT);

        cout << boost::format("Opened %1% (%2% fps, %3%x%4%)") % file % fps % width % height << endl;

        if (width != 1280 || height != 720) {
            throw std::runtime_error("720p video expected");
        }

        //if (tess.Init("C:\\Program Files (x86)\\Tesseract-OCR", "spacex")) {
        if (tess.Init("D:\\dev\\spacex-ocr\\spacex-telemetry\\spacex-telemetry", "spacex")) {
            throw std::runtime_error("Couldn't initialize tesseract");
        }
    }

    Telemetry process() {
        Telemetry result(fps, frame_count);

        //cap.set(cv::CAP_PROP_POS_MSEC, 603 * 1000 + 2000);// +221000);
        //cap.set(cv::CAP_PROP_POS_MSEC, 1786 * 1000);
        cv::namedWindow("JCSAT-14", cv::WINDOW_AUTOSIZE);
        cv::setMouseCallback("JCSAT-14", onMouse, &frame);

        cout << "Searching for liftoff.." << endl;

        std::array<int, 2> start_seq{ 0, 1 };
        if (!skip_to_telemetry_start(start_seq.begin(), start_seq.end())) {
            throw std::runtime_error("Couldn't locate start of telemetry");
        }

        begin_processing_telemetry(result);
        return std::move(result);
        cout << "Found approximate liftoff time.. Searching for T-0..." << endl;

        if (tess.Init("D:\\dev\\spacex-ocr\\spacex-telemetry\\spacex-telemetry", "spacex")) {
            throw std::runtime_error("Couldn't initialize tesseract for digits");
        }

        // Get our first reading..
        configure_for_velocity();
        int prev_velocity = 0;

        for (int x = 0; x < fps * 20; x++) {
            cap >> frame;

            if (frame.empty()) {
                break;
            }

            char velocity_str[6];
            if (!get_velocity(velocity_rect, velocity_mat, velocity_str))// recognize<int>(frame, velocity_rect, velocity_mat);
                continue;
            int ocr_velocity = atoi(velocity_str);
            cout << ocr_velocity << endl;

            if (prev_velocity == 0 && ocr_velocity == 1) {
                double liftoff = cap.get(cv::CAP_PROP_POS_MSEC) / 1000;
                printf("Liftoff at %.2f seconds into video\n", liftoff);

                imshow("JCSAT-14", frame);
                cv::waitKey();

                begin_processing_telemetry(result);
                return std::move(result);
            }

            prev_velocity = ocr_velocity;
        }

        throw std::runtime_error("Couldn't find precise liftoff frame");
    }

};

int main(int argc, char* argv[])
{
    try {
        //SpaceXOCR ocr("D:\\dev\\spacex-ocr\\ses9.mp4");
        //SpaceXOCR ocr("D:\\dev\\spacex-ocr\\crs8.mp4");
        SpaceXOCR ocr("E:\\jcsat14.mp4");
        Telemetry telemetry = ocr.process();

        std::ofstream myfile;
        myfile.open("velocity.csv");
        telemetry.write_data(myfile);
    } catch (std::exception& e) {
        cerr << "Failed: " << e.what() << endl;
    }

     return 0;
}


// Boost.Units - A C++ library for zero-overhead dimensional analysis and
// unit/quantity manipulation and conversion
//
// Copyright (C) 2003-2008 Matthias Christian Schabel
// Copyright (C) 2008 Steven Watanabe
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_UNITS_TIME_BASE_DIMENSION_HPP
#define BOOST_UNITS_TIME_BASE_DIMENSION_HPP

#include <boost/units/config.hpp>
#include <boost/units/base_dimension.hpp>

namespace boost {

namespace units {

/// base dimension of time
struct time_base_dimension :
    boost::units::base_dimension<time_base_dimension,-7>
{ };

} // namespace units

} // namespace boost

#if BOOST_UNITS_HAS_BOOST_TYPEOF

#include BOOST_TYPEOF_INCREMENT_REGISTRATION_GROUP()

BOOST_TYPEOF_REGISTER_TYPE(boost::units::time_base_dimension)

#endif

namespace boost {

namespace units {

/// dimension of time (T)
typedef time_base_dimension::dimension_type time_dimension;

} // namespace units

} // namespace boost

#endif // BOOST_UNITS_TIME_BASE_DIMENSION_HPP


#include <boost/hana/concept/monoid.hpp>


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   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_ARITHMETIC_FUNCTIONS_MIN_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_MIN_HPP_INCLUDED
#include <boost/simd/include/functor.hpp>
#include <boost/dispatch/include/functor.hpp>

namespace boost { namespace simd { namespace tag
  {
   /*!
      @brief  min generic tag

      Represents the min function in generic contexts.

      @par Models:
      Hierarchy
    **/
    struct min_ : ext::elementwise_<min_>
    {
      /// @brief Parent hierarchy
      typedef ext::elementwise_<min_> parent;
      template<class... Args>
      static BOOST_FORCEINLINE BOOST_AUTO_DECLTYPE dispatch(Args&&... args)
      BOOST_AUTO_DECLTYPE_BODY( dispatching_min_( ext::adl_helper(), static_cast<Args&&>(args)... ) )
    };
  }
  namespace ext
  {
   template<class Site>
   BOOST_FORCEINLINE generic_dispatcher<tag::min_, Site> dispatching_min_(adl_helper, boost::dispatch::meta::unknown_<Site>, ...)
   {
     return generic_dispatcher<tag::min_, Site>();
   }
   template<class... Args>
   struct impl_min_;
  }
  /*!
    Computes the smallest of its parameter.

    @par semantic:
    For any given value @c x and @c y of type @c T:

    @code
    T r = min(x, y);
    @endcode

    is similar to:

    @code
    T r =  if (x < y) ? x : y;
    @endcode

    @param  a0
    @param  a1

    @return an value of the same type as the input.

  **/
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::min_, min, 2)
} }

#endif


#include <iostream>
#include <fstream>

//using namespace std;
#include "tests_cryptoTools/UnitTests.h"
#include "libOTe_Tests/UnitTests.h"

#include <cryptoTools/Common/Defines.h>
using namespace osuCrypto;

#include <cryptoTools/Network/Channel.h>
#include <cryptoTools/Network/Session.h>
#include <cryptoTools/Network/IOService.h>
#include <numeric>
#include <cryptoTools/Common/Timer.h>
#include <cryptoTools/Common/Matrix.h>
#include <cryptoTools/Common/BitVector.h>
#include <cryptoTools/Common/Log.h>
int miraclTestMain();
#include <cryptoTools/Crypto/PRNG.h>

#include "libOTe/TwoChooseOne/KosOtExtReceiver.h"
#include "libOTe/TwoChooseOne/KosOtExtSender.h"
#include "libOTe/TwoChooseOne/KosDotExtReceiver.h"
#include "libOTe/TwoChooseOne/KosDotExtSender.h"
#include "libOTe/TwoChooseOne/IknpOtExtReceiver.h"
#include "libOTe/TwoChooseOne/IknpOtExtSender.h"
#include "libOTe/TwoChooseOne/IknpDotExtReceiver.h"
#include "libOTe/TwoChooseOne/IknpDotExtSender.h"

#include "libOTe/NChooseOne/Oos/OosNcoOtReceiver.h"
#include "libOTe/NChooseOne/Oos/OosNcoOtSender.h"
#include "libOTe/NChooseOne/Kkrt/KkrtNcoOtReceiver.h"
#include "libOTe/NChooseOne/Kkrt/KkrtNcoOtSender.h"

#include "libOTe/TwoChooseOne/SilentOtExtReceiver.h"
#include "libOTe/TwoChooseOne/SilentOtExtSender.h"

#include "libOTe/NChooseK/AknOtReceiver.h"
#include "libOTe/NChooseK/AknOtSender.h"

#include <cryptoTools/Common/CLP.h>
#include "util.h"
#include <iomanip>
#include <boost/preprocessor/variadic/size.hpp>

#include "libOTe/Base/SimplestOT.h"
#include "libOTe/Base/MasnyRindal.h"
#include "libOTe/Base/MasnyRindalKyber.h"
#include "libOTe/Base/naor-pinkas.h"

#include "cryptoTools/Crypto/RandomOracle.h"

template<typename NcoOtSender, typename  NcoOtReceiver>
void execution(Role role, int n, int K, int numThreads, std::string ip)
{
	const u64 step = 1024;
	int totalOTs = n;
	auto numChosenMsgs = K;

	if (totalOTs == 0)
		totalOTs = 1 << 20;

	bool randomOT = false;
	auto numOTs = totalOTs / numThreads;

	// get up the networking
	auto rr = role == Role::Sender ? SessionMode::Server : SessionMode::Client;
	IOService ios;
	Session  ep0(ios, ip, rr);
	PRNG prng(sysRandomSeed());

	// for each thread we need to construct a channel (socket) for it to communicate on.
	std::vector<Channel> chls(numThreads);
	for (int i = 0; i < numThreads; ++i)
		chls[i] = ep0.addChannel();

	std::vector<NcoOtReceiver> recvers(numThreads);
	std::vector<NcoOtSender> senders(numThreads);

	// all Nco Ot extenders must have configure called first. This determines
	// a variety of parameters such as how many base OTs are required.
	bool maliciousSecure = false;
	u64 statSecParam = 40;
	u64 inputBitCount = K;
	recvers[0].configure(maliciousSecure, statSecParam, inputBitCount);
	senders[0].configure(maliciousSecure, statSecParam, inputBitCount);

	// Generate new base OTs for the first extender. This will use
	// the default BaseOT protocol. You can also manually set the
	// base OTs with setBaseOts(...);
	if (role == Role::Sender)
		senders[0].genBaseOts(prng, chls[0]);
	else
		recvers[0].genBaseOts(prng, chls[0]);

	// now that we have one valid pair of extenders, we can call split on
	// them to get more copies which can be used concurrently.
	for (int i = 1; i < numThreads; ++i)
	{
		recvers[i] = recvers[0].splitBase();
		senders[i] = senders[0].splitBase();
	}

	// create a lambda function that performs the computation of a single receiver thread.
	auto recvRoutine = [&](int k)
	{
		auto& chl = chls[k];
		PRNG prng(sysRandomSeed());

		std::vector<block>recvMsgs(numOTs);
		std::vector<u64> choices(numOTs);

		// define which messages the receiver should learn.
		for (int i = 0; i < numOTs; ++i){
			choices[i] = ((uint64_t) prng.get<u64>()) % K; // REJECTION SAMPLING make 128 > 40
			// << "Choices: " << choices[i] << std::endl;
			//std::cout << "Choices: " << (uint32_t) (uint8_t) choices[i] << std::endl;
		}

		// the messages that were learned are written to recvMsgs.
		recvers[k].receiveChosen(numChosenMsgs, recvMsgs, choices, prng, chl);

		std::ofstream exportFile;
		exportFile.open("./../data/client/rotkey.txt", std::ofstream::binary);
		const std::array<char, 16> arr = prng.getSeed().as<char>();
		exportFile.write((char *) &arr, 16);
		exportFile.close();

		std::ofstream exportFile2;
		exportFile2.open("./../data/client/rot", std::ofstream::binary);

		exportFile2.write((char *)&n, sizeof(n));
		exportFile2.write((char *)&K, sizeof(K));
		for (int i = 0; i < numOTs; i++){
			const std::array<char, 16> arr = recvMsgs.at(i).as<char>();
			//exportFile2.write((char *) &arr[0], 1);
		}
		for (int i = 0; i < numOTs; i++){
			uint8_t reduced = (uint8_t) choices[i];
			// << "Choices: " << (uint32_t) reduced << std::endl;
			//exportFile2.write((char *) &reduced, 1);
		}
		exportFile2.close();
		std::cout << "Sent Data: " << chl.getTotalDataSent() << std::endl;
		std::cout << "Recv Data: " << chl.getTotalDataRecv() << std::endl;
	};

	// create a lambda function that performs the computation of a single sender thread.
	auto sendRoutine = [&](int k)
	{
		auto& chl = chls[k];
		PRNG prng(sysRandomSeed());

		Matrix<block> sendMessages(numOTs, numChosenMsgs);
		prng.get(sendMessages.data(), sendMessages.size());
		//populate sender OT correlation with randomness

		Matrix<uint8_t> shortMessages(numOTs, numChosenMsgs);
		for (int i = 0; i < numOTs; ++i){
			for (int j = 0; j < numChosenMsgs; ++j){
				shortMessages(i, j) = sendMessages(i, j).as<char>()[0];
			}
		}

		//senders[k].encode()

		// perform the OTs with the given messages.
		senders[k].sendChosen(sendMessages, prng, chl);

		for (int i = 0; i < numOTs*numChosenMsgs; i++){
			const std::array<char, 16> arr = sendMessages.data()[i].as<char>();
			for (int j = 0; j < 16; j++){
				// << (int) arr[j] << std::endl;
			}
		}

		std::ofstream exportFile;
		exportFile.open("./../data/server/sotkey.txt", std::ofstream::binary);
		const std::array<char, 16> arr = prng.getSeed().as<char>();

		exportFile.write((char *) &arr, 16);
		exportFile.close();

		std::ofstream exportFile2;
		exportFile2.open("./../data/server/sot", std::ofstream::binary);

		exportFile2.write((char *)&n, sizeof(n));
		exportFile2.write((char *)&K, sizeof(K));
		for (int i = 0; i < numOTs*numChosenMsgs; i++){
			const std::array<char, 16> arr = sendMessages.data()[i].as<char>();
			//exportFile2.write((char *) &arr[0], 1);
		}
		exportFile2.close();
	};

	std::vector<std::thread> thds(numThreads);
	std::function<void(int)> routine;

	if (role == Role::Sender)
		routine = sendRoutine;
	else
		routine = recvRoutine;

	Timer time;
	auto s = time.setTimePoint("start");

	for (int k = 0; k < numThreads; ++k)
		thds[k] = std::thread(routine, k);

	for (int k = 0; k < numThreads; ++k)
		thds[k].join();

	auto e = time.setTimePoint("finish");
	auto milli = std::chrono::duration_cast<std::chrono::milliseconds>(e - s).count();

	if (role == Role::Sender)
		std::cout << "KKRT: " << " n=" << totalOTs << " " << milli << " ms" << std::endl;
}

using ProtocolFunc = std::function<void(Role, int, int, int, std::string)>;

bool runIf(ProtocolFunc protocol, uint8_t roles, std::string ip, int n, int k)
{
	uint8_t t = 1;

	if (roles == 1)
	{
		protocol(Role::Sender, n, k, t, ip);
	}
	else if (roles == 2)
	{
		protocol(Role::Receiver, n, k, t, ip);
	}
	else{
		auto thrd = std::thread([&] {
			try { protocol(Role::Sender, n, k, t, ip); }
			catch (std::exception & e)
			{
				lout << "Sender: " << e.what() << std::endl;
			}
			});

		try { protocol(Role::Receiver, n, k, t, ip); }
		catch (std::exception & e)
		{
			lout << "Receiver: " <<e.what() << std::endl;
		}
		thrd.join();
	}

	return true;
}

int main(int argc, char** argv)
{
	uint8_t roles;
	roles = 0; //BOTH
	roles = 1; //SENDER
	roles = 2; //RECEIVER

	roles = 0;
	std::string ip = "localhost:1212";
	runIf(execution<KkrtNcoOtSender, KkrtNcoOtReceiver>, roles, ip, 16000000, 13);

}


#include <boost/numeric/interval/interval.hpp>


#include <boost/geometry/algorithms/point_on_surface.hpp>


// __BEGIN_LICENSE__
// Copyright (C) 2006-2011 United States Government as represented by
// the Administrator of the National Aeronautics and Space Administration.
// All Rights Reserved.
// __END_LICENSE__

/// \file rmax2cahvor.cc
///

/************************************************************************
 *     File: rmaxcahvor.cc
 *     Date: May 2007
 *       By: Todd Templeton
 *      For: NASA Ames Research Center, Intelligent Mechanisms Group
 * Function: Extract CAHVOR camera model from RMAX images
 *
 ************************************************************************/
#include <stdlib.h>
#include <iostream>

#include <boost/algorithm/string.hpp>
#include <boost/program_options.hpp>
namespace po = boost::program_options;

#include <vw/Camera/CAHVORModel.h>
using namespace vw;
using namespace vw::camera;

#include <asp/Sessions/RMAX/RMAX.h>

static std::string prefix_from_filename(std::string const& filename) {
  std::string result = filename;
  int index = result.rfind(".");
  if (index != -1)
    result.erase(index, result.size());
  return result;
}

int main(int argc, char* argv[]) {
  std::string image_file;
  std::string adjustment_filename;

  po::options_description general_options("Options");
  general_options.add_options()
    ("adjustment-file,a", po::value<std::string>(&adjustment_filename), "Apply adjustments from file.\n")
    ("help,h", "Display this help message")
    ("verbose", "Verbose output");

  po::options_description hidden_options("");
  hidden_options.add_options()
    ("input-file", po::value<std::string>(&image_file));

  po::options_description options("Allowed Options");
  options.add(general_options).add(hidden_options);

  po::positional_options_description p;
  p.add("input-file", -1);

  po::variables_map vm;
  po::store( po::command_line_parser( argc, argv ).options(options).positional(p).run(), vm );
  po::notify( vm );

  std::ostringstream usage;
  usage << "Usage: " << argv[0] << " [options] <rmax image filenames>..." << std::endl << std::endl;
  usage << general_options << std::endl;

  if( vm.count("help") ) {
    std::cout << usage.str() << std::endl;
    return 1;
  } else if ( vm.count("input-file") < 1 ) {
    std::cout << "No input files!\n" << usage.str() << std::endl;
    return 1;
  }

  std::string output;
  if( vm.count("adjustment-file") ) {
    ImageInfo info;
    read_image_info( image_file, info );

    std::ifstream f(adjustment_filename.c_str());
    Vector3 position_adjustment;
    Vector3 pose_adjustment;

    f >> position_adjustment[0] >> position_adjustment[1] >> position_adjustment[2];
    f >> pose_adjustment[0] >> pose_adjustment[1] >> pose_adjustment[2];
    std::cout << position_adjustment << "   " << pose_adjustment<< "\n";
    CAHVORModel cahvor = rmax_image_camera_model(info, position_adjustment, pose_adjustment);
    output = prefix_from_filename(image_file) + ".rmax_adjust.cahvor";
    cahvor.write(output);
  } else {
    CAHVORModel cahvor = rmax_image_camera_model(argv[1]);
    output = prefix_from_filename(image_file) + ".cahvor";
    cahvor.write(output);
  }
}


/*
 * LikelihoodRecoAnalyser.cpp
 *
 *  Created on: Mey 13, 2015
 *      Author: ejclemen
 */
#include "../../interface/Analysers/LikelihoodRecoAnalyser.h"
#include "../../interface/LumiReWeighting.h"
#include "../../interface/EventWeightProvider.h"
#include "../../interface/PoissonMeanShifter.h"
#include "../../interface/GlobalVariables.h"
#include <boost/scoped_ptr.hpp>

//using namespace reweight;
using namespace BAT;

void LikelihoodRecoAnalyser::analyse(const EventPtr event) {
}

void LikelihoodRecoAnalyser::analyse(const EventPtr event, const JetCollection jets, const JetCollection bjets, const LeptonPointer selectedLepton, const METPointer met ) {

	weight_ = event->weight();
	treeMan_->setCurrentFolder(histogramFolder_);
	histMan_->setCurrentHistogramFolder(histogramFolder_);
	int ttbarpartonsfound = 0;
	const JetCollection AllJets(event->Jets());
	int TypeofSolution = 0;//1 = correct, 2=wrong cobination, 3 = not reconstructible, 4 = not semileptonic
	int EventCorrect = 0;//0=False, 1=True

	LikelihoodBasedTopPairReconstruction likelihoodReco( selectedLepton, met, jets, bjets );

	likelihoodReco.reconstruct();

	TtbarHypothesisCollection allSolutions = likelihoodReco.getAllSolutions();

	if (allSolutions.size() != 0){

		for (unsigned int index = 0; index<allSolutions.size(); ++index){
			TtbarHypothesisPointer solution = allSolutions[index];
			double Discrim = solution->discriminator;
			treeMan_->Fill( "likelihoodRatioDiscrimAllJetCombinations", Discrim );
		}

		TtbarHypothesisPointer bestSolution = likelihoodReco.getBestSolution();

		ParticlePointer TopHad = bestSolution->hadronicTop;
		ParticlePointer TopLep = bestSolution->leptonicTop;
		ParticlePointer W = bestSolution->hadronicW;
		// double Nu = bestSolution->neutrinoChi2;

		treeMan_->Fill("LeptonicTop_Pt",TopLep->pt() );
		treeMan_->Fill("LeptonicTop_Eta",TopLep->eta() );
		treeMan_->Fill("LeptonicTop_Energy",TopLep->energy() );
		treeMan_->Fill("LeptonicTop_Mass",TopLep->mass() );

		treeMan_->Fill("HadronicTop_Pt",TopHad->pt());
		treeMan_->Fill("HadronicTop_Eta",TopHad->eta() );
		treeMan_->Fill("HadronicTop_Energy",TopHad->energy());
		treeMan_->Fill("HadronicTop_Mass",TopHad->mass() );

		// Require one of the event selections to have been satisfied, and we have a genuine ttbar event
		int selectionCriteria = -1;
		if ( event->PassesElectronTriggerAndSelection() && event->isSemiLeptonicElectron() ) selectionCriteria = SelectionCriteria::ElectronPlusJetsReference;
		else if ( event->PassesMuonTriggerAndSelection() && event->isSemiLeptonicMuon() ) selectionCriteria = SelectionCriteria::MuonPlusJetsReference;
		if ( selectionCriteria < 0 ) {
			TypeofSolution = 4;
		}

		for ( unsigned int AllJetIndex=0; AllJetIndex < AllJets.size(); ++AllJetIndex ) {

			JetPointer AllJet = AllJets[AllJetIndex];

			if ( AllJet->matched_parton() == 0 ) continue;//if null pointer

			if ( AllJet->ttbar_decay_parton() == 3 ){
				// cout << "Found qbar decay parton" << endl;
				++ttbarpartonsfound;
			}
			if ( AllJet->ttbar_decay_parton() == 4 ){
				// cout << "Found q decay parton" << endl;
				++ttbarpartonsfound;
			}
			if ( AllJet->ttbar_decay_parton() == 5 ){
				// cout << "Found leptonicb decay parton" << endl;
				++ttbarpartonsfound;
			}
			if ( AllJet->ttbar_decay_parton() == 6 ){
				// cout << "Found hadronicb decay parton" << endl;
				++ttbarpartonsfound;
			}
		}
		// cout << "-------------------" << endl;
		if ( ttbarpartonsfound != 4 && TypeofSolution == 0){
			TypeofSolution = 3;
		}

		double Discrim = bestSolution->discriminator;
		double CSVDiscrim = bestSolution->CSVDiscriminator;
		double MassDiscrim = bestSolution->MassDiscriminator;
		double NuChi2Discrim = bestSolution->NuChi2Discriminator;

		treeMan_->Fill( "likelihoodRatioDiscrimBest", Discrim );
		treeMan_->Fill( "CSVDiscrimBest", CSVDiscrim );
		treeMan_->Fill( "NuChi2DiscrimBest", NuChi2Discrim );
		treeMan_->Fill( "MassDiscrimBest", MassDiscrim );

		if (bestSolution->isCorrect() && TypeofSolution == 0){
			TypeofSolution = 1;
			EventCorrect = 1;
		}

		else{
			if (TypeofSolution == 0){
				TypeofSolution = 2;
			}
		}
		treeMan_->Fill("EventReconstruction", EventCorrect);
		treeMan_->Fill("TypeofSolution", TypeofSolution);

		// cout << "**********************************" << endl;
		// cout << "Got " << allSolutions.size() << " solutions" << endl;
		// cout << "Is the solution correct : " << bestSolution->isCorrect() << endl;
		// cout << "Got Best Solution : " << Discrim << ", Mass T : " << TopHad->mass() << ", Mass W : " << W->mass() << ", NuChi2 : " << Nu << endl;
		// cout << "NuChi2Discrim : " << NuChi2Discrim << ", MassDiscrim : " << MassDiscrim << ", CSVDiscrim" << CSVDiscrim << endl;
		// cout << "**********************************" << endl;
	}
}

LikelihoodRecoAnalyser::LikelihoodRecoAnalyser(HistogramManagerPtr histMan, TreeManagerPtr treeMan, const bool isElectronChannel, std::string histogramFolder ) :
		BasicAnalyser(histMan, treeMan, histogramFolder) //
{

}

LikelihoodRecoAnalyser::~LikelihoodRecoAnalyser() {
}

void LikelihoodRecoAnalyser::createHistograms() {
	histMan_->setCurrentHistogramFolder(histogramFolder_);

}

void LikelihoodRecoAnalyser::createTrees() {
	treeMan_->setCurrentFolder(histogramFolder_);

	treeMan_->addBranch("likelihoodRatioDiscrimBest", "F", "Discriminator" + Globals::treePrefix_);
	treeMan_->addBranch("CSVDiscrimBest", "F", "Discriminator" + Globals::treePrefix_);
	treeMan_->addBranch("MassDiscrimBest", "F", "Discriminator" + Globals::treePrefix_);
	treeMan_->addBranch("NuChi2DiscrimBest", "F", "Discriminator" + Globals::treePrefix_);

	treeMan_->addBranch("likelihoodRatioDiscrimAllJetCombinations", "F", "Discriminator" + Globals::treePrefix_);
	treeMan_->addBranch("EventReconstruction", "F", "Discriminator" + Globals::treePrefix_);

	treeMan_->addBranch("LeptonicTop_Pt", "F", "TopReco" + Globals::treePrefix_);
	treeMan_->addBranch("LeptonicTop_Eta", "F", "TopReco" + Globals::treePrefix_);
	treeMan_->addBranch("LeptonicTop_Energy", "F", "TopReco" + Globals::treePrefix_);
	treeMan_->addBranch("LeptonicTop_Mass", "F", "TopReco" + Globals::treePrefix_);

	treeMan_->addBranch("HadronicTop_Pt", "F", "TopReco" + Globals::treePrefix_);
	treeMan_->addBranch("HadronicTop_Eta", "F", "TopReco" + Globals::treePrefix_);
	treeMan_->addBranch("HadronicTop_Energy", "F", "TopReco" + Globals::treePrefix_);
	treeMan_->addBranch("HadronicTop_Mass", "F", "TopReco" + Globals::treePrefix_);

	treeMan_->addBranch("TypeofSolution", "F", "TopReco" + Globals::treePrefix_);

}


/**
 * \brief Adapt gtsam::DiscreteFactorGraph and it's descendants to a boost
 * graph that is model of following concepts:
 *
 * IncidenceGraph
 * AdjancencyGraph
 * VertexListGraph
 * EdgeListGraph
 * AdjacencyMatrix
 * PropertyGraph
 */

// Talk::
// What is gtsam::DiscreteFactorGraph
// factors_ : a vector of factor nodes
//
// it has been specialized by OccupancyGrid which adds the following
// information
//
// cell2factors_ : edges from cells to factors_ . Since the edges are
// undirected, these are the only edges we have.
//
// the vertex descriptors are boost::shared_ptrs of type
// gtsam::DiscreteFactors . These classes can also contain some of the
// required properties of factor type
//
// We don't really need a variable(cell)  class, but we have decide for a
// single type of vertex_descriptor. Probably we can create another level of
// redirection for factors
//
// So here goes the design:
//
// vertex_descriptor is gtsam::Index
//
// cells is gtsam::Index, but we need to know the cellCount()
//
// factors gtsam::Index, factors start from cellCount() + 1
//
// IsFactorMap:
// is_factor is simply a function mapping w.r.t. cellCount()
//
// FactorMap
// factor_pointers_ : gtsam::Index -> shared_ptr to gtsam::DiscreteFactors
// cell2factors_ needs to be changed to gtsam::Index -> gtsam::Index
//
// SampleSpaceMap in this case is a constant function
//
// edge_descriptor is just std::pair<vertex_descriptor, vertex_descriptor>
//
// MessageValues is also sorted out
//
#include "utility.hpp"

#include <gtsam/discrete/DiscreteFactorGraph.h>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/properties.hpp>
#include <boost/functional.hpp>
#include <boost/bind.hpp>
#include <boost/unordered_map.hpp>
#include <boost/iterator/counting_iterator.hpp>
#include <boost/lambda/lambda.hpp>
#include <boost/lambda/bind.hpp>
#include <boost/lambda/construct.hpp>
#include "BPLaserFactor.hpp"
#include "visualiser.h"

namespace occgrid {
  struct gtsam_traversal_category :
    public virtual boost::adjacency_graph_tag,
    public virtual boost::incidence_graph_tag,
    public virtual boost::vertex_list_graph_tag,
    public virtual occgrid::factor_graph_tag {
    };

  // internal property names
  struct sample_space_t {
    typedef boost::vertex_property_tag kind;
  };
  struct is_factor_t {
    typedef boost::vertex_property_tag kind;
  };
  struct factor_map_t {
    typedef boost::vertex_property_tag kind;
  };

  template <typename gtsamGraph, typename belief_type, typename SampleSpaceType, typename VertexType>
  class G {
    public:
    const gtsamGraph& g_;
    typedef belief_type factor_return_type;
    typedef SampleSpaceType sample_space_type;
    typedef VertexType vertex_descriptor;
    typedef std::pair<gtsam::Index, gtsam::Index> edge_descriptor;
    typedef boost::undirected_tag directed_category;
    typedef gtsam_traversal_category traversal_category;
    typedef size_t vertices_size_type;
    typedef size_t degree_size_type;
    typedef typename std::vector<vertex_descriptor>::const_iterator adjacency_iterator;

    typedef boost::function< std::pair<vertex_descriptor, vertex_descriptor> (vertex_descriptor) > Function;
    typedef typename boost::transform_iterator<Function, adjacency_iterator> out_edge_iterator;
    typedef boost::counting_iterator<vertex_descriptor> vertex_iterator;
    typedef boost::counting_iterator<vertex_descriptor> variable_iterator;
    // I don't know why we need them?
    typedef size_t edges_size_type;
    typedef typename std::vector<edge_descriptor>::iterator in_edge_iterator;
    typedef typename std::vector<edge_descriptor>::iterator edge_iterator;
    typedef typename boost::disallow_parallel_edge_tag edge_parallel_category;

    G(const gtsamGraph& g) : g_(g) {};

    std::pair<adjacency_iterator, adjacency_iterator>
    adjacent_vertices(vertex_descriptor v) const
    {
      return std::make_pair(g_.getNeighbors(v).begin(), g_.getNeighbors(v).end());
    }

    std::pair<out_edge_iterator, out_edge_iterator>
      out_edges(vertex_descriptor v) const
      {
        namespace bl = boost::lambda;
        BOOST_AUTO(f, bl::bind(
              bl::constructor<std::pair<vertex_descriptor, vertex_descriptor> >(),
              v, bl::_1));

        return std::make_pair(
            out_edge_iterator(g_.getNeighbors(v).begin(), f),
            out_edge_iterator(g_.getNeighbors(v).end(), f));
      }

    degree_size_type
      out_degree(vertex_descriptor v)  const
      {
        return g_.getNeighbors(v).size();
      }

    vertices_size_type
      num_vertices() const {
        return g_.cellCount() + g_.factorCount();
      }

    vertices_size_type
    num_variables() const {
      return g_.cellCount();
    }

    vertices_size_type
      num_factors() const {
        return g_.factorCount();
      }

    bool is_factor(vertex_descriptor v) const {
      return g_.is_factor(v);
    }

    template <typename MessageValues>
    belief_type compute_belief(
        vertex_descriptor node_idx,
        vertex_descriptor cell,
        const sample_space_type& cell_value,
        MessageValues& msgs
        ) const {
      return BPLaserFactor(*g_.factorFromNodeId(node_idx)).belief(
          cell, cell_value, msgs);
    }

    template <typename Visualizer, typename MessageValues>
      void display(Visualizer& vis, const MessageValues& msgs) const {
        vis.setMarginals(msgs);
        vis.show();
      }

    // inline boost::function<belief_type (typename Factor::argument_type)>
    //   factorFromNodeId(gtsam::Index node_idx) const {
    //     BOOST_AUTO_TPL(factor, *(g_.factorFromNodeId(node_idx)));
    //     return boost::bind(toprobability<belief_type>, boost::bind(factor, _1));
    //   }

    struct SampleSpaceMap {
      typedef sample_space_type ValueType;
      typedef std::vector<ValueType> vector_type;
      typedef typename std::vector<ValueType>::const_iterator const_iterator;
      typedef std::pair<const_iterator, const_iterator> value_type;
      typedef std::pair<const_iterator, const_iterator> reference;
      typedef vertex_descriptor key_type;
      typedef boost::readable_property_map_tag category;

      reference
      get(key_type key) const {
        typedef sample_space_type ValueType;
        const static ValueType binary[] = {0, 1};
        const static vector_type binary_vector(binary, binary + 2);
        return std::make_pair(binary_vector.begin(), binary_vector.end());
      }

      static size_t size() { return 2; }
    };

    class IsFactorMap {
      private:
        const G& g_;
      public:
      typedef bool value_type;
      typedef bool reference;
      typedef vertex_descriptor key_type;
      typedef boost::readable_property_map_tag category;
      IsFactorMap(const G& g) : g_(g) { }
      reference get(key_type key) const {
        return g_.is_factor(key) ;
      }
    };

    struct MessageTypes {
      struct key_type :
        public std::pair< std::pair<vertex_descriptor, vertex_descriptor>, sample_space_type> {
        explicit key_type(vertex_descriptor u, vertex_descriptor v, sample_space_type ss) :
          std::pair< std::pair<vertex_descriptor, vertex_descriptor>, sample_space_type>(
              std::pair<vertex_descriptor, vertex_descriptor>(u, v), ss) {};
      };
      /// provides default value for a message.
      struct base_type
        : public boost::unordered_map<key_type, belief_type> {
          private:
            belief_type default_msg_;
          public:
          base_type() : boost::unordered_map<key_type, belief_type>(),
                        default_msg_(1 / SampleSpaceMap::size()) {
                          assert(! isnan(default_msg_));
                        }
          base_type(belief_type default_msg)
            : boost::unordered_map<key_type, belief_type>(),
            default_msg_(default_msg) {
              assert(! isnan(default_msg_));
            }
          belief_type& operator[](const key_type& k) const {
            if (this->find(k) == this->end())
              (const_cast<base_type*>(this))->boost::unordered_map<key_type, belief_type>::operator[](k) = default_msg_;
            return (const_cast<base_type*>(this))->boost::unordered_map<key_type, belief_type>::operator[](k);
          }
      };
      typedef boost::associative_property_map<base_type> property_map_type;
    };

    typedef typename MessageTypes::property_map_type  MessageValues;
    typedef boost::function<belief_type (MessageValues)> factor_type;
    struct FactorMap {
      private:
        const G& g_;
      public:
      typedef factor_type value_type;
      typedef const value_type reference;
      typedef vertex_descriptor key_type;
      typedef boost::readable_property_map_tag category;
      FactorMap(const G& g) : g_(g) { }
      reference get(key_type key) const {
        return g_.factorFromNodeId(key);
      }
    };
  };

  template <typename G>
  std::pair<typename G::adjacency_iterator, typename G::adjacency_iterator>
  adjacent_vertices(typename G::vertex_descriptor v, const G& g)
  {
    return g.adjacent_vertices(v);
  }

  template <typename G>
  std::pair<typename G::out_edge_iterator, typename G::out_edge_iterator>
  out_edges(typename G::vertex_descriptor v, const G& g)
  {
    return g.out_edges(v);
  }

  template <typename G>
  typename G::degree_size_type
  out_degree(typename G::vertex_descriptor v, const G& g)
  {
    return g.out_degree(v);
  }

  template <typename G>
    typename G::vertices_size_type
    num_vertices(const G& g) {
      return g.num_vertices();
    }

  template <typename G>
    typename G::vertices_size_type
    num_variables(const G& g) {
      return g.num_variables();
    }

  template <typename G>
    typename G::vertices_size_type
    num_factors(const G& g) {
      return g.num_factors();
    }

  template <typename G>
    bool is_factor(typename boost::graph_traits<G>::vertex_descriptor x, const G& g) {
      return g.is_factor(x);
    }

  template <typename G>
    std::pair<typename G::vertex_iterator, typename G::vertex_iterator>
    vertices(const G& g) {
      typedef typename G::vertices_size_type vertices_size_type;
      vertices_size_type n(num_vertices(g));
      return std::make_pair(boost::counting_iterator<vertices_size_type>(0),
                            boost::counting_iterator<vertices_size_type>(n));
    }

  template <typename G>
    std::pair<typename G::vertex_iterator, typename G::vertex_iterator>
    factors(const G& g) {
      typedef typename G::vertices_size_type vertices_size_type;
      vertices_size_type nv(num_vertices(g));
      vertices_size_type nf(num_factors(g));
      return std::make_pair(boost::counting_iterator<vertices_size_type>(nv - nf),
                            boost::counting_iterator<vertices_size_type>(nv));
    }

  template <typename G>
    std::pair<typename G::variable_iterator, typename G::variable_iterator>
    variables(const G& g) {
      typedef typename G::vertices_size_type vertices_size_type;
      vertices_size_type n(num_variables(g));
      return std::make_pair(boost::counting_iterator<vertices_size_type>(0),
                            boost::counting_iterator<vertices_size_type>(n));
    }

  template <typename G>
    typename G::edges_size_type
    num_edges(const G& ogg) {
      typedef typename boost::graph_traits<G>::vertex_iterator vertex_iterator;
      vertex_iterator v, v_end;
      typename G::edges_size_type nedges = 0;
      for (boost::tie(v, v_end) = vertices(ogg); v != v_end; ++v)
        nedges += out_degree(*v, ogg);
      return nedges;
    }

  // TODO implement edges() by concatenating iterators from adjacency_list

  template <typename G>
  typename G::SampleSpaceMap get(sample_space_t, const G& g) {
    typename G::SampleSpaceMap ssm;
    return ssm;
  }

  template <typename G>
  typename G::IsFactorMap get(is_factor_t, const G& g) {
    return typename G::IsFactorMap(g);
  }
  template <typename G>
  typename G::FactorMap get(factor_map_t, const G& g) {
    return typename G::FactorMap(g);
  }

} // namespace occgrid


#include "IfSearch.h"

#include <DDTcore.h>
#include <EigenFace.h>
#include <EigenFaceData.h>
#include <EigenFaceParameters.h>
#include <EigenFaceMaskedArray.h>
#include <FileWriteProfile.h>
#include <InfoMacros.h>
#include <Settings.h>

#include <Key.h>

#ifdef ENABLE_AVGFACE
#include <AverageFace.h>

bool IfSearch::avgFaceInit(void)
{
    NULLPTR(eigenParms);
    NULLPTR(fwpAvgFace);

    avgFace = new AverageFace(eigenParms->normalizedSize(),
                              eigenParms->normalEyes());
    NULLPTR(avgFace);

    int min = appSettings->value(tr("AvgFace/MinKey", "config"), 100001).toInt();
    int max = appSettings->value(tr("AvgFace/MaxKey", "config"), 199999).toInt();
    AverageFaceKeyTraits::setRange(min, max);

    QDir dir = fwpAvgFace->dir();
    QStringList nameFilters;
    nameFilters << "*." + fwpAvgFace->format();
    QStringList files = dir.entryList(nameFilters);
    PROGRESS("Reading %1 previous average faces", files.size());
    DDT::Info::flush();

    foreach (QString fileName, files)
    {
        if (fileName.startsWith("Avg"))
            continue;
        QImage image(dir.absoluteFilePath(fileName));
        avgFace->add(image, eigenParms->normalEyes());
        AverageFaceKey key(fileName.toInt());
        AverageFaceKey::take(key);
    }
    return true;
} // avgFaceInit()

bool IfSearch::avgFaceProcess(QImage normImage, Eyes normEyes)
{
    NULLPTR(avgFace);
    NULLPTR(fwpAvgFace);

    AverageFaceKey key = AverageFaceKey::newKey();
    avgFace->add(normImage, normEyes);
    fwpAvgFace->write(normImage, QString::number(key));
    return true;
} // avgFaceProcess()

bool IfSearch::avgFaceFinish(void)
{
    NULLPTR(avgFace);
    NULLPTR(fwpAvgFace);

    QImage avgImage = avgFace->average();
    fwpAvgFace->write(avgImage, "AvgFace");
    QImage eigenFace = INDI::EigenFace::normalize(avgImage,
                                            eigenParms->normalEyes(),
                                            eigenData->eigenSize(),
                                            eigenData->eigenEyes(),
                                            QImage::Format_Indexed8);
    INDI::EigenFaceMaskedArray avgArray(eigenData);
    avgArray.fromImage(eigenFace);
    QString xmlFileName = fwpAvgFace->dir().filePath("AvgFace.xml");
    QDomDocument doc;
    QDomElement root = doc.createElement("INDIfaceData");
    QDomElement de = doc.createElement(INDI::EigenFaceData::meanFaceTagName);
    avgArray.write(&de);
    root.appendChild(de);
    doc.appendChild(root);
    DDTcore::writeXmlFile(xmlFileName, doc);
    QImage avgEigen = avgArray.toImage();
    fwpAvgFace->write(avgEigen, "AvgEigen");
    return true;
} // avgFaceFinish()

#endif


/*! @file frequency_bands.cc
 *  @brief Cut the frequency bands from a signal.
 *  @author Markovtsev Vadim <v.markovtsev@samsung.com>
 *  @version 1.0
 *
 *  @section Notes
 *  This code partially conforms to Google C++ Style Guide.
 *
 *  @section Copyright
 *  Copyright © 2013 Samsung R&D Institute Russia
 *
 *  @section License
 *  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.
 */

#include "src/transforms/frequency_bands.h"
#include <algorithm>
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wold-style-cast"
#include <boost/regex.hpp>
#pragma GCC diagnostic pop
#include "src/safe_omp.h"
#include "src/transforms/lowpass_filter.h"
#include "src/transforms/bandpass_filter.h"
#include "src/transforms/highpass_filter.h"
#include "src/stoi_function.h"

namespace sound_feature_extraction {
namespace transforms {

FilterOrders Parse(const std::string& value, identity<FilterOrders>) {
  if (value == "auto") {
    return FilterOrders();
  }
  FilterOrders result;
  std::transform(boost::sregex_token_iterator(value.begin(), value.end(),
                                              boost::regex("\\s*(\\d+)\\s*"),
                                              1),
                 boost::sregex_token_iterator(),
                 std::back_inserter(result),
                 std::stoi_function);
  if (result.empty()) {
    throw InvalidParameterValueException();
  }
  return result;
}

constexpr IIRFilterType FrequencyBands::kDefaultFilterType;

FrequencyBands::FrequencyBands()
    : number_(kDefaultBandsNumber),
      bands_(),
      filter_(kDefaultFilterType),
      lengths_() {
}

bool FrequencyBands::validate_number(const int& value) noexcept {
  return value >= 1 && value <= 8000;
}

bool FrequencyBands::validate_bands(const std::string& value) noexcept {
  if (value.empty()) {
    return true;
  }
  static const boost::regex all_regex("^\\s*(\\d+\\s*(\\s|$))+");
  boost::smatch match;
  if (!boost::regex_match(value, match, all_regex)) {
    return false;
  }
  std::vector<int> bands;
  std::transform(boost::sregex_token_iterator(value.begin(), value.end(),
                                              boost::regex("\\s*(\\d+)\\s*"),
                                              1),
                 boost::sregex_token_iterator(),
                 std::back_inserter(bands),
                 std::stoi_function);
  if (bands.size() < 1) {
    return false;
  }
  for (size_t i = 1; i < bands.size(); i++) {
    if (bands[i] <= bands[i - 1]) {
      return false;
    }
  }
  return true;
}
ALWAYS_VALID_TP(FrequencyBands, filter)

bool FrequencyBands::validate_lengths(const FilterOrders& value) noexcept {
  for (int len : value) {
    if (len < 1) {
      return false;
    }
  }
  return true;
}

const std::vector<std::shared_ptr<IIRFilterBase>>&
FrequencyBands::filters() const {
  return filters_;
}

void FrequencyBands::Initialize() const {
  filters_.clear();
  std::string bands = bands_;
  if (bands.empty()) {
    for (int i = 1; i < number_; i++) {
      bands += std::to_string(static_cast<int>(
          input_format_->SamplingRate() / (2.f * number_) * i)) + " ";
    }
  }

  int last_freq = 0, index = 0;
  static const boost::regex numbers_regex("\\s*(\\d+)\\s*");
  static const boost::sregex_token_iterator empty;
  boost::sregex_token_iterator bands_iterator(
        bands.begin(), bands.end(), numbers_regex, 1);
  assert(bands_iterator != empty);
  while (bands_iterator != empty) {
    int freq = std::stoi(*bands_iterator++);
    if (freq > input_format_->SamplingRate() / 2) {
      WRN("Warning: the bands after %i (defined by sampling "
          "rate %i) will be discarded (first greater band was "
          "%i).\n",
          input_format_->SamplingRate() / 2,
          input_format_->SamplingRate(),
          freq);
      break;
    }
    std::shared_ptr<IIRFilterBase> filter;
    if (last_freq == 0) {
      auto f = std::make_shared<LowpassFilter>();
      f->set_frequency(freq);
      filter = f;
    } else {
      auto f = std::make_shared<BandpassFilter>();
      f->set_frequency_low(last_freq);
      f->set_frequency_high(freq);
      filter = f;
    }
    SetupFilter(index++, freq, filter.get());
    filters_.push_back(filter);
    last_freq = freq;
  }
  // Append high-pass filter
  auto filter = std::make_shared<HighpassFilter>();
  filter->set_frequency(last_freq);
  SetupFilter(index, last_freq, filter.get());
  filters_.push_back(filter);

  for (const auto& filter : filters_) {  // NOLINT(*)
    filter->SetInputFormat(input_format_, 1);
    filter->Initialize();
  }
}

void FrequencyBands::SetupFilter(size_t index, int frequency,
                                 IIRFilterBase* filter) const {
  filter->set_type(filter_);
  auto ratio = frequency / input_format_->SamplingRate() * 2;
  if (lengths_.size() > index) {
    filter->set_length(lengths_[index]);
  } else {
    if (ratio < 0.1f) {
      filter->set_length(64);
    } else if (ratio < 0.3f) {
      filter->set_length(128);
    }
  }
}

void FrequencyBands::Do(const BuffersBase<float*>& in,
                        BuffersBase<float*>* out) const noexcept {
#ifdef HAVE_OPENMP
  #pragma omp parallel for num_threads(threads_number())
#endif
  for (size_t i = 0; i < in.Count(); i++) {
    filters_[i % filters_.size()]->Do(in[i], (*out)[i]);
  }
}

RTP(FrequencyBands, number)
RTP(FrequencyBands, bands)
RTP(FrequencyBands, filter)
RTP(FrequencyBands, lengths)
REGISTER_TRANSFORM(FrequencyBands);

}  // namespace transforms
}  // namespace sound_feature_extraction


/* Copyright Singapore-MIT Alliance for Research and Technology */

#include <cmath>

#include <boost/thread/thread.hpp>
#include <boost/thread/tss.hpp>
#include <boost/random/random_device.hpp>
#include <boost/random.hpp>

#include "geospatial/network/Lane.hpp"
#include "geospatial/network/Link.hpp"
#include "geospatial/network/RoadNetwork.hpp"
#include "geospatial/network/Node.hpp"
#include "geospatial/network/LaneConnector.hpp"
#include "geospatial/network/PT_Stop.hpp"
#include "geospatial/network/Node.hpp"
#include "geospatial/network/PolyLine.hpp"
#include "geospatial/network/TurningGroup.hpp"

#include "logging/Log.hpp"
#include "path/PathSetManager.hpp"
#include "conf/ConfigParams.hpp"
#include "conf/ConfigManager.hpp"
#include "A_StarShortestTravelTimePathImpl.hpp"

using std::map;
using std::set;
using std::vector;
using std::string;

using namespace sim_mob;

namespace sim_mob
{

namespace
{
    //create all time helpers objects outside loop
    const std::string MORNING_PEAK_START = "06:00:00";
    const std::string MORNING_PEAK_END = "10:00:00";
    const std::string EVENING_PEAK_START = "17:00:00";
    const std::string EVENING_PEAK_END = "20:00:00";
    const std::string OFF_PEAK1_START = "00:00:00";
    const std::string OFF_PEAK3_START = "24:00:00";

    const sim_mob::DailyTime morningPeakStartTime(MORNING_PEAK_START);
    const sim_mob::DailyTime morningPeakEndTime(MORNING_PEAK_END);
    const sim_mob::DailyTime eveningPeakStartTime(EVENING_PEAK_START);
    const sim_mob::DailyTime eveningPeakEndTime(EVENING_PEAK_END);
    const sim_mob::DailyTime offPeak1StartTime(OFF_PEAK1_START);
    const sim_mob::DailyTime offPeak3EndTime(OFF_PEAK3_START);
}

A_StarShortestTravelTimePathImpl::A_StarShortestTravelTimePathImpl(const sim_mob::RoadNetwork& network)
{
    //initialize random graph pool
    int randomCount = sim_mob::ConfigManager::GetInstance().FullConfig().getPathSetConf().perturbationIteration;
    for (int i = 0; i < randomCount; ++i)
    {
        drivingMapRandomPool.push_back(StreetDirectory::Graph());
        drivingNodeLookupRandomPool.push_back(NodeVertexLookup());
        drivingLinkLookupRandomPool.push_back(LinkEdgeLookup());
        drivingLinkVertexLookupRandomPool.push_back(LinkVertexLookup());
    }
    initDrivingNetwork(network);
}

void A_StarShortestTravelTimePathImpl::initDrivingNetwork(const sim_mob::RoadNetwork& network)
{
    const std::map<unsigned int, sim_mob::Link *>& links =  network.getMapOfIdVsLinks();

    //Various lookup structures
    NodeLookup nodeLookupMorningPeak;
    NodeLookup nodeLookupEveningPeak;
    NodeLookup nodeLookupNormalTime;
    NodeLookup nodeLookupDefault;
    NodeLookup nodeLookupHighwayBiasDistance;
    NodeLookup nodeLookupHighwayBiasMorningPeak;
    NodeLookup nodeLookupHighwayBiasEveningPeak;
    NodeLookup nodeLookupHighwayBiasNormalTime;
    NodeLookup nodeLookupHighwayBiasDefault;
    std::vector<NodeLookup> nodeLookupRandomPool;
    for (size_t i = 0; i < drivingMapRandomPool.size(); ++i) {
        nodeLookupRandomPool.push_back(NodeLookup());
    }

    //Add our initial set of vertices. Iterate through Links to ensure no un-used Node are added.
    std::map<unsigned int, sim_mob::Link *>::const_iterator iter;
    for (iter = links.begin(); iter != links.end(); ++iter) {
//      procAddDrivingNodes(drivingMapMorningPeak, iter->second, nodeLookupMorningPeak);
//      procAddDrivingNodes(drivingMapEveningPeak, iter->second, nodeLookupEveningPeak);
//      procAddDrivingNodes(drivingMapNormalTime, iter->second, nodeLookupNormalTime);
        procAddDrivingNodes(drivingMapDefault, iter->second, nodeLookupDefault);
        procAddDrivingNodes(drivingMapHighwayBiasDistance, iter->second, nodeLookupHighwayBiasDistance);
//      procAddDrivingNodes(drivingMapHighwayBiasMorningPeak, iter->second, nodeLookupHighwayBiasMorningPeak);
//      procAddDrivingNodes(drivingMapHighwayBiasEveningPeak, iter->second, nodeLookupHighwayBiasEveningPeak);
//      procAddDrivingNodes(drivingMapHighwayBiasNormalTime, iter->second, nodeLookupHighwayBiasNormalTime);
        procAddDrivingNodes(drivingMapHighwayBiasDefault, iter->second, nodeLookupHighwayBiasDefault);
        for (size_t i = 0; i < drivingMapRandomPool.size(); ++i) {
            procAddDrivingNodes(drivingMapRandomPool[i], iter->second, nodeLookupRandomPool[i]);
        }
    }

    //Proceed through our Links, adding each Link to the graph.
    for (iter = links.begin(); iter != links.end(); ++iter) {
//      procAddDrivingLinks(drivingMapMorningPeak, iter->second,nodeLookupMorningPeak, drivingLinkLookupMorningPeak,getEdgeWeight(iter->second,MorningPeak));
//      procAddDrivingLinks(drivingMapEveningPeak, iter->second,nodeLookupEveningPeak, drivingLinkLookupEveningPeak,getEdgeWeight(iter->second,EveningPeak));
//      procAddDrivingLinks(drivingMapNormalTime, iter->second, nodeLookupNormalTime, drivingLinkLookupNormalTime,getEdgeWeight(iter->second,OffPeak));
        procAddDrivingLinks(drivingMapDefault, iter->second, nodeLookupDefault, drivingLinkLookupDefault, drivingLinkVertexLookupDefault, getEdgeWeight(iter->second,Default));
        procAddDrivingLinks(drivingMapHighwayBiasDistance, iter->second,nodeLookupHighwayBiasDistance,drivingLinkLookupHighwayBiasDistance, drivingLinkVertexLookupHighwayBiasDistance, getEdgeWeight(iter->second,HighwayBiasDistance));
//      procAddDrivingLinks(drivingMapHighwayBiasMorningPeak, iter->second, nodeLookupHighwayBiasMorningPeak,drivingLinkLookupHighwayBiasMorningPeak,getEdgeWeight(iter->second,HighwayBiasMorningPeak));
//      procAddDrivingLinks(drivingMapHighwayBiasEveningPeak, iter->second, nodeLookupHighwayBiasEveningPeak,drivingLinkLookupHighwayBiasEveningPeak,getEdgeWeight(iter->second,HighwayBiasEveningPeak));
//      procAddDrivingLinks(drivingMapHighwayBiasNormalTime, iter->second, nodeLookupHighwayBiasNormalTime, drivingLinkLookupHighwayBiasNormalTime,getEdgeWeight(iter->second,HighwayBiasOffPeak));
        procAddDrivingLinks(drivingMapHighwayBiasDefault, iter->second, nodeLookupHighwayBiasDefault,drivingLinkLookupHighwayBiasDefault, drivingLinkVertexLookupHighwayBiasDefault, getEdgeWeight(iter->second,HighwayBiasDefault));
        for (size_t i = 0; i < drivingMapRandomPool.size(); ++i) {
        procAddDrivingLinks(drivingMapRandomPool[i], iter->second,nodeLookupRandomPool[i], drivingLinkLookupRandomPool[i], drivingLinkVertexLookupRandomPool[i], getEdgeWeight(iter->second,Random));
        }
    }

    //Now add all Intersection edges (link connectors)
    for (NodeLookup::const_iterator it = nodeLookupDefault.begin(); it != nodeLookupDefault.end(); it++) {
//      procAddDrivingLinkConnectors(drivingMapMorningPeak, (it->first),nodeLookupMorningPeak);
//      procAddDrivingLinkConnectors(drivingMapEveningPeak, (it->first),nodeLookupEveningPeak);
//      procAddDrivingLinkConnectors(drivingMapNormalTime, (it->first),nodeLookupNormalTime);
        procAddDrivingLinkConnectors(drivingMapDefault, (it->first),nodeLookupDefault);
        procAddDrivingLinkConnectors(drivingMapHighwayBiasDistance, (it->first),nodeLookupHighwayBiasDistance);
//      procAddDrivingLinkConnectors(drivingMapHighwayBiasMorningPeak, (it->first), nodeLookupHighwayBiasMorningPeak);
//      procAddDrivingLinkConnectors(drivingMapHighwayBiasEveningPeak, (it->first), nodeLookupHighwayBiasEveningPeak);
//      procAddDrivingLinkConnectors(drivingMapHighwayBiasNormalTime, (it->first), nodeLookupHighwayBiasNormalTime);
        procAddDrivingLinkConnectors(drivingMapHighwayBiasDefault, (it->first), nodeLookupHighwayBiasDefault);
        for (size_t i = 0; i < drivingMapRandomPool.size(); ++i) {
            procAddDrivingLinkConnectors(drivingMapRandomPool[i], (it->first), nodeLookupRandomPool[i]);
        }
    }

//  procAddStartNodesAndEdges(drivingMapMorningPeak, nodeLookupMorningPeak, &drivingNodeLookupMorningPeak);
//  procAddStartNodesAndEdges(drivingMapEveningPeak, nodeLookupEveningPeak, &drivingNodeLookupEveningPeak);
//  procAddStartNodesAndEdges(drivingMapNormalTime, nodeLookupNormalTime, &drivingNodeLookupNormalTime);
    procAddStartNodesAndEdges(drivingMapDefault, nodeLookupDefault, &drivingNodeLookupDefault);
    procAddStartNodesAndEdges(drivingMapHighwayBiasDistance, nodeLookupHighwayBiasDistance, &drivingNodeLookupHighwayBiasDistance);
//  procAddStartNodesAndEdges(drivingMapHighwayBiasMorningPeak, nodeLookupHighwayBiasMorningPeak, &drivingNodeLookupHighwayBiasMorningPeak);
//  procAddStartNodesAndEdges(drivingMapHighwayBiasEveningPeak, nodeLookupHighwayBiasEveningPeak, &drivingNodeLookupHighwayBiasEveningPeak);
//  procAddStartNodesAndEdges(drivingMapHighwayBiasNormalTime, nodeLookupHighwayBiasNormalTime, &drivingNodeLookupHighwayBiasNormalTime);
    procAddStartNodesAndEdges(drivingMapHighwayBiasDefault, nodeLookupHighwayBiasDefault, &drivingNodeLookupHighwayBiasDefault);
    for (size_t i = 0; i < drivingMapRandomPool.size(); ++i) {
        procAddStartNodesAndEdges(drivingMapRandomPool[i], nodeLookupRandomPool[i], &drivingNodeLookupRandomPool[i]);
    }
}

double A_StarShortestTravelTimePathImpl::getEdgeWeight(const sim_mob::Link* link, sim_mob::TimeRange timeRange)
{
    const TravelTimeManager* ttMgr = TravelTimeManager::getInstance();
    double highwayBias = PathSetParam::getInstance()->getHighwayBias();
    double edgeWeight = std::numeric_limits<double>::max();
    switch (timeRange)
    {
//  case sim_mob::MorningPeak:
//  {
//      edgeWeight = sim_mob::PathSetParam::getInstance()->getSegRangeTT(rs, "Car", morningPeakStartTime, morningPeakEndTime);
//      break;
//  }
//  case sim_mob::EveningPeak:
//  {
//      edgeWeight = PathSetParam::getInstance()->getSegRangeTT(rs, "Car", eveningPeakStartTime, eveningPeakEndTime);
//      break;
//  }
//  case sim_mob::OffPeak:
//  {
//      double key1 = PathSetParam::getInstance()->getSegRangeTT(rs, "Car", offPeak1StartTime, morningPeakStartTime); //Off-peak 1
//      double key2 = PathSetParam::getInstance()->getSegRangeTT(rs, "Car", morningPeakEndTime, eveningPeakStartTime); //Off-peak 2
//      double key3 = PathSetParam::getInstance()->getSegRangeTT(rs, "Car", eveningPeakEndTime, offPeak3EndTime); //Off-peak 3
//      edgeWeight = (key1 + key2 + key3) / 3.0;
//      break;
//  }
    case sim_mob::Default:
    {
        edgeWeight = ttMgr->getDefaultLinkTT(link);
        break;
    }
    case sim_mob::HighwayBiasDistance:
    {
        edgeWeight = link->getLength();
        if (link->getLinkType() != LinkType::LINK_TYPE_EXPRESSWAY)
        {
            edgeWeight = edgeWeight / highwayBias;//if not highway, increase edge weight
        }
        break;
    }
//  case sim_mob::HighwayBiasMorningPeak:
//  {
//      edgeWeight = sim_mob::PathSetParam::getInstance()->getSegRangeTT(rs, "Car", morningPeakStartTime, morningPeakEndTime);
//      if (!link->getLinkType() != LinkType::LINK_TYPE_EXPRESSWAY)
//      {
//          edgeWeight = edgeWeight / highwayBias;
//      } //if not highway, increase edge weight
//      break;
//  }
//  case sim_mob::HighwayBiasEveningPeak:
//  {
//      edgeWeight = PathSetParam::getInstance()->getSegRangeTT(rs, "Car", eveningPeakStartTime, eveningPeakEndTime);
//      if (!link->getLinkType() != LinkType::LINK_TYPE_EXPRESSWAY)
//      {
//          edgeWeight = edgeWeight / highwayBias;
//      } //if not highway, increase edge weight
//      break;
//  }
//  case sim_mob::HighwayBiasOffPeak:
//  {
//      double key1 = PathSetParam::getInstance()->getSegRangeTT(rs, "Car", offPeak1StartTime, morningPeakStartTime); //Off-peak 1
//      double key2 = PathSetParam::getInstance()->getSegRangeTT(rs, "Car", morningPeakEndTime, eveningPeakStartTime); //Off-peak 2
//      double key3 = PathSetParam::getInstance()->getSegRangeTT(rs, "Car", eveningPeakEndTime, offPeak3EndTime); //Off-peak 3
//      edgeWeight = (key1 + key2 + key3) / 3.0;
//      if (link->getLinkType() != LinkType::LINK_TYPE_EXPRESSWAY)
//      {
//          edgeWeight = edgeWeight / highwayBias;
//      } //if not highway, increase edge weight
//      break;
//  }
    case sim_mob::HighwayBiasDefault:
    {
        edgeWeight = ttMgr->getDefaultLinkTT(link);
        if (link->getLinkType() != LinkType::LINK_TYPE_EXPRESSWAY)
        {
            edgeWeight = edgeWeight / highwayBias;
        } //if not highway, increase edge weight
        break;
    }
    case sim_mob::Random:
    {
        boost::random_device seedGen;
        boost::mt19937 rng(seedGen());
        const std::pair<int, int> &range = sim_mob::ConfigManager::GetInstance().FullConfig().getPathSetConf().perturbationRange;
        boost::uniform_int<> uniformInt(range.first, range.second);
        boost::variate_generator<boost::mt19937, boost::uniform_int<> > rollDice(rng, uniformInt);
        edgeWeight = rollDice() * ttMgr->getDefaultLinkTT(link); //randomNo * defTT
        if (edgeWeight <= 0)
        {
            std::stringstream out("");
            out << "Invalid random perturbation key for segment " << link->getLinkId();
            throw std::runtime_error(out.str());
        }
        break;
    }
    default:
    {
        throw std::runtime_error("edge weight requested for unsupported graph type");
    }
    }
    return edgeWeight;
}

StreetDirectory::VertexDesc A_StarShortestTravelTimePathImpl::DrivingVertex(const sim_mob::Node& node, TimeRange timeRange, int randomGraphIdx) const
{
    StreetDirectory::VertexDesc res;
    switch (timeRange)
        {
        case Default:
            res = DrivingVertexDefault(node);
            break;
        case HighwayBiasDefault:
            res = DrivingVertexHighwayBiasDefault(node);
            break;
        case HighwayBiasDistance:
            res = DrivingVertexHighwayBiasDistance(node);
            break;
        case Random:
            res = DrivingVertexRandom(node, randomGraphIdx);
            break;
        }
        return res;
}
StreetDirectory::VertexDesc A_StarShortestTravelTimePathImpl::DrivingVertex(const sim_mob::Link& link, TimeRange timeRange, int randomGraphIdx) const
{
    StreetDirectory::VertexDesc res;
    switch (timeRange)
    {
        case Default:
            res = DrivingVertexDefault(link);
            break;
        case HighwayBiasDefault:
            res = DrivingVertexHighwayBiasDefault(link);
            break;
        case HighwayBiasDistance:
            res = DrivingVertexHighwayBiasDistance(link);
            break;
        case Random:
            res = DrivingVertexRandom(link, randomGraphIdx);
            break;
        }
        return res;
}
//StreetDirectory::VertexDesc A_StarShortestTravelTimePathImpl::DrivingVertexMorningPeak(const sim_mob::Node& n) const
//{
//  StreetDirectory::VertexDesc res;
//  NodeVertexLookup::const_iterator vertexIt = drivingNodeLookupMorningPeak.find(&n);
//  if (vertexIt != drivingNodeLookupMorningPeak.end()) {
//      res.valid = true;
//      res.source = vertexIt->second.first;
//      res.sink = vertexIt->second.second;
//      return res;
//  }
//  return res;
//}
//StreetDirectory::VertexDesc A_StarShortestTravelTimePathImpl::DrivingVertexEveningPeak(const sim_mob::Node& n) const
//{
//  StreetDirectory::VertexDesc res;
//  NodeVertexLookup::const_iterator vertexIt = drivingNodeLookupEveningPeak.find(&n);
//  if (vertexIt != drivingNodeLookupEveningPeak.end()) {
//      res.valid = true;
//      res.source = vertexIt->second.first;
//      res.sink = vertexIt->second.second;
//      return res;
//  }
//  return res;
//}
//StreetDirectory::VertexDesc A_StarShortestTravelTimePathImpl::DrivingVertexNormalTime(const sim_mob::Node& n) const
//{
//  StreetDirectory::VertexDesc res;
//  NodeVertexLookup::const_iterator vertexIt = drivingNodeLookupNormalTime.find(&n);
//  if (vertexIt != drivingNodeLookupNormalTime.end()) {
//      res.valid = true;
//      res.source = vertexIt->second.first;
//      res.sink = vertexIt->second.second;
//      return res;
//  }
//  return res;
//}
StreetDirectory::VertexDesc A_StarShortestTravelTimePathImpl::DrivingVertexDefault(const sim_mob::Node& n) const
{
    StreetDirectory::VertexDesc res;
    NodeVertexLookup::const_iterator vertexIt = drivingNodeLookupDefault.find(&n);
        if (vertexIt != drivingNodeLookupDefault.end())
        {
        res.valid = true;
        res.source = vertexIt->second.first;
        res.sink = vertexIt->second.second;
        return res;
    }
        return res;
}

StreetDirectory::VertexDesc A_StarShortestTravelTimePathImpl::DrivingVertexDefault(const Link &link) const
{
        StreetDirectory::VertexDesc res;
        LinkVertexLookup::const_iterator vertexIt = drivingLinkVertexLookupDefault.find(&link);
        if (vertexIt != drivingLinkVertexLookupDefault.end())
        {
            res.valid = true;
            res.source = vertexIt->second.first;
            res.sink = vertexIt->second.second;
            return res;
        }
        return res;
}

StreetDirectory::VertexDesc A_StarShortestTravelTimePathImpl::DrivingVertexHighwayBiasDistance(const sim_mob::Node& n) const
{
    StreetDirectory::VertexDesc res;
    NodeVertexLookup::const_iterator vertexIt = drivingNodeLookupHighwayBiasDistance.find(&n);
        if (vertexIt != drivingNodeLookupHighwayBiasDistance.end())
        {
        res.valid = true;
        res.source = vertexIt->second.first;
        res.sink = vertexIt->second.second;
        return res;
    }
        return res;
}

StreetDirectory::VertexDesc A_StarShortestTravelTimePathImpl::DrivingVertexHighwayBiasDistance(const Link &link) const
{
        StreetDirectory::VertexDesc res;
        LinkVertexLookup::const_iterator vertexIt = drivingLinkVertexLookupHighwayBiasDistance.find(&link);
        if (vertexIt != drivingLinkVertexLookupHighwayBiasDistance.end())
        {
            res.valid = true;
            res.source = vertexIt->second.first;
            res.sink = vertexIt->second.second;
            return res;
        }
        return res;
}
//StreetDirectory::VertexDesc A_StarShortestTravelTimePathImpl::DrivingVertexHighwayBiasMorningPeak(const sim_mob::Node& n) const
//{
//  StreetDirectory::VertexDesc res;
//  NodeVertexLookup::const_iterator vertexIt = drivingNodeLookupHighwayBiasMorningPeak.find(&n);
//  if (vertexIt != drivingNodeLookupHighwayBiasMorningPeak.end()) {
//      res.valid = true;
//      res.source = vertexIt->second.first;
//      res.sink = vertexIt->second.second;
//      return res;
//  }
//
//  return res;
//}
//StreetDirectory::VertexDesc A_StarShortestTravelTimePathImpl::DrivingVertexHighwayBiasEveningPeak(const sim_mob::Node& n) const
//{
//  StreetDirectory::VertexDesc res;
//  NodeVertexLookup::const_iterator vertexIt = drivingNodeLookupHighwayBiasEveningPeak.find(&n);
//  if (vertexIt != drivingNodeLookupHighwayBiasEveningPeak.end()) {
//      res.valid = true;
//      res.source = vertexIt->second.first;
//      res.sink = vertexIt->second.second;
//      return res;
//  }
//
//  return res;
//}
//StreetDirectory::VertexDesc A_StarShortestTravelTimePathImpl::DrivingVertexHighwayBiasNormalTIme(const sim_mob::Node& n) const
//{
//  StreetDirectory::VertexDesc res;
//  NodeVertexLookup::const_iterator vertexIt = drivingNodeLookupHighwayBiasNormalTime.find(&n);
//  if (vertexIt != drivingNodeLookupHighwayBiasNormalTime.end()) {
//      res.valid = true;
//      res.source = vertexIt->second.first;
//      res.sink = vertexIt->second.second;
//      return res;
//  }
//  return res;
//}
StreetDirectory::VertexDesc A_StarShortestTravelTimePathImpl::DrivingVertexHighwayBiasDefault(const sim_mob::Node& n) const
{
    StreetDirectory::VertexDesc res;
    NodeVertexLookup::const_iterator vertexIt = drivingNodeLookupHighwayBiasDefault.find(&n);
        if (vertexIt != drivingNodeLookupHighwayBiasDefault.end())
        {
        res.valid = true;
        res.source = vertexIt->second.first;
        res.sink = vertexIt->second.second;
        return res;
    }
        return res;
}

StreetDirectory::VertexDesc A_StarShortestTravelTimePathImpl::DrivingVertexHighwayBiasDefault(const sim_mob::Link &link) const
{
        StreetDirectory::VertexDesc res;
        LinkVertexLookup::const_iterator vertexIt = drivingLinkVertexLookupHighwayBiasDefault.find(&link);
        if (vertexIt != drivingLinkVertexLookupHighwayBiasDefault.end())
        {
            res.valid = true;
            res.source = vertexIt->second.first;
            res.sink = vertexIt->second.second;
            return res;
        }
        return res;
}

StreetDirectory::VertexDesc A_StarShortestTravelTimePathImpl::DrivingVertexRandom(const sim_mob::Node& node, unsigned int randomGraphId) const
{
    StreetDirectory::VertexDesc res;
    if (randomGraphId < drivingNodeLookupRandomPool.size()) {
        NodeVertexLookup::const_iterator vertexIt = drivingNodeLookupRandomPool[randomGraphId].find(&node);
        if (vertexIt != drivingNodeLookupRandomPool[randomGraphId].end())
        {
            res.valid = true;
            res.source = vertexIt->second.first;
            res.sink = vertexIt->second.second;
            return res;
        }
    }
        return res;
}

StreetDirectory::VertexDesc A_StarShortestTravelTimePathImpl::DrivingVertexRandom(const sim_mob::Link &link, unsigned int randomGraphId) const
{
        StreetDirectory::VertexDesc res;
        if (randomGraphId < drivingLinkVertexLookupRandomPool.size())
        {
            LinkVertexLookup::const_iterator vertexIt = drivingLinkVertexLookupRandomPool[randomGraphId].find(&link);
            if (vertexIt != drivingLinkVertexLookupRandomPool[randomGraphId].end())
            {
                    res.valid = true;
                    res.source = vertexIt->second.first;
                    res.sink = vertexIt->second.second;
                    return res;
            }
        }
    return res;
}

std::vector<WayPoint> A_StarShortestTravelTimePathImpl::searchShortestTTPathWithBlackList(const StreetDirectory::Graph& graph, const StreetDirectory::Vertex& fromVertex,
                                                                              const StreetDirectory::Vertex& toVertex,
                                                                              const std::set<StreetDirectory::Edge>& blacklist)
{
    //Lock for read access.
    boost::shared_lock<boost::shared_mutex> lock(GraphSearchMutex);

    //Filter it.
    BlackListEdgeConstraint filter(blacklist);
    boost::filtered_graph<StreetDirectory::Graph, BlackListEdgeConstraint> filtered(graph, filter);

    vector<WayPoint> res;
    std::list<StreetDirectory::Vertex> partialRes;
    //Output variable for A* searching
    vector<StreetDirectory::Vertex> p(boost::num_vertices(filtered));
    //Output variable for A* searching
    vector<double> d(boost::num_vertices(filtered));

    //Use A* to search for a path
    try
    {
        boost::astar_search(filtered, fromVertex, A_StarShortestTravelTimePathImpl::DistanceHeuristicFiltered(&filtered, toVertex),
                            boost::predecessor_map(&p[0]).distance_map(&d[0]).visitor(GoalVisitor(toVertex)));
    }
    catch (Goal& goal)
    {
        //Build backwards.
        for (StreetDirectory::Vertex v = toVertex;; v = p[v])
        {
            partialRes.push_front(v);
            if (p[v] == v)
            {
                break;
            }
        }

        //Now build forwards.
        std::list<StreetDirectory::Vertex>::const_iterator prev = partialRes.end();
        for (std::list<StreetDirectory::Vertex>::const_iterator it = partialRes.begin(); it != partialRes.end(); it++)
        {
            //Add this edge.
            if (prev != partialRes.end())
            {
                //This shouldn't fail.
                std::pair < StreetDirectory::Edge, bool> edge = boost::edge(*prev, *it, filtered);
                if (!edge.second)
                {
                    Warn() << "ERROR: Boost can't find an edge that it should know about."
                            << std::endl;
                    return std::vector<WayPoint>();
                }

                //Retrieve, add this edge's WayPoint.
                WayPoint wp = boost::get(boost::edge_name, filtered, edge.first);
                res.push_back(wp);
            }

            //Save for later.
            prev = it;
        }
    }

    return res;
}

vector<WayPoint> A_StarShortestTravelTimePathImpl::searchShortestTTPath(const StreetDirectory::Graph& graph, const StreetDirectory::Vertex& fromVertex,
                                                            const StreetDirectory::Vertex& toVertex)
{
    vector<WayPoint> res;
    std::list<StreetDirectory::Vertex> partialRes;

    //Lock for read access.
    boost::shared_lock<boost::shared_mutex> lock(GraphSearchMutex);
    //Output variable for A* searching
    vector<StreetDirectory::Vertex> p(boost::num_vertices(graph));
    //Output variable for A* searching
    vector<double> d(boost::num_vertices(graph));

    //Use A* to search for a path
    try
    {
        boost::astar_search(graph, fromVertex, A_StarShortestTravelTimePathImpl::DistanceHeuristicGraph(&graph, toVertex),
                            boost::predecessor_map(&p[0]).distance_map(&d[0]).visitor(GoalVisitor(toVertex)));
    }
    catch (Goal& goal)
    {
        //Build backwards.
        for (StreetDirectory::Vertex v = toVertex;; v = p[v])
        {
            partialRes.push_front(v);
            if (p[v] == v)
            {
                break;
            }
        }

        //Now build forwards.
        std::list<StreetDirectory::Vertex>::const_iterator prev = partialRes.end();
        for (std::list<StreetDirectory::Vertex>::const_iterator it = partialRes.begin(); it != partialRes.end(); it++)
        {
            //Add this edge.
            if (prev != partialRes.end())
            {
                //This shouldn't fail.
                std::pair < StreetDirectory::Edge, bool> edge = boost::edge(*prev, *it, graph);
                if (!edge.second)
                {
                    Warn() << "ERROR: Boost can't find an edge that it should know about."
                            << std::endl;
                    return std::vector<WayPoint>();
                }

                //Retrieve, add this edge WayPoint.
                WayPoint wp = boost::get(boost::edge_name, graph, edge.first);
                res.push_back(wp);
            }

            //Save for later.
            prev = it;
        }
    }

    return res;
}

vector<sim_mob::WayPoint> A_StarShortestTravelTimePathImpl::GetShortestDrivingPath(
        const StreetDirectory::VertexDesc& from, const StreetDirectory::VertexDesc& to,
        const vector<const sim_mob::Link*>& blacklist, TimeRange tmRange,int randomGraphId) const
{
    if (!(from.valid && to.valid))
    {
        return vector<sim_mob::WayPoint>();
    }

    StreetDirectory::Vertex fromV = from.source;
    StreetDirectory::Vertex toV = to.sink;
    if (fromV == toV)
    {
        return vector<sim_mob::WayPoint>();
    }

    //Convert the blacklist into a list of blocked Vertices.
    set<StreetDirectory::Edge> blacklistV;
    for (vector<const sim_mob::Link*>::const_iterator it = blacklist.begin(); it != blacklist.end(); it++)
    {
//      if (tmRange == MorningPeak)
//      {
//          LinkEdgeLookup::const_iterator lookIt = drivingLinkLookupMorningPeak.find(*it);
//          if (lookIt != drivingLinkLookupMorningPeak.end())
//          {
//              blacklistV.insert(lookIt->second.begin(), lookIt->second.end());
//          }
//      }
//      else if (tmRange == EveningPeak)
//      {
//          LinkEdgeLookup::const_iterator lookIt = drivingLinkLookupEveningPeak.find(*it);
//          if (lookIt != drivingLinkLookupEveningPeak.end())
//          {
//              blacklistV.insert(lookIt->second.begin(), lookIt->second.end());
//          }
//      }
//      else if (tmRange == OffPeak)
//      {
//          LinkEdgeLookup::const_iterator lookIt = drivingLinkLookupNormalTime.find(*it);
//          if (lookIt != drivingLinkLookupNormalTime.end())
//          {
//              blacklistV.insert(lookIt->second.begin(), lookIt->second.end());
//          }
//      }
//      else
        if (tmRange == Default)
        {
            LinkEdgeLookup::const_iterator lookIt = drivingLinkLookupDefault.find(*it);
            if (lookIt != drivingLinkLookupDefault.end())
            {
                blacklistV.insert(lookIt->second.begin(), lookIt->second.end());
            }
        }
        else if (tmRange == HighwayBiasDistance)
        {
            LinkEdgeLookup::const_iterator lookIt = drivingLinkLookupHighwayBiasDistance.find(*it);
            if (lookIt != drivingLinkLookupHighwayBiasDistance.end())
            {
                blacklistV.insert(lookIt->second.begin(), lookIt->second.end());
            }
        }
        else
//      if (tmRange == HighwayBiasMorningPeak)
//      {
//          LinkEdgeLookup::const_iterator lookIt = drivingLinkLookupHighwayBiasMorningPeak.find(*it);
//          if (lookIt != drivingLinkLookupHighwayBiasMorningPeak.end())
//          {
//              blacklistV.insert(lookIt->second.begin(), lookIt->second.end());
//          }
//      }
//      else if (tmRange == HighwayBiasEveningPeak)
//      {
//          LinkEdgeLookup::const_iterator lookIt = drivingLinkLookupHighwayBiasEveningPeak.find(*it);
//          if (lookIt != drivingLinkLookupHighwayBiasEveningPeak.end())
//          {
//              blacklistV.insert(lookIt->second.begin(), lookIt->second.end());
//          }
//      }
//      else if (tmRange == HighwayBiasOffPeak)
//      {
//          LinkEdgeLookup::const_iterator lookIt = drivingLinkLookupHighwayBiasNormalTime.find(*it);
//          if (lookIt != drivingLinkLookupHighwayBiasNormalTime.end())
//          {
//              blacklistV.insert(lookIt->second.begin(), lookIt->second.end());
//          }
//      }
//      else
        if (tmRange == HighwayBiasDefault)
        {
            LinkEdgeLookup::const_iterator lookIt = drivingLinkLookupHighwayBiasDefault.find(*it);
            if (lookIt != drivingLinkLookupHighwayBiasDefault.end())
            {
                blacklistV.insert(lookIt->second.begin(), lookIt->second.end());
            }
        }
        else if (tmRange == Random)
        {
            if (randomGraphId >= drivingMapRandomPool.size())
            {
                randomGraphId = 0;
            }
            if (drivingMapRandomPool.size() == 0)
            {
                return vector<sim_mob::WayPoint>();
            }
            LinkEdgeLookup::const_iterator lookIt = drivingLinkLookupRandomPool[randomGraphId].find(*it);
            if (lookIt != drivingLinkLookupRandomPool[randomGraphId].end())
            {
                blacklistV.insert(lookIt->second.begin(), lookIt->second.end());
            }
        }
    }

    if (blacklistV.empty())
    {
//      if (tmRange == MorningPeak)
//      {
//          return searchShortestPath(drivingMapMorningPeak, fromV, toV);
//      }
//      else if (tmRange == EveningPeak)
//      {
//          return searchShortestPath(drivingMapEveningPeak, fromV, toV);
//      }
//      else if (tmRange == OffPeak)
//      {
//          return searchShortestPath(drivingMapNormalTime, fromV, toV);
//      }
//      else
        if (tmRange == Default)
        {
            return searchShortestTTPath(drivingMapDefault, fromV, toV);
        }
        else if (tmRange == HighwayBiasDistance)
        {
            return searchShortestTTPath(drivingMapHighwayBiasDistance, fromV, toV);
        }
        else
//      if (tmRange == HighwayBiasMorningPeak)
//      {
//          return searchShortestPath(drivingMapHighwayBiasMorningPeak, fromV, toV);
//      }
//      else if (tmRange == HighwayBiasEveningPeak)
//      {
//          return searchShortestPath(drivingMapHighwayBiasEveningPeak, fromV, toV);
//      }
//      else if (tmRange == HighwayBiasOffPeak)
//      {
//          return searchShortestPath(drivingMapHighwayBiasNormalTime, fromV, toV);
//      }
//      else
        if (tmRange == HighwayBiasDefault)
        {
            return searchShortestTTPath(drivingMapHighwayBiasDefault, fromV, toV);
        }
        else if (tmRange == Random)
        {
            if (randomGraphId >= drivingMapRandomPool.size())
            {
                return vector<sim_mob::WayPoint>();
            }
            return searchShortestTTPath(drivingMapRandomPool[randomGraphId], fromV, toV);
        }
        else
        {
            throw std::runtime_error("A_StarShortestTravelTimePathImpl: unknown time range");
        }
    }
    else
    {
//      if (tmRange == MorningPeak)
//      {
//          return searchShortestPathWithBlackList(drivingMapMorningPeak, fromV, toV, blacklistV);
//      }
//      else if (tmRange == EveningPeak)
//      {
//          return searchShortestPathWithBlackList(drivingMapEveningPeak, fromV, toV, blacklistV);
//      }
//      else if (tmRange == OffPeak)
//      {
//          return searchShortestPathWithBlackList(drivingMapNormalTime, fromV, toV, blacklistV);
//      }
//      else
        if (tmRange == Default)
        {
            return searchShortestTTPathWithBlackList(drivingMapDefault, fromV, toV, blacklistV);
        }
        else if (tmRange == HighwayBiasDistance)
        {
            return searchShortestTTPathWithBlackList(drivingMapHighwayBiasDistance, fromV, toV, blacklistV);
        }
        else
//      if (tmRange == HighwayBiasMorningPeak)
//      {
//          return searchShortestPathWithBlackList(drivingMapHighwayBiasMorningPeak, fromV, toV, blacklistV);
//      }
//      else if (tmRange == HighwayBiasEveningPeak)
//      {
//          return searchShortestPathWithBlackList(drivingMapHighwayBiasEveningPeak, fromV, toV, blacklistV);
//      }
//      else if (tmRange == HighwayBiasOffPeak)
//      {
//          return searchShortestPathWithBlackList(drivingMapHighwayBiasNormalTime, fromV, toV, blacklistV);
//      }
//      else
        if (tmRange == HighwayBiasDefault)
        {
            return searchShortestTTPathWithBlackList(drivingMapHighwayBiasDefault, fromV, toV, blacklistV);
        }
        else if (tmRange == Random)
        {
            if (randomGraphId >= drivingMapRandomPool.size())
            {
                return vector<sim_mob::WayPoint>();
            }
            return searchShortestTTPathWithBlackList(drivingMapRandomPool[randomGraphId], fromV, toV, blacklistV);
        }
        else
        {
            throw std::runtime_error("A_StarShortestTravelTimePathImpl: unknown time range2");
        }
    }
}
}