// "Copyright [year] <Copyright Owner>"
#include <ros/ros.h>
#include <tf/transform_datatypes.h>
#include "doogie_msgs/DoogiePose.h"

#include <cmath>
#include <string>

#include "doogie_navigation/move_in_maze.hpp"
#include <boost/geometry/algorithms/distance.hpp>

namespace doogie_navigation {
using Pose = doogie_navigation::MazePose;
using GlobalOrientation = doogie::GlobalOrientation;
using Direction = doogie::Direction;

MoveInMaze::MoveInMaze(const ros::NodeHandle& robot_nh)
: MoveBase::MoveBase(robot_nh)
, move_base_action_server(nh_, "move_base_action_server", false) {
  move_base_action_server.registerGoalCallback(boost::bind(&MoveInMaze::receiveGoalCallback, this));
  move_base_action_server.start();

  pose_pub_ = nh_.advertise<doogie_msgs::DoogiePose>("doogie_pose", 5);
  robot_pose_ = Pose{params_.row_init_position, params_.row_init_position, GlobalOrientation::NORTH};
  initializeCommandMap();
}

void MoveInMaze::initializeCommandMap() {
  command_map[GlobalOrientation::NORTH] =  std::bind(&MoveInMaze::sendMoveToNorthCommand, this, std::placeholders::_1);
  command_map[GlobalOrientation::SOUTH] =  std::bind(&MoveInMaze::sendMoveToSouthCommand, this, std::placeholders::_1);
  command_map[GlobalOrientation::EAST]  =  std::bind(&MoveInMaze::sendMoveToEastCommand, this, std::placeholders::_1);
  command_map[GlobalOrientation::WEST]  =  std::bind(&MoveInMaze::sendMoveToWestCommand, this, std::placeholders::_1);
}

void MoveInMaze::receiveGoalCallback() {
  goal_ = move_base_action_server.acceptNewGoal();
  auto goal_direction = goal_.getDirection();
  ROS_DEBUG_STREAM("goal received");

  if (goal_direction == Direction::FRONT) {
    robot_state_ = State::MOVING;
    return;
  }

  robot_state_ = State::TURNNING;
}

void MoveInMaze::moveStraight(int8_t number_of_cells) {
  auto distance_to_move = params_.cell_size * number_of_cells;
  auto doogie_orientation = robot_pose_.getGlobalOrientation();
  command_map[doogie_orientation](distance_to_move);
}

void MoveInMaze::sendMoveToNorthCommand(double distance_to_move) {
  double goal_position = localization_iface_->getCurrentXPosition() + distance_to_move;
  moveHeadingXAxis(goal_position, Heading::POSITIVE);
}

void MoveInMaze::sendMoveToSouthCommand(double distance_to_move) {
  double goal_position = localization_iface_->getCurrentXPosition() - distance_to_move;
  moveHeadingXAxis(goal_position, Heading::NEGATIVE);
}

void MoveInMaze::sendMoveToEastCommand(double distance_to_move) {
  double goal_position = localization_iface_->getCurrentYPosition() - distance_to_move;
  moveHeadingYAxis(goal_position, Heading::NEGATIVE);
}

void MoveInMaze::sendMoveToWestCommand(double distance_to_move) {
  double goal_position = localization_iface_->getCurrentYPosition() + distance_to_move;
  moveHeadingYAxis(goal_position, Heading::POSITIVE);
}

void MoveInMaze::updatePosition(int cells) {
  auto global_orientation = robot_pose_.getGlobalOrientation();
  switch (global_orientation) {
    case GlobalOrientation::NORTH:
      robot_pose_.incrementRow(cells);
      robot_pose_.setGlobalOrientation(GlobalOrientation::NORTH);
      break;
    case GlobalOrientation::SOUTH:
      robot_pose_.decrementRow(cells);
      robot_pose_.setGlobalOrientation(GlobalOrientation::SOUTH);
      break;
    case GlobalOrientation::EAST:
      robot_pose_.incrementColumn(cells);
      robot_pose_.setGlobalOrientation(GlobalOrientation::EAST);
      break;
    case GlobalOrientation::WEST:
      robot_pose_.decrementColumn(cells);
      robot_pose_.setGlobalOrientation(GlobalOrientation::WEST);
      break;
  }
  auto doogie_pose = robot_pose_.toDoogieMsg();
  ROS_INFO_STREAM("NAVIGATION: Updating Doogie Position to " << doogie_pose.position);
  pose_pub_.publish(doogie_pose);
}

void MoveInMaze::rotate(Direction goal_direction) {
  auto current_orientation = (robot_pose_.getGlobalOrientation() -1) * doogie::PI/2;
  double goal_angle = current_orientation + goal_direction*(doogie::PI/2);
  ROS_INFO_STREAM("NAVIGATION: Current Orientation " << current_orientation);
  ROS_INFO_STREAM("NAVIGATION: Goal Direction " << goal_direction);
  ROS_INFO_STREAM("NAVIGATION: Rotating " << goal_angle);

  turnRobot(-goal_angle);
}

void MoveInMaze::updateOrientation(Direction goal_direction) {
  robot_pose_.transformOrientation(goal_direction);
  auto doogie_pose = robot_pose_.toDoogieMsg();
  ROS_INFO_STREAM("NAVIGATION: Update Orientation to " << (int)doogie_pose.orientation.direction);
  pose_pub_.publish(doogie_pose);
}

void MoveInMaze::start() {
  ROS_DEBUG_STREAM("Start");
  ros::Rate rate{params_.loop_frequency};

  ROS_INFO("Move base node has started");
  while (ros::ok()) {
    if (!goal_.empty()) {

      if (robot_state_ == State::IDLE) {
        ROS_INFO_STREAM("IDLE");
        motion_iface_.stop();
        continue;
      }

      if (robot_state_ == State::TURNNING) {
        ROS_INFO_STREAM("Goal is Rotate");
        rotate(goal_.getDirection());
        motion_iface_.stop();
        updateOrientation(goal_.getDirection());
        robot_state_ = State::MOVING;
        continue;
      }

      if (robot_state_ == State::MOVING) {
        ROS_INFO_STREAM("Goal is Move Straight");
        moveStraight(goal_.getNumberOfCells());
        motion_iface_.stop();
        updatePosition(goal_.getNumberOfCells());
        goal_.finish();
        move_base_action_server.setSucceeded(goal_.getResult());
        robot_state_ = State::IDLE;
        continue;
      }
    }
    motion_iface_.publishVelocityMessage();

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

void MoveInMaze::spin() {
  ros::Rate rate(params_.loop_frequency);
  ros::spinOnce();
  rate.sleep();
}

}  // namespace doogie_navigation


#pragma once

#include <boost/multiprecision/cpp_int.hpp>

#include <cryptopp/osrng.h>

namespace rai
{
using stream = std::basic_streambuf<uint8_t>;
// Random pool used by RaiBlocks.
// This must be thread_local as long as the AutoSeededRandomPool implementation requires it
extern thread_local CryptoPP::AutoSeededRandomPool random_pool;

using uint32_t = ::uint32_t;
using uint64_t = ::uint64_t;
using uint128_t = boost::multiprecision::uint128_t;
using uint256_t = boost::multiprecision::uint256_t;
using uint512_t = boost::multiprecision::uint512_t;

// Used for timestamp
struct uint32_struct
{
public:
	uint32_struct () = default;
	uint32_struct (uint32_t);
	uint32_struct (std::string const &);
	bool operator== (rai::uint32_struct const &) const;
	void encode_dec (std::string &) const;
	bool decode_dec (std::string const &);
	std::string to_string_dec () const;
	bool is_zero () const;
	void clear ();
	rai::uint32_t number () const;
	rai::uint32_t number_big_endian () const;
	void serialize (rai::stream & stream_a) const;
	bool deserialize (rai::stream & stream_a);
	// wrapped storage is a native int type, not an always-big-endian byte array
	::uint32_t data;
};

// Used for amounts
struct uint64_struct
{
public:
	uint64_struct () = default;
	uint64_struct (std::string const &);
	uint64_struct (rai::uint64_t);
	uint64_struct (rai::uint64_struct const &) = default;
	bool operator== (rai::uint64_struct const &) const;
	bool operator!= (rai::uint64_struct const &) const;
	bool operator< (rai::uint64_struct const &) const;
	bool operator> (rai::uint64_struct const &) const;
	void encode_hex (std::string &) const;
	bool decode_hex (std::string const &);
	void encode_dec (std::string &) const;
	bool decode_dec (std::string const &);
	rai::uint64_t number () const;
	rai::uint64_t number_big_endian () const;
	void clear ();
	bool is_zero () const;
	std::string to_string () const;
	std::string to_string_dec () const;
	void serialize (rai::stream & stream_a) const;
	bool deserialize (rai::stream & stream_a);
	// wrapped storage is a native int type, not an always-big-endian byte array
	::uint64_t data;
};

using work = uint64_struct;

// Balances are 64 bit.
using amount_t = ::uint64_t;

struct amount : uint64_struct
{
public:
	amount () = default;
	amount (std::string const &);
	amount (rai::amount_t);
	amount (rai::amount const &) = default;
	std::string format_balance (rai::uint64_t scale, int precision, bool group_digits);
	std::string format_balance (rai::uint64_t scale, int precision, bool group_digits, const std::locale & locale);
};

// SI dividers
rai::amount_t const Gxrb_ratio = rai::amount_t(10000000000000); // 10^13  // TODO test-only
rai::amount_t const Mxrb_ratio = rai::amount_t(10000000000); // 10^10
rai::amount_t const xrb_ratio = rai::amount_t(10000); // 10^4

// Used in keys
union uint128_struct
{
public:
	uint128_struct () = default;
	//uint128_struct (std::string const &);
	//uint128_struct (uint64_t);
	//uint128_struct (rai::uint128_struct const &) = default;
	uint128_struct (rai::uint128_t const &);
	//bool operator== (rai::uint128_struct const &) const;
	//bool operator!= (rai::uint128_struct const &) const;
	//bool operator< (rai::uint128_struct const &) const;
	//bool operator> (rai::uint128_struct const &) const;
	//void encode_hex (std::string &) const;
	//bool decode_hex (std::string const &);
	void encode_dec (std::string &) const;
	bool decode_dec (std::string const &);
	//std::string format_balance (rai::uint64_t scale, int precision, bool group_digits);
	//std::string format_balance (rai::uint64_t scale, int precision, bool group_digits, const std::locale & locale);
	rai::uint128_t number () const;
	//void clear ();
	//bool is_zero () const;
	//std::string to_string () const;
	std::string to_string_dec () const;
	std::array<uint8_t, 16> bytes;
	//std::array<char, 16> chars;
	//std::array<uint32_t, 4> dwords;
	//std::array<uint64_t, 2> qwords;
};

class raw_key;
union uint256_union
{
	uint256_union () = default;
	uint256_union (std::string const &);
	uint256_union (uint64_t);
	uint256_union (rai::uint256_t const &);
	void encrypt (rai::raw_key const &, rai::raw_key const &, uint128_struct const &);
	uint256_union & operator^= (rai::uint256_union const &);
	uint256_union operator^ (rai::uint256_union const &) const;
	bool operator== (rai::uint256_union const &) const;
	bool operator!= (rai::uint256_union const &) const;
	bool operator< (rai::uint256_union const &) const;
	void encode_hex (std::string &) const;
	bool decode_hex (std::string const &);
	void encode_dec (std::string &) const;
	bool decode_dec (std::string const &);
	void encode_account (std::string &) const;
	std::string to_account () const;
	std::string to_account_split () const;
	bool decode_account (std::string const &);
	std::array<uint8_t, 32> bytes;
	std::array<char, 32> chars;
	std::array<uint32_t, 8> dwords;
	std::array<uint64_t, 4> qwords;
	std::array<uint128_struct, 2> owords;
	void clear ();
	bool is_zero () const;
	std::string to_string () const;
	rai::uint256_t number () const;
};
// All keys and hashes are 256 bit.
using block_hash = uint256_union;
using account = uint256_union;
using public_key = uint256_union;
using private_key = uint256_union;
using secret_key = uint256_union;
using checksum = uint256_union;
class raw_key
{
public:
	raw_key () = default;
	~raw_key ();
	void decrypt (rai::uint256_union const &, rai::raw_key const &, uint128_struct const &);
	bool operator== (rai::raw_key const &) const;
	bool operator!= (rai::raw_key const &) const;
	rai::uint256_union data;
};
union uint512_union
{
	uint512_union () = default;
	uint512_union (rai::uint512_t const &);
	bool operator== (rai::uint512_union const &) const;
	bool operator!= (rai::uint512_union const &) const;
	rai::uint512_union & operator^= (rai::uint512_union const &);
	void encode_hex (std::string &) const;
	bool decode_hex (std::string const &);
	std::array<uint8_t, 64> bytes;
	std::array<uint32_t, 16> dwords;
	std::array<uint64_t, 8> qwords;
	std::array<uint256_union, 2> uint256s;
	void clear ();
	rai::uint512_t number () const;
	std::string to_string () const;
};

// A variable length array of bytes, with 2-bytes length
class var_len_bytes16
{
public:
	var_len_bytes16 (std::vector<uint8_t> const &);
	var_len_bytes16 () = default;
	size_t length () const
	{
		return (size_t)len;
	}
	std::vector<uint8_t> const & value () const
	{
		return bytes;
	}
	void clear ();
	static const uint16_t max_length = 65535;
	// return hex represenatation
	std::string to_string () const;
	void encode_hex (std::string &) const;
	bool decode_hex (std::string const &);
	void serialize (rai::stream &) const;
	bool deserialize (rai::stream &);
	bool operator== (rai::var_len_bytes16 const &) const;

protected:
	uint16_t len;
	std::vector<uint8_t> bytes;
};

// A variable length UTF string (max len 65K), storage is var_len_bytes16
class var_len_string : public var_len_bytes16
{
public:
	var_len_string (std::string const &);
	var_len_string (std::string const &, size_t);
	var_len_string () = default;
	std::string value_string () const;
	void set_from_string (std::string const &, size_t);
	static std::string raw_to_string (std::vector<uint8_t> const &, uint16_t);
};

// Signatures are 512 bit.
using signature = uint512_union;
// Shott UTF-8 string
using short_string = uint512_union;

rai::uint512_union sign_message (rai::raw_key const &, rai::public_key const &, rai::uint256_union const &);
bool validate_message (rai::public_key const &, rai::uint256_union const &, rai::uint512_union const &);
void deterministic_key (rai::uint256_union const &, uint32_t, rai::uint256_union &);
rai::public_key pub_key (rai::private_key const &);
}

namespace std
{
template <>
struct hash<rai::uint256_union>
{
	size_t operator() (rai::uint256_union const & data_a) const
	{
		return *reinterpret_cast<size_t const *> (data_a.bytes.data ());
	}
};
template <>
struct hash<rai::uint256_t>
{
	size_t operator() (rai::uint256_t const & number_a) const
	{
		return number_a.convert_to<size_t> ();
	}
};
}


// Copyright 2015 National ICT Australia Limited (NICTA)
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "NetworkParser.h"

#include "Common.h"
#include "PowerFlow.h"
#include "Random.h"

#include <boost/config/warning_disable.hpp>
#include <boost/spirit/include/qi.hpp>
#include <boost/spirit/include/phoenix_operator.hpp>
#include <boost/spirit/include/phoenix_object.hpp>

namespace qi = boost::spirit::qi;
namespace ascii = boost::spirit::ascii;

namespace
{
    using boost::spirit::ascii::space;

    template <typename Iterator> struct Calculator : qi::grammar<Iterator, int(), ascii::space_type>
    {
        Calculator() : Calculator::base_type(expression)
    {
        qi::_val_type _val;
        qi::_1_type _1;
        qi::uint_type uint_;

        expression =
            term                            [_val = _1]
            >> *(   ('+' >> term            [_val += _1])
                    |   ('-' >> term            [_val -= _1])
                )
            ;

        term =
            factor                          [_val = _1]
            >> *(   ('*' >> factor          [_val *= _1])
                    |   ('/' >> factor          [_val /= _1])
                    |   ('%' >> factor          [_val = _val % _1])
                )
            ;

        factor =
            uint_                           [_val = _1]
            |   '(' >> expression           [_val = _1] >> ')'
            |   ('-' >> factor              [_val = -_1])
            |   ('+' >> factor              [_val = _1])
            ;
    }

    qi::rule<Iterator, int(), ascii::space_type> expression, term, factor;
    };

    Calculator<std::string::const_iterator> calc;
}

namespace Sgt
{
    void assertFieldPresent(const YAML::Node& nd, const std::string& field)
    {
        sgtAssert(nd[field], "Parsing: " << field << " field not present.");
    };

    YAML::Node getTopNode(const std::string& fname)
    {
        sgtLogMessage() << "Opening file " << fname << " for parsing." << std::endl;
        auto top = YAML::LoadFile(fname);
        sgtAssert(top.size() > 0, "File " << fname << " is empty or doesn't exist.");
        return top;
    }

    ParserBase::ParserLoop& ParserBase::parseLoop(const YAML::Node& nd)
    {
        assertFieldPresent(nd, "loop_variable");
        assertFieldPresent(nd, "loop_body");

        const YAML::Node& ndLoopVar = nd["loop_variable"];
        const YAML::Node& ndBody = nd["loop_body"];

        std::string name = ndLoopVar[0].as<std::string>();
        sgtAssert(ndLoopVar.size() == 4,
                "loop_variable expression is invalid. Format is [name, start, upper, stride].");
        int start = expand<int>(ndLoopVar[1]);
        int upper = expand<int>(ndLoopVar[2]);
        int stride = expand<int>(ndLoopVar[3]);
        loops_.push_back(std::unique_ptr<ParserLoop>(new ParserLoop{name, start, upper, stride, ndBody}));

        return *loops_.back();
    }

    void ParserBase::parseLogging(const YAML::Node& nd)
    {
        auto ndIndent = nd["indent"];
        if (ndIndent)
        {
            LogIndent::defaultTabWidth = ndIndent.as<unsigned int>();
        }

        for (const auto& subNd : nd)
        {
            std::string nodeType = subNd.first.as<std::string>();
            if (nodeType == "indent") continue;

            const YAML::Node& nodeVal = subNd.second;

            LogLevel* logLevel = nullptr;
            std::string* logFilter = nullptr;
            if (nodeType == "message")
            {
                logLevel = &messageLogLevel(); logFilter = &messageLogFilter();
            }
            else if (nodeType == "warning")
            {
                logLevel = &warningLogLevel(); logFilter = &warningLogFilter();
            }
            else if (nodeType == "error")
            {
                logLevel = &errorLogLevel(); logFilter = &errorLogFilter();
            }
            else if (nodeType == "debug")
            {
                logLevel = &debugLogLevel(); logFilter = &debugLogFilter();
            }

            const YAML::Node& ndLogLevel = nodeVal["log_level"];
            if (ndLogLevel)
            {
                auto lev = ndLogLevel.as<std::string>();
                if (lev == "none")
                {
                    *logLevel = LogLevel::NONE;
                }
                else if (lev == "normal")
                {
                    *logLevel = LogLevel::NORMAL;
                }
                else if (lev == "verbose")
                {
                    *logLevel = LogLevel::VERBOSE;
                }
                else
                {
                    sgtError(std::string("Unknown log level ") + lev);
                }
            }

            const YAML::Node& ndLogFilter = nodeVal["log_filter"];
            if (ndLogFilter)
            {
                *logFilter = ndLogFilter.as<std::string>();
            }
        }
    }

    void ParserBase::parseRandomSeed(const YAML::Node& nd)
    {
        randSeed(nd.as<unsigned int>());
    }

    namespace
    {
        const std::regex expr(                      // Submatch 0: whole expr, with <>.
            "<"
            "("                                     // Submatch 1: whole expr body, without <>.
                "("                                 // Submatch 2: whole math expr body.
                    "[-+*/%0-9.eEfFlL() \t]+"       // Any string of allowed chars in math expression.
                ")|"                                // End submatch 2.
                "("                                 // Submatch 3: whole var/loop expr body.
                    "("                             // Submatch 4: non-index part of var/loop expr body.
                        "[a-zA-Z_][a-zA-Z0-9_]*"    // Word not starting with digit.
                    ")+"                            // End submatch 4.
                    "("                             // Submatch 5: index part of var/loop expr body, with ().
                        "\\("
                        "("                         // Submatch 6: index part of var/loop expr, without ().
                            "[a-zA-Z0-9_]*"
                        ")"                         // End submatch 6.
                        "\\)"
                    ")?"                            // End submatch 5.
                ")"                                 // End submatch 3.
            ")"                                     // End submatch 1.
            ">"
        );                                          // End submatch 0.
    }

    std::string ParserBase::expandString(const std::string& str) const
    {
        sgtLogDebug(LogLevel::VERBOSE) << "Expand:" << std::endl;
        LogIndent indent;
        sgtLogDebug(LogLevel::VERBOSE) << "String:" << std::endl;
        indent.in(); sgtLogDebug(LogLevel::VERBOSE) << str << std::endl; indent.out();
        std::string result;

        // Iterator over expansion expressions in the target.
        std::sregex_iterator begin(str.cbegin(), str.cend(), expr);
        std::sregex_iterator end;

        if (begin == end)
        {
            // No expressions, just return the string.
            result = str;
        }
        else
        {
            std::string exprSuffix;
            for (auto it = begin; it != end; ++it) // Loop over all expansion expressions.
            {
                result += it->prefix();
                exprSuffix = it->suffix();
                std::string exprExpansion = expandExpression(*it);
                result += exprExpansion; // Put the prefix on the result.
            }
            result += exprSuffix;
            // Now recursively check if the expansion of this expression has created any more expressions to expand.
            result = expandString(result);
        }
        sgtLogDebug(LogLevel::VERBOSE) << "Result:" << std::endl;
        indent.in(); sgtLogDebug(LogLevel::VERBOSE) << result << std::endl; indent.out();
        return result;
    }

    std::string ParserBase::expandExpression(const std::smatch& m) const
    {
        std::string result;
        if (m[2] != "")
        {
            // Math expression, e.g. ${2 + 3/2}.
            result = expandMathExpressionBody(m[1]);
        }
        else
        {
            // Loop or parameter expression, e.g. $idx or $phases[3].
            result = expandLoopOrParameterExpressionBody(m[4], m[6]);
        }
        return result;
    }

    std::string ParserBase::expandMathExpressionBody(const std::string& str) const
    {
        int calcResult;
        std::string::const_iterator i1 = str.begin();
        std::string::const_iterator i2 = str.end();
        bool ok = phrase_parse(i1, i2, calc, space, calcResult); // And do the math...
        sgtAssert(ok, "Ill-formed expression " << str << ".");
        std::string result = std::to_string(calcResult);
        return result;
    }

    std::string ParserBase::expandLoopOrParameterExpressionBody(const std::string& s1, const std::string& s2) const
    {
        std::string result;

        auto it = std::find_if(loops_.begin(), loops_.end(),
                [&s1](const std::unique_ptr<ParserLoop>& l) {return l->name_ == s1;});
        if (it != loops_.end())
        {
            // We matched a loop variable. Make sure there is no index!
            sgtAssert(s2 == "", "Loop variable " << s1 << " can not be indexed with " << s2 << ".");
            result = std::regex_replace(s1, std::regex((**it).name_), std::to_string((**it).i_));
            sgtLogDebug(LogLevel::VERBOSE) << "Expand loop: " << s1 << " -> " << result << std::endl;
        }
        else
        {
            // No matching loop, try parameters.
            YAML::Node nd;
            try
            {
                nd = parameters_.at(s1);
            }
            catch (std::out_of_range e)
            {
                sgtError("Parameter or loop " << s1 << " was not found.")
            }

            YAML::Node nd2;
            if (s2 != "")
            {
                if (nd.IsSequence())
                {
                    nd2 = nd[std::stoi(s2)];
                }
                else if (nd.IsMap())
                {
                    nd2 = nd[s2];
                }
                else
                {
                    sgtError("Can't index parameter " << s1 << ".");
                }

                sgtAssert(nd2, "For parameter " << s1 << ", index " << s2 << " doesn't exist.");
            }
            else
            {
                nd2 = nd;
            }
            result = nd2Str(nd2);
        }

        return result;
    }

    std::string ParserBase::nd2Str(const YAML::Node& nd) const
    {
        std::string result;
        if (!nd.IsScalar())
        {
            // Have to coerce List and Map nodes into the form of a string.
            std::ostringstream oss;
            oss << nd << std::endl;
            result = oss.str();
        }
        else
        {
            // For scalars, this way prevents extra newlines, etc. being inserted, which matters for strings.
            result = nd.as<std::string>();
        }
        return result;
    }
}


/**
 * @file hybrid_height.cpp
 *
 */

#include "hybrid_height.h"
#include "logger.h"
#include "plugin_factory.h"
#include <boost/thread.hpp>
#include <future>

#include "cache.h"
#include "neons.h"
#include "radon.h"
#include "writer.h"

using namespace std;
using namespace himan::plugin;

const string itsName("hybrid_height");

const himan::param ZParam("Z-M2S2");
const himan::params GPParam{himan::param("LNSP-N"), himan::param("P-PA")};
const himan::param PParam("P-HPA");
const himan::param TParam("T-K");
const himan::param TGParam("TG-K");

static mutex prefetchedMutex;
static vector<string> prefetched;

static mutex asyncMutex;
static vector<future<void>> asyncs;

hybrid_height::hybrid_height() : itsBottomLevel(kHPMissingInt), itsUseGeopotential(true), itsUseWriterThreads(false)
{
	itsLogger = logger(itsName);
}

hybrid_height::~hybrid_height() {}
void hybrid_height::Process(std::shared_ptr<const plugin_configuration> conf)
{
	Init(conf);

	HPDatabaseType dbtype = conf->DatabaseType();

	if (dbtype == kNeons || dbtype == kNeonsAndRadon)
	{
		auto n = GET_PLUGIN(neons);

		itsBottomLevel = stoi(n->ProducerMetaData(itsConfiguration->SourceProducer().Id(), "last hybrid level number"));
	}

	if ((dbtype == kRadon || dbtype == kNeonsAndRadon) && itsBottomLevel == kHPMissingInt)
	{
		auto r = GET_PLUGIN(radon);

		itsBottomLevel =
		    stoi(r->RadonDB().GetProducerMetaData(itsConfiguration->SourceProducer().Id(), "last hybrid level number"));
	}

	if (itsConfiguration->Info()->Producer().Id() == 240 || itsConfiguration->Info()->Producer().Id() == 243)
	{
		itsUseGeopotential = false;

		// Using separate writer threads is only efficient when we are calculating with iteration (ECMWF)
		// and if we are using external packing like gzip. In those condition it should give according to initial
		// tests a ~30% increase in total calculation speed.

		PrimaryDimension(kTimeDimension);

		if (itsConfiguration->FileCompression() != kNoCompression &&
		    (itsConfiguration->FileWriteOption() == kDatabase || itsConfiguration->FileWriteOption() == kMultipleFiles))
		{
			itsUseWriterThreads = true;
		}

		/*
		 * With iteration method, we must start from the lowest level.
		 */

		if (itsInfo->SizeLevels() > 1)
		{
			auto first = itsInfo->PeekLevel(0), second = itsInfo->PeekLevel(1);

			if (first.Value() < second.Value())
			{
				itsInfo->LevelOrder(kBottomToTop);
			}
		}
	}

	SetParams({param("HL-M", 3, 0, 3, 6)});

	Start();

	if (itsUseWriterThreads)
	{
		itsWriterGroup.join_all();
	}
}

void hybrid_height::Calculate(shared_ptr<info> myTargetInfo, unsigned short threadIndex)
{
	auto myThreadedLogger = logger(itsName + "Thread #" + to_string(threadIndex));

	myThreadedLogger.Info("Calculating time " + static_cast<string>(myTargetInfo->Time().ValidDateTime()) + " level " +
	                      static_cast<string>(myTargetInfo->Level()) + " forecast type " +
	                      static_cast<string>(myTargetInfo->ForecastType()));

	if (itsUseGeopotential)
	{
		bool ret = WithGeopotential(myTargetInfo);

		if (!ret)
		{
			myThreadedLogger.Warning("Skipping step " + to_string(myTargetInfo->Time().Step()) + ", level " +
			                         static_cast<string>(myTargetInfo->Level()));
			return;
		}
	}
	else
	{
		bool ret = WithIteration(myTargetInfo);

		if (!ret)
		{
			myThreadedLogger.Warning("Skipping step " + to_string(myTargetInfo->Time().Step()) + ", level " +
			                         static_cast<string>(myTargetInfo->Level()));
			return;
		}
	}

	string deviceType = "CPU";

	myThreadedLogger.Info("[" + deviceType + "] Missing values: " + to_string(myTargetInfo->Data().MissingCount()) +
	                      "/" + to_string(myTargetInfo->Data().Size()));
}

bool hybrid_height::WithGeopotential(info_t& myTargetInfo)
{
	const himan::level H0(himan::kHeight, 0);
	auto GPInfo = Fetch(myTargetInfo->Time(), myTargetInfo->Level(), ZParam, myTargetInfo->ForecastType(), false);
	auto zeroGPInfo = Fetch(myTargetInfo->Time(), H0, ZParam, myTargetInfo->ForecastType(), false);

	if (!GPInfo || !zeroGPInfo)
	{
		return false;
	}

	SetAB(myTargetInfo, GPInfo);

	auto& target = VEC(myTargetInfo);

	for (auto&& tup : zip_range(target, VEC(GPInfo), VEC(zeroGPInfo)))
	{
		double& result = tup.get<0>();
		double GP = tup.get<1>();
		double zeroGP = tup.get<2>();

		if (GP == kFloatMissing || zeroGP == kFloatMissing)
		{
			continue;
		}

		result = (GP - zeroGP) * himan::constants::kIg;
	}

	return true;
}

void hybrid_height::Prefetch(info_t myTargetInfo)
{
	const auto forecastTime = myTargetInfo->Time();
	const auto forecastType = myTargetInfo->ForecastType();

	const string label = static_cast<string>(forecastTime.ValidDateTime()) + "_" + static_cast<string>(forecastType);

	{
		lock_guard<mutex> lock(prefetchedMutex);

		if (find(prefetched.begin(), prefetched.end(), label) != prefetched.end())
		{
			return;
		}

		prefetched.push_back(label);
	}

	double firstLevelValue, lastLevelValue;

	if (itsInfo->LevelOrder() == kTopToBottom)
	{
		firstLevelValue = myTargetInfo->PeekLevel(0).Value();
		lastLevelValue = myTargetInfo->PeekLevel(myTargetInfo->SizeLevels() - 1).Value();
	}
	else
	{
		firstLevelValue = myTargetInfo->PeekLevel(myTargetInfo->SizeLevels() - 1).Value();
		lastLevelValue = myTargetInfo->PeekLevel(0).Value();
	}

	if (firstLevelValue != lastLevelValue)
	{
		lock_guard<mutex> lock(asyncMutex);
		itsLogger.Trace("Prefetching from " + to_string(firstLevelValue - 1) + " to " + to_string(lastLevelValue));

		double levelValue = firstLevelValue;

		asyncs.push_back(async(launch::async,
		                       [=](double levelValue) {
			                       while (levelValue >= lastLevelValue)
			                       {
				                       levelValue--;

				                       Fetch(forecastTime, level(kHybrid, levelValue), PParam, forecastType, false);
			                       }
			                   },
		                       levelValue));

		asyncs.push_back(async(launch::async,
		                       [=](double levelValue) {
			                       while (levelValue >= lastLevelValue)
			                       {
				                       levelValue--;

				                       Fetch(forecastTime, level(kHybrid, levelValue), TParam, forecastType, false);
			                       }
			                   },
		                       levelValue));
	}
}

bool hybrid_height::WithIteration(info_t& myTargetInfo)
{
	const auto forecastTime = myTargetInfo->Time();
	const auto forecastType = myTargetInfo->ForecastType();

	level prevLevel;
	info_t prevTInfo, prevPInfo, prevHInfo;

	bool firstLevel = false;

	if (myTargetInfo->Level().Value() == itsBottomLevel)
	{
		firstLevel = true;
		if (myTargetInfo->Producer().Id() == 240 || myTargetInfo->Producer().Id() == 243)
		{
			// LNSP is always at level 1 for ECMWF
			prevPInfo = Fetch(forecastTime, level(himan::kHybrid, 1), GPParam, forecastType, false);
			prevTInfo = Fetch(forecastTime, level(himan::kHybrid, itsBottomLevel), TParam, forecastType, false);

			if (prevPInfo)
			{
				// LNSP to regular pressure
				for (double& val : prevPInfo->Data().Values())
				{
					val = exp(val);
				}
			}
		}
		else
		{
			prevPInfo = Fetch(forecastTime, level(himan::kHeight, 0), GPParam, forecastType, false);
			prevTInfo = Fetch(forecastTime, level(himan::kHeight, 2), TParam, forecastType,
			                  false);  // t2 is better than ground temperature here?
		}
	}
	else
	{
		prevLevel = level(myTargetInfo->Level());
		prevLevel.Value(myTargetInfo->Level().Value() + 1);

		prevLevel.Index(prevLevel.Index() + 1);

		prevTInfo = Fetch(forecastTime, prevLevel, TParam, forecastType, false);
		prevPInfo = Fetch(forecastTime, prevLevel, PParam, forecastType, false);
		prevHInfo = Fetch(forecastTime, prevLevel, param("HL-M"), forecastType, false);
	}

	auto PInfo = Fetch(forecastTime, myTargetInfo->Level(), PParam, forecastType, false);
	auto TInfo = Fetch(forecastTime, myTargetInfo->Level(), TParam, forecastType, false);

	if (!prevTInfo || !prevPInfo || (!prevHInfo && !firstLevel) || !PInfo || !TInfo)
	{
		itsLogger.Error("Source data missing for level " + to_string(myTargetInfo->Level().Value()) + " step " +
		                to_string(myTargetInfo->Time().Step()) + ", stopping processing");
		return false;
	}

	SetAB(myTargetInfo, TInfo);

	// First level for ECMWF is LNSP which needs to be converted
	// to regular pressure

	double scale = 1.;
	vector<double> prevHV;

	if (firstLevel)
	{
		scale = 0.01;

		prevHV.resize(myTargetInfo->Data().Size(), 0);
	}
	else
	{
		assert(prevLevel.Value() > myTargetInfo->Level().Value());
		prevHV = VEC(prevHInfo);
	}

	// Prefetch next level data
	// Iteration-style processing of hybrid_height is sequential by nature, and there is nothing
	// we can do about it. What we can do is to make sure that all data is present in cache when the
	// executing thread starts to process a level. That is done here: both temperature and pressure
	// are fetched asynchronously (both separately!) to cache.
	// This change has a positive impact on performance especially on ensemble data processing:
	// hybrid_level calculation time for ECMWF ensemble (51 members, singe time step) is cut to half.

	Prefetch(myTargetInfo);

	auto& target = VEC(myTargetInfo);

	for (auto&& tup : zip_range(target, VEC(PInfo), VEC(prevPInfo), VEC(TInfo), VEC(prevTInfo), prevHV))
	{
		double& result = tup.get<0>();
		double P = tup.get<1>();
		double prevP = tup.get<2>();
		double T = tup.get<3>();
		double prevT = tup.get<4>();
		double prevH = tup.get<5>();

		if (prevT == kFloatMissing || prevP == kFloatMissing || T == kFloatMissing || P == kFloatMissing ||
		    prevH == kFloatMissing)
		{
			continue;
		}

		prevP *= scale;

		double deltaZ = 14.628 * (prevT + T) * log(prevP / P);
		double totalHeight = prevH + deltaZ;

		assert(isfinite(totalHeight));

		result = totalHeight;
	}

	// Check if we have data in grid. If all values are missing, it is impossible to continue
	// processing any level above this one.

	if (myTargetInfo->Data().Size() == myTargetInfo->Data().MissingCount())
	{
		itsLogger.Error("All data missing for level " + to_string(myTargetInfo->Level().Value()) + " step " +
		                to_string(myTargetInfo->Time().Step()) + ", stopping processing");
		return false;
	}

	// If we are writing to a single file, launch a separate writing thread and let the main
	// thread proceed. This speeds up the processing in those machines where writing of files
	// is particularly slow (for example when external packing is used)

	if (itsUseWriterThreads)
	{
		// First the data needs to be added to cache, otherwise this current calculating
		// thread cannot find it in the next step

		if (itsConfiguration->UseCache())
		{
			auto c = GET_PLUGIN(cache);

			c->Insert(*myTargetInfo);
		}

		// Write to disk asynchronously
		boost::thread* t = new boost::thread(&hybrid_height::Write, this, boost::ref(*myTargetInfo));
		itsWriterGroup.add_thread(t);
		itsLogger.Trace("Writer thread started");
	}

	return true;
}

// This functions copies targetInfo intentionally!
void hybrid_height::Write(himan::info targetInfo)
{
	using namespace himan;
	auto aWriter = GET_PLUGIN(writer);

	assert(itsConfiguration->FileWriteOption() != kSingleFile);

	targetInfo.ResetParam();

	while (targetInfo.NextParam())
	{
		aWriter->ToFile(targetInfo, itsConfiguration);
	}

	if (itsConfiguration->UseDynamicMemoryAllocation())
	{
		compiled_plugin_base::DeallocateMemory(targetInfo);
	}
}

void hybrid_height::WriteToFile(const info& targetInfo, write_options writeOptions)
{
	if (!itsUseWriterThreads)
	{
		compiled_plugin_base::WriteToFile(targetInfo, writeOptions);
	}
}


#include "base/color.hpp"
#include "boost/tokenizer.hpp"
#include "core/plan/runner.hpp"

#include <boost/algorithm/string.hpp>
#include <cmath>
#include <regex>
#include <string>

DEFINE_int32(num_of_docs, 1, "# number of docs");
DEFINE_int32(num_doc_partition, 10, "");
DEFINE_int32(num_term_partition, 10, "");

DEFINE_string(url, "", "The url for hdfs file");

using namespace xyz;

class Document {
public:
  using KeyT = std::string;
  Document() = default;
  explicit Document(const KeyT &t) : title(t) {}
  KeyT title;
  std::vector<double> tf;
  std::vector<double> tf_idf;
  std::vector<int> words;
  int total_words = 0;
  const KeyT &id() const { return title; }
  KeyT Key() const { return title; }
  friend SArrayBinStream &operator<<(SArrayBinStream &stream,
                                     const Document &doc) {
    stream << doc.title << doc.tf << doc.tf_idf << doc.words << doc.total_words;
    return stream;
  }
  friend SArrayBinStream &operator>>(SArrayBinStream &stream, Document &doc) {
    stream >> doc.title >> doc.tf >> doc.tf_idf >> doc.words >> doc.total_words;
    return stream;
  }
};

class Term {
public:
  // using KeyT = std::size_t;
  using KeyT = int;
  Term() = default;
  explicit Term(const KeyT &term) : termid(term) { idf = 0; }
  KeyT termid;
  int idf;
  const KeyT &id() const { return termid; }
  KeyT Key() const { return termid; }
  friend SArrayBinStream &operator<<(SArrayBinStream &stream, const Term &t) {
    stream << t.termid << t.idf;
    return stream;
  }
  friend SArrayBinStream &operator>>(SArrayBinStream &stream, Term &t) {
    stream >> t.termid >> t.idf;
    return stream;
  }
};

int main(int argc, char **argv) {
  Runner::Init(argc, argv);

  // load and generate two collections
  /*
  auto loaded_docs =
      Context::load_wholefiles(FLAGS_url, [](std::string content) {
        // parse extract title
        std::regex rgx(".*?title=\"(.*?)\".*?");
        std::smatch match;
        std::string title;
        if (std::regex_search(content, match, rgx)) {
          // LOG(INFO) << match[1];
          title = match[1];
        } else {
          CHECK(false) << "cannot match title";
        }

        Document doc(title);
        std::vector<int> count;
        if (content.size() > 0) {
          boost::char_separator<char> sep(" \t\n.,()\'\":;!?<>");
          boost::tokenizer<boost::char_separator<char>> tok(content, sep);
          for (auto &w : tok) {
            doc.words.push_back(std::hash<std::string>{}(w));
            // std::transform(doc.words.back().begin(), doc.words.back().end(),
            // doc.words.back().begin(), ::tolower);
          }
          doc.total_words = doc.words.size();
          std::sort(doc.words.begin(), doc.words.end());
          int n = 0;
          for (int i = 0, j = 0; i < doc.words.size(); i = j) {
            for (j = i + 1; j < doc.words.size(); j++) {
              if (doc.words.at(i) != doc.words.at(j))
                break;
            }
            count.push_back(j - i);
            doc.words[n++] = doc.words[i];
          }
          doc.words.resize(n);
          doc.words.shrink_to_fit();
          doc.tf.resize(doc.words.size());
          doc.tf_idf.resize(doc.words.size());
          for (int i = 0; i < doc.words.size(); i++) {
            doc.tf.at(i) = static_cast<double>(count.at(i)) / doc.total_words;
          }
        }

        return doc;
      });
      */
  auto loaded_docs =
      Context::load(FLAGS_url, [](std::string content) {
        // parse extract title

        Document doc("");
        std::vector<int> count;
        if (content.size() > 0) {
	  std::transform(content.begin(), content.end(), content.begin(), ::tolower);
          // boost::char_separator<char> sep(" \t\n.,()\'\":;!?<>");
          boost::char_separator<char> sep(" \t\n");
          boost::tokenizer<boost::char_separator<char>> tok(content, sep);
          for (auto &w : tok) {
            // doc.words.push_back(std::hash<std::string>{}(w)%1000);
            doc.words.push_back(std::hash<std::string>{}(w)%(2<<18));
            // doc.words.push_back(std::hash<std::string>{}(w)%(2<<20));
            // std::transform(doc.words.back().begin(), doc.words.back().end(),
            // doc.words.back().begin(), ::tolower);
          }
          doc.total_words = doc.words.size();
          std::sort(doc.words.begin(), doc.words.end());
          int n = 0;
          for (int i = 0, j = 0; i < doc.words.size(); i = j) {
            for (j = i + 1; j < doc.words.size(); j++) {
              if (doc.words.at(i) != doc.words.at(j))
                break;
            }
            count.push_back(j - i);
            doc.words[n++] = doc.words[i];
          }
          doc.words.resize(n);
          doc.words.shrink_to_fit();
          doc.tf.resize(doc.words.size());
          doc.tf_idf.resize(doc.words.size());
          for (int i = 0; i < doc.words.size(); i++) {
            doc.tf.at(i) = static_cast<double>(count.at(i));
          }
        }

        return doc;
      });

  Context::count(loaded_docs);
  // no need indexed_docs using pull model.
  // auto indexed_docs =
  // Context::placeholder<Document>(FLAGS_num_doc_partition);
  auto terms_key_part_mapper =
      std::make_shared<HashKeyToPartMapper<int>>(FLAGS_num_term_partition);
  auto terms = Context::placeholder<Term>(FLAGS_num_term_partition,
                                          terms_key_part_mapper);

  // Context::sort_each_partition(indexed_docs);
  Context::sort_each_partition(terms);

  Context::mappartupdate(loaded_docs, terms,
                       [](TypedPartition<Document> *p, Output<int, int> *o) {
                         for (auto &doc : *p) {
                           for (int i = 0; i < doc.words.size(); i++) {
                             o->Add(doc.words[i], 1);
                           }
                         }
                       },
                       [](Term *term, int n) {
                         term->idf += n;
			 // LOG(INFO) << "idf: " << term->idf;
                       })
      ->SetCombine([](int *a, int b) { *a += b; })
      ->SetName("Out all the doc");

  // Context::count(terms);
  auto dummy_collection = Context::placeholder<KVObjT<int, double>>(1);
  Context::mappartwithupdate(
      loaded_docs, terms, dummy_collection,
      [terms_key_part_mapper](TypedPartition<Document> *p,
                              TypedCache<Term> *typed_cache,
                              Output<int, double> *o) {

        std::vector<std::shared_ptr<IndexedSeqPartition<Term>>> with_parts(
            FLAGS_num_term_partition);
        std::map<std::string, int> terms_map;
        int start_idx = rand() % FLAGS_num_term_partition; // random start_idx
                                                           // to avoid overload
                                                           // on one point
        for (int i = 0; i < FLAGS_num_term_partition; i++) {
          int idx = (start_idx + i) % FLAGS_num_term_partition;
          auto start_time = std::chrono::system_clock::now();
          auto part = typed_cache->GetPartition(idx);
          auto end_time = std::chrono::system_clock::now();
          std::chrono::duration<double> duration = end_time - start_time;
          // if (FLAGS_node_id == 0) {
          //   LOG(INFO) << GREEN("fetch time for " + std::to_string(idx) << " :
          //   " + std::to_string(duration.count()));
          // }
          with_parts[idx] =
              std::dynamic_pointer_cast<IndexedSeqPartition<Term>>(part);
        }
        // LOG(INFO) << "fetch done";
        // method1: copy and find

        /*
        for (auto with_p : with_parts) {
          for (auto& term : *with_p) {
            terms_map[term.id()] = term.idf;
          }
        }

        LOG(INFO) << "building local terms_map";
        for (auto& doc : *p) {
          for(int i = 0; i < doc.words.size(); i++){
            std::string term  = doc.words[i];
            int count = terms_map[term];
            doc.tf_idf[i] = doc.tf[i] * std::log(FLAGS_num_of_docs /
        double(count));
          }
        }
        */

        // method2
        for (auto &doc : *p) {
          for (int i = 0; i < doc.words.size(); i++) {
            int term = doc.words[i];
            auto &with_p = with_parts[terms_key_part_mapper->Get(term)];
            auto *t = with_p->Find(term);
            CHECK_NOTNULL(t);
            int count = t->idf;
	    // LOG(INFO) << "@@ tf: " << doc.tf[i] << " count: " << count;
            doc.tf_idf[i] =
                // doc.tf[i] * std::log(FLAGS_num_of_docs / double(count));
                doc.tf[i] * 1.0/ double(count);
	    o->Add(0, doc.tf_idf[i]);
          }
        }

        for (int i = 0; i < FLAGS_num_term_partition; i++) {
          typed_cache->ReleasePart(i);
        }
      },
      [](KVObjT<int, double> *t, double acc) { t->b += acc; })
      ->SetCombine([](double* a, double b) { *a += b; })
      ->SetName("Send idf back to doc");

      Context::foreach(dummy_collection, [](const KVObjT<int, double>& obj) {
        LOG(INFO) << "********** res: " << obj.b << " *********";
      });
  Runner::Run();
}


/* #include <boost/python.hpp> */
#include "inference.hpp"

vector<float> Inference::prepareImage(cv::Mat& img)
{
    using namespace cv;

    /* int c = parser::getIntValue("C"); */
    /* int h = parser::getIntValue("H");   //net h */
    /* int w = parser::getIntValue("W");   //net w */

    float scale = min(float(w)/img.cols,float(h)/img.rows);
    auto scaleSize = cv::Size(img.cols * scale,img.rows * scale);

    cv::Mat rgb ;
    cv::cvtColor(img, rgb, CV_BGR2RGB);
    cv::Mat resized;
    cv::resize(rgb, resized,scaleSize,0,0,INTER_CUBIC);

    cv::Mat cropped(h, w,CV_8UC3, 127);
    Rect rect((w- scaleSize.width)/2, (h-scaleSize.height)/2, scaleSize.width,scaleSize.height);
    resized.copyTo(cropped(rect));

    cv::Mat img_float;
    if (c == 3)
        cropped.convertTo(img_float, CV_32FC3, 1/255.0);
    else
        cropped.convertTo(img_float, CV_32FC1 ,1/255.0);

    //HWC TO CHW
    vector<Mat> input_channels(c);
    cv::split(img_float, input_channels);

    vector<float> result(h*w*c);
    auto data = result.data();
    int channelLength = h * w;
    for (int i = 0; i < c; ++i) {
        memcpy(data,input_channels[i].data,channelLength*sizeof(float));
        data += channelLength;
    }

    return result;
}

void Inference::DoNms(vector<Detection>& detections,int classes ,float nmsThresh)
{
    auto t_start = chrono::high_resolution_clock::now();

    vector<vector<Detection>> resClass;
    resClass.resize(classes);

    for (const auto& item : detections)
        resClass[item.classId].push_back(item);

    auto iouCompute = [](float * lbox, float* rbox)
    {
        float interBox[] = {
            max(lbox[0] - lbox[2]/2.f , rbox[0] - rbox[2]/2.f), //left
            min(lbox[0] + lbox[2]/2.f , rbox[0] + rbox[2]/2.f), //right
            max(lbox[1] - lbox[3]/2.f , rbox[1] - rbox[3]/2.f), //top
            min(lbox[1] + lbox[3]/2.f , rbox[1] + rbox[3]/2.f), //bottom
        };

        if(interBox[2] > interBox[3] || interBox[0] > interBox[1])
            return 0.0f;

        float interBoxS =(interBox[1]-interBox[0])*(interBox[3]-interBox[2]);
        return interBoxS/(lbox[2]*lbox[3] + rbox[2]*rbox[3] -interBoxS);
    };

    vector<Detection> result;
    for (int i = 0;i<classes;++i)
    {
        auto& dets =resClass[i];
        if(dets.size() == 0)
            continue;

        sort(dets.begin(),dets.end(),[=](const Detection& left,const Detection& right){
            return left.prob > right.prob;
        });

        for (unsigned int m = 0;m < dets.size() ; ++m)
        {
            auto& item = dets[m];
            result.push_back(item);
            for(unsigned int n = m + 1;n < dets.size() ; ++n)
            {
                if (iouCompute(item.bbox,dets[n].bbox) > nmsThresh)
                {
                    dets.erase(dets.begin()+n);
                    --n;
                }
            }
        }
    }

    //swap(detections,result);
    detections = move(result);

    auto t_end = chrono::high_resolution_clock::now();
    float total = chrono::duration<float, milli>(t_end - t_start).count();
    cout << "Time taken for nms is " << total << " ms." << endl;
}

vector<Bbox> Inference::postProcessImg(cv::Mat& img,vector<Detection>& detections,int classes)
{
    using namespace cv;

    /* int h = parser::getIntValue("H");   //net h */
    /* int w = parser::getIntValue("W");   //net w */

    //scale bbox to img
    int width = img.cols;
    int height = img.rows;
    float scale = min(float(w)/width,float(h)/height);
    float scaleSize[] = {width * scale,height * scale};

    //correct box
    for (auto& item : detections)
    {
        auto& bbox = item.bbox;
        bbox[0] = (bbox[0] * w - (w - scaleSize[0])/2.f) / scaleSize[0];
        bbox[1] = (bbox[1] * h - (h - scaleSize[1])/2.f) / scaleSize[1];
        bbox[2] /= scaleSize[0];
        bbox[3] /= scaleSize[1];
    }

    //nms
    /* float nmsThresh = parser::getFloatValue("nms"); */
    if(nmsThresh > 0)
        DoNms(detections,classes,nmsThresh);

    vector<Bbox> boxes;
    for(const auto& item : detections)
    {
        auto& b = item.bbox;
        Bbox bbox =
        {
            item.classId,   //classId
            max(int((b[0]-b[2]/2.)*width),0), //left
            min(int((b[0]+b[2]/2.)*width),width), //right
            max(int((b[1]-b[3]/2.)*height),0), //top
            min(int((b[1]+b[3]/2.)*height),height), //bot
            item.prob       //score
        };
        boxes.push_back(bbox);
    }

    return boxes;
}

vector<string> Inference::split(const string& str, char delim)
{
    stringstream ss(str);
    string token;
    vector<string> container;
    while (getline(ss, token, delim)) {
        container.push_back(token);
    }

    return container;
}

bool Inference::loadTRTModel(string netname)
{

    // cout << "Enter loadTRTModel function \n";

    if (netname.length() > 0){
        net.reset(new trtNet(netname));
        assert(net->getBatchSize() == batchSize);
    }
    else
      cerr << "no net present";
    return false;
    /* outputCount = net->getOutputSize()/sizeof(float); */
    /* outputData = make_unique<float[]>(outputCount); */
    return true;
}

std::list<vector<Bbox>> Inference::infer(vector<cv::Mat> image)
{
    std::list<vector<Bbox>> outputs;
    int outputCount;
    outputCount = net->getOutputSize()/sizeof(float);
    unique_ptr<float[]> outputData(new float[outputCount]);
    inputData.reserve(h*w*c*batchSize);
    for (auto r = 0; r<image.size(); r++)
    {
        vector<float> curInput = prepareImage(image[r]);
        if (!curInput.data())
            continue;
        inputImgs.emplace_back(image[r]);

        inputData.insert(inputData.end(), curInput.begin(), curInput.end());
        batchCount++;

        if(batchCount < batchSize && r + 1 <  image.size())
            continue;

        net->doInference(inputData.data(), outputData.get(),batchCount);

        //Get Output
        auto output = outputData.get();
        auto outputSize = net->getOutputSize()/ sizeof(float) / batchCount;
        for(int i = 0;i< batchCount ; ++i)
        {
        //first detect count
            int detCount = output[0];
        //later detect result
        vector<Detection> result;
            result.resize(detCount);
            memcpy(result.data(), &output[1], detCount*sizeof(Detection));

            auto boxes = postProcessImg(inputImgs[i],result,classNum);
        outputs.emplace_back(boxes);

            output += outputSize;
        }
        inputImgs.clear();
        inputData.clear();

        batchCount = 0;
    }
    return outputs;
}

vector<Bbox> Inference::infer_single_image(cv::Mat image)
{
    vector<Bbox> outputs;
    int outputCount;
    vector<Bbox> fail_case;
    outputCount = net->getOutputSize()/sizeof(float);
    unique_ptr<float[]> outputData(new float[outputCount]);
    inputData.reserve(h*w*c*batchSize);
    vector<float> curInput = prepareImage(image);
    if (!curInput.data())
    {
        cerr << "No input data" << endl;
        return fail_case;
        /* exit(EXIT_FAILURE) */
    }
    inputImgs.emplace_back(image);

    inputData.insert(inputData.end(), curInput.begin(), curInput.end());
    batchCount++;

    if(batchCount < batchSize)
    {
        cerr << "BatchCount > batchSize" << endl;
        return fail_case;
        /* exit(EXIT_FAILURE) */
    }

    net->doInference(inputData.data(), outputData.get(),batchCount);

    //Get Output
    auto output = outputData.get();
    auto outputSize = net->getOutputSize()/ sizeof(float) / batchCount;
    for(int i = 0;i< batchCount ; ++i)
    {
    //first detect count
        int detCount = output[0];
    //later detect result
    vector<Detection> result;
        result.resize(detCount);
        memcpy(result.data(), &output[1], detCount*sizeof(Detection));

        auto boxes = postProcessImg(inputImgs[i],result,classNum);
	outputs = boxes;

	output += outputSize;
    }
    inputImgs.clear();
    inputData.clear();

    batchCount = 0;
    return outputs;
}

/* class World */
/* { */
/*     public: */
/*         void print(); */
/*         World(std::string m) { cout << "Hello" << endl;} */
/* }; */
/* void World::print() */
/* { */
/*     cout<< "hello" << endl; */
/* } */

// ******************************************************************************
// boost wrapper for python

/* using namespace boost::python; */

/* BOOST_PYTHON_MODULE(inference) */
/* { */
/*     /1* std::list<vector<Bbox>>(inference::*in1)(vector<cv::Mat>) = &inference::infer; *1/ */
/*     /1* std::list<vector<Bbox>>(inference::*in2)(cv::Mat) = &inference::infer; *1/ */

/*     class_<Inference, Inference*, boost::noncopyable>("Inference", boost::python::no_init) */
/*         .def("doNMS", &Inference::DoNms) */
/*         .def("postprocessimg", &Inference::postProcessImg) */
/*         .def("prepareimg", &Inference::prepareImage) */
/*         .def("split", &Inference::split) */
/*         .def("load_trt", &Inference::loadTRTModel) */
/*         .def("infer", &Inference::infer) */
/*         .def("infer_single_image", &Inference::infer_single_image) */
/*         ; */

/*     /1* class_<World>("World", init<std::string>()) *1/ */
/*     /1*     .def("print", &World::print) *1/ */
/*     /1*     ; *1/ */
/* }; */


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

#include <boost/simd/toolbox/swar/functions/arith.hpp>
#include <boost/simd/include/functions/simd/plus.hpp>
#include <boost/simd/include/functions/simd/multiplies.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::arith_, tag::cpu_
                                    , (X)(T)
                                    , ((target_< simd_< arithmetic_<T>,X> >))
                                    )
  {
    typedef typename T::type result_type;

    result_type operator()(T const& ) const
    {
      result_type that;
      for(std::size_t i=0;i<result_type::static_size;++i) that[i] = i;
      return that;
    }
  };
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::arith_, tag::cpu_
                                      , (A0)(X)(T)
                                      , (scalar_< arithmetic_<A0> >)
                                        ((target_< simd_< arithmetic_<T>,X> >))
                                      )
  {
    typedef typename T::type result_type;

    result_type operator()(A0 const& a0, T const& ) const
    {
      const result_type tmp =  boost::simd::arith<result_type>();
      return plus(tmp, a0);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::arith_, tag::cpu_
                                      , (A0)(X)(T)
                                      , ((simd_< arithmetic_<A0>, X >))
                                        ((target_< simd_< arithmetic_<T>,X> >))
                                      )
  {
    typedef typename T::type result_type;

    result_type operator()(A0 const& a0, T const& ) const
    {
      return bitwise_cast<result_type>(a0);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::arith_, tag::cpu_
                                      , (A0)(A1)(X)(T)
                                      , (scalar_< arithmetic_<A0> >)
                                        (scalar_< arithmetic_<A1> >)
                                        ((target_< simd_< arithmetic_<T>,X> >))
                                      )
  {
    typedef typename T::type result_type;

    result_type operator()(A0 const& a0, A1 const& a1, T const& ) const
    {
      const result_type tmp = boost::simd::arith<result_type>();
      return plus(multiplies(tmp, a1), a0);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::arith_, tag::cpu_
                                      , (A0)(A1)(X)(T)
                                      , ((simd_< arithmetic_<A0>, X >))
                                        (generic_< arithmetic_<A1> >)
                                        ((target_< simd_< arithmetic_<T>,X> >))
                                      )
  {
    typedef typename T::type result_type;

    result_type operator()(A0 const& a0, A1 const& a1, T const& ) const
    {
      return bitwise_cast<result_type>(a0);
    }
  };
} } }

#endif


/*
  mallocMC: Memory Allocator for Many Core Architectures.
  http://www.icg.tugraz.at/project/mvp

  Copyright (C) 2012 Institute for Computer Graphics and Vision,
                     Graz University of Technology
  Copyright (C) 2014 Institute of Radiation Physics,
                     Helmholtz-Zentrum Dresden - Rossendorf

  Author(s):  Markus Steinberger - steinberger ( at ) icg.tugraz.at
              Rene Widera - r.widera ( at ) hzdr.de
              Axel Huebl - a.huebl ( at ) hzdr.de
              Carlchristian Eckert - c.eckert ( at ) hzdr.de

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

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

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

#pragma once

#include <boost/mpl/bool.hpp>
#include <boost/mpl/int.hpp>

namespace mallocMC{
namespace CreationPolicies{
namespace ScatterConf{
  struct DefaultScatterConfig{
    typedef boost::mpl::int_<4096>  pagesize;
    typedef boost::mpl::int_<8>     accessblocks;
    typedef boost::mpl::int_<16>    regionsize;
    typedef boost::mpl::int_<2>     wastefactor;
    typedef boost::mpl::bool_<false> resetfreedpages;
  };

  struct DefaultScatterHashingParams{
    typedef boost::mpl::int_<38183> hashingK;
    typedef boost::mpl::int_<17497> hashingDistMP;
    typedef boost::mpl::int_<1>     hashingDistWP;
    typedef boost::mpl::int_<1>     hashingDistWPRel;
  };
}

  /**
   * @brief fast memory allocation based on ScatterAlloc
   *
   * This CreationPolicy implements a fast memory allocator that trades speed
   * for fragmentation of memory. This is based on the memory allocator
   * "ScatterAlloc"
   * (http://ieeexplore.ieee.org/xpl/articleDetails.jsp?arnumber=6339604), and
   * is extended to report free memory slots of a given size (both on host and
   * accelerator).
   * To work properly, this policy class requires a pre-allocated heap on the
   * accelerator and works only with Nvidia CUDA capable accelerators that have
   * at least compute capability 2.0.
   *
   * @tparam T_Config (optional) configure the heap layout. The
   *        default can be obtained through Scatter<>::HeapProperties
   * @tparam T_Hashing (optional) configure the parameters for
   *        the hashing formula. The default can be obtained through
   *        Scatter<>::HashingProperties
   */
  template<
  class T_Config = ScatterConf::DefaultScatterConfig,
  class T_Hashing = ScatterConf::DefaultScatterHashingParams
  >
  class Scatter;

}// namespace CreationPolicies
}// namespace mallocMC


#include "dynet/nodes.h"
#include "dynet/dynet.h"
#include "dynet/training.h"
#include "dynet/timing.h"
#include "dynet/rnn.h"
#include "dynet/gru.h"
#include "dynet/lstm.h"
#include "dynet/fast-lstm.h"
#include "dynet/expr.h"

#include <iostream>
#include <fstream>
#include <sstream>
#include <type_traits>
#include <sys/types.h>
#include <sys/stat.h>

#include <boost/archive/text_iarchive.hpp>
#include <boost/archive/text_oarchive.hpp>
#include <boost/program_options.hpp>

#include "s2s/dynet/dict.h"
#include "s2s/corpus/options.hpp"
#include "s2s/corpus/corpora.hpp"
#include "s2s/corpus/dicts.hpp"
#include "s2s/corpus/batch.hpp"

#include "word2vec/readbin.hpp"

#ifndef INCLUDE_GUARD_ENCODER_DECODER_ENCODER_HPP
#define INCLUDE_GUARD_ENCODER_DECODER_ENCODER_HPP

namespace s2s {

class encoder_decoder {

public:

    dynet::VanillaLSTMBuilder rev_enc_builder;
    dynet::VanillaLSTMBuilder fwd_enc_builder;

    dynet::VanillaLSTMBuilder dec_builder;

    dynet::VanillaLSTMBuilder rev_char_enc_builder;
    dynet::VanillaLSTMBuilder fwd_char_enc_builder;

    dynet::LookupParameter p_word_enc;
    dynet::LookupParameter p_char_enc;
    std::vector<dynet::LookupParameter> p_feat_enc;
    std::vector<dynet::Parameter> p_dec_init_bias;
    std::vector<dynet::Parameter> p_dec_init_w;
    std::vector<std::vector<dynet::Parameter> > p_layer_w;
    dynet::Parameter p_add_W;
    dynet::Parameter p_add_bias;
    dynet::Parameter p_out_R;
    dynet::Parameter p_out_bias;

    const s2s_options& opts;
    dicts& d;

    std::vector<dynet::Expression> h_lookup;

    bool flag_drop_out;
    unsigned int slen;
    unsigned int batch_size;

    explicit encoder_decoder(dynet::ParameterCollection& model, const s2s_options& opts, dicts& d) : opts(opts),d(d) {

        flag_drop_out = true;

        unsigned int enc_input_size = 0;

        assert(opts.enc_feature_vocab_size.size() == opts.enc_feature_vec_size.size());
        //  word embedding
        if(opts.lookup_type == "word" || opts.lookup_type == "word+char"){
            p_word_enc = model.add_lookup_parameters(d.dict_set_src.d_word.size(), {opts.enc_word_vec_size});
            enc_input_size += opts.enc_word_vec_size;
        }
        // charactor embedding
        if(opts.lookup_type == "char" || opts.lookup_type == "word+char"){
            enc_input_size += opts.char_rnn_size * 2;
            p_char_enc = model.add_lookup_parameters(d.dict_set_src.d_char.size(), {opts.enc_char_vec_size});
        }
        // feature embedding
        for(unsigned int feat_id = 0; feat_id < opts.enc_feature_vec_size.size(); feat_id++){
            p_feat_enc.push_back(model.add_lookup_parameters(d.dict_set_src.d_feat.at(feat_id).size(), {opts.enc_feature_vec_size.at(feat_id)}));
            enc_input_size += opts.enc_feature_vec_size.at(feat_id);
        }
        // elmo style embedding
        p_layer_w.resize(opts.elmo_hdf5_layers.size());
        for(unsigned int eid = 0; eid < p_layer_w.size(); eid++){
            p_layer_w[eid].resize(opts.elmo_hdf5_layers.at(eid));
            for(unsigned int lid = 0; lid < p_layer_w.at(eid).size(); lid++){
                p_layer_w[eid][lid] = model.add_parameters({opts.elmo_hdf5_dims.at(eid)});
            }
            enc_input_size += opts.elmo_hdf5_dims.at(eid);
        }
        // charactor level LSTM
        rev_char_enc_builder = dynet::VanillaLSTMBuilder(
            opts.char_num_layers,
            opts.enc_char_vec_size,
            opts.char_rnn_size,
            model
        );
        fwd_char_enc_builder = dynet::VanillaLSTMBuilder(
            opts.char_num_layers,
            opts.enc_char_vec_size,
            opts.char_rnn_size,
            model
        );
        // word level LSTM
        rev_enc_builder = dynet::VanillaLSTMBuilder(
            opts.num_layers,
            enc_input_size,
            opts.rnn_size,
            model
        );
        fwd_enc_builder = dynet::VanillaLSTMBuilder(
            opts.num_layers,
            enc_input_size,
            opts.rnn_size,
            model
        );
    }

    void init (const batch &batch_local, dynet::ComputationGraph& cg){
        batch_size = batch_local.batch_size();
        if(opts.lookup_type == "word" || opts.lookup_type == "word+char" || opts.lookup_type == "none"){
            slen = batch_local.sent_word.size();
        }else if(opts.lookup_type == "char"){
            slen = batch_local.sent_char.size();
        }else{
            assert(false);
        }
    }

    void embedding (const batch &batch_local, dynet::ComputationGraph& cg){

        h_lookup.resize(slen);

        std::vector<std::vector<dynet::Expression> > vec_lookup(slen);
        // word
        if(opts.lookup_type == "word" || opts.lookup_type == "word+char"){
            for(unsigned int t_i = 0; t_i < slen; t_i++){
                for(unsigned int b_i = 0; b_i < batch_size; b_i++){
                    if(batch_local.sent_word.at(t_i).at(b_i) >= p_word_enc.dim().d[1]){
                        std::cerr << "0 < " << batch_local.sent_word.at(t_i).at(b_i) << " < " << p_word_enc.dim().d[1] << std::endl;
                        assert(false);
                    }
                }
                vec_lookup[t_i].push_back(lookup(cg, p_word_enc, batch_local.sent_word[t_i]));
            }
        }else if(opts.lookup_type != "char" and opts.lookup_type != "none"){
            assert(false);
        }
        // char
        if(opts.lookup_type == "char" || opts.lookup_type == "word+char"){
            fwd_char_enc_builder.new_graph(cg);
            rev_char_enc_builder.new_graph(cg);
            for(unsigned int t_i = 0; t_i < slen; t_i++){
                fwd_char_enc_builder.start_new_sequence();
                rev_char_enc_builder.start_new_sequence();
                if(flag_drop_out == true && opts.dropout_rate_lstm_char > 0.f){
                    fwd_char_enc_builder.set_dropout_masks(batch_size);
                    rev_char_enc_builder.set_dropout_masks(batch_size);
                }
                unsigned int char_len = batch_local.sent_char[t_i].size();
                for(unsigned int c_i = 0; c_i < char_len; c_i++){
                    // std::cerr << __LINE__ << ", " << c_i << std::endl;
                    for(unsigned int b_i = 0; b_i < batch_size; b_i++){
                        if(batch_local.sent_char.at(t_i).at(c_i).at(b_i) >= p_char_enc.dim().d[1]){
                            std::cerr << "0 < " << batch_local.sent_char.at(t_i).at(c_i).at(b_i) << " < " << p_char_enc.dim().d[1] << std::endl;
                            assert(false);
                        }
                    }
                    fwd_char_enc_builder.add_input(lookup(cg, p_char_enc, batch_local.sent_char[t_i][c_i]));
                }
                for(unsigned int c_i_d = 0; c_i_d < char_len; c_i_d++){
                    unsigned int c_i = (char_len - 1) - c_i_d;
                    // std::cerr << __LINE__ << ", " << c_i << std::endl;
                    for(unsigned int b_i = 0; b_i < batch_size; b_i++){
                        if(batch_local.sent_char.at(t_i).at(c_i).at(b_i) >= p_char_enc.dim().d[1]){
                            std::cerr << "0 < " << batch_local.sent_char.at(t_i).at(c_i).at(b_i) << " < " << p_char_enc.dim().d[1] << std::endl;
                            assert(false);
                        }
                    }
                    rev_char_enc_builder.add_input(lookup(cg, p_char_enc, batch_local.sent_char[t_i][c_i]));
                }
                vec_lookup[t_i].push_back(concatenate(std::vector<dynet::Expression>({fwd_char_enc_builder.back(), rev_char_enc_builder.back()})));
            }
        }else if(opts.lookup_type != "word" and opts.lookup_type != "none"){
            assert(false);
        }
        // feature
        for(unsigned int f_id = 0; f_id < batch_local.sent_feat.size(); f_id++){
            for(unsigned int tok_pos = 0; tok_pos < slen; tok_pos++){
                for(unsigned int b_i = 0; b_i < batch_size; b_i++){
                    if(batch_local.sent_feat.at(f_id).at(tok_pos).at(b_i) >= p_feat_enc.at(f_id).dim().d[1]){
                        std::cerr << "0 < " << batch_local.sent_feat.at(f_id).at(tok_pos).at(b_i) << " < " << p_feat_enc.at(f_id).dim().d[1] << std::endl;
                        assert(false);
                    }
                }
                vec_lookup[tok_pos].push_back(lookup(cg, p_feat_enc[f_id], batch_local.sent_feat[f_id][tok_pos]));
            }
        }
        // elmo style embeddings
        // elmo_type, token_id, sentence_id, layer_id, float_vector
        for(unsigned int tid = 0; tid < batch_local.sent_elmo.at(0).size(); tid++){
            for(unsigned int eid = 0; eid < batch_local.sent_elmo.size(); eid++){
                const unsigned int dim_size = opts.elmo_hdf5_dims.at(eid);
                const unsigned int layer_size = opts.elmo_hdf5_layers.at(eid);
                const unsigned int slen = batch_local.sent_elmo.at(eid).at(tid).size();
                // extract elmo layers
                std::vector<float> layers;
                layers.reserve(dim_size*layer_size*batch_size);
                for(unsigned int sid = 0; sid < slen; sid++){
                    for(unsigned int lid = 0; lid < opts.elmo_hdf5_layers[eid]; lid++){
                        const std::vector<float> &flt_vec = batch_local.sent_elmo.at(eid).at(tid).at(sid).at(lid);
                        layers.insert(layers.end(), flt_vec.begin(), flt_vec.end());
                    }
                }
                assert(layers.size() == dim_size*layer_size*batch_size);
                dynet::Expression i_elmo_layers = dynet::input(cg, dynet::Dim({layer_size, dim_size}, batch_size), layers);
                // calculate weights for each layer
                std::vector<dynet::Expression> elmo_weights;
                for(unsigned int lid = 0; lid < opts.elmo_hdf5_layers[eid]; lid++){
                    std::vector<float> layer;
                    layer.reserve(dim_size*batch_size);
                    for(unsigned int sid = 0; sid < slen; sid++){
                        const std::vector<float> &flt_vec = batch_local.sent_elmo.at(eid).at(tid).at(sid).at(lid);
                        layer.insert(layer.end(), flt_vec.begin(), flt_vec.end());
                    }
                    assert(layer.size() == dim_size*batch_size);
                    dynet::Expression i_elmo_layer = dynet::input(cg, dynet::Dim({dim_size}, batch_size), layer);
                    elmo_weights.push_back(transpose(parameter(cg, p_layer_w[eid][lid])) * i_elmo_layer);
                }
                dynet::Expression i_elmo_weights = concatenate_cols(elmo_weights);
                // calculate weighted sum
                dynet::Expression i_elmo_alpha = softmax(i_elmo_weights);
                dynet::Expression i_elmo_weighted_layers = i_elmo_alpha * i_elmo_layers;
                vec_lookup[tid].push_back(transpose(i_elmo_weighted_layers));
            }
        }
        // concatenate all input vectors
        for(unsigned int tok_pos = 0; tok_pos < slen; tok_pos++){
            h_lookup[tok_pos] = concatenate(vec_lookup[tok_pos]);
        }
    }

    virtual void encoder(const batch &batch_local, dynet::ComputationGraph& cg)=0;

    virtual void disable_dropout()=0;

    virtual void enable_dropout()=0;

    void read_pre_train_src(const std::string file_name, dynet::Dict& d_word_src){
        std::unordered_map<unsigned int, std::vector<float > > init_lookup;
        unsigned int embed_size = p_word_enc.dim().d[0];
        word2vec::readbin(init_lookup, file_name, d_word_src, embed_size);
        for(std::pair<unsigned int, std::vector<float> > p : init_lookup){
            p_feat_enc[0].initialize(p.first, p.second);
        }
    }

};

}

#endif


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

#include <vw/Math/Matrix.h>
#include <vw/Core/Exception.h>

#include <vw/FileIO/DiskImageResourcePDS.h>
#include <vw/Cartography/GeoReferenceResourcePDS.h>
#include <vw/Cartography/Datum.h>
#include <vw/Cartography/GeoReference.h>

// For boost::split
#include <boost/algorithm/string.hpp>

namespace vw {
namespace cartography {

  bool read_pds_georeference( GeoReference& georef, DiskImageResourcePDS const& resource ) {

    std::string key, value;
    bool failed = false;

    Datum datum;
    datum.name() = "PDS Datum";
    datum.spheroid_name() = "PDS Spheroid";
    // Collect datum information
    key = "A_AXIS_RADIUS";
    if ( resource.query(key,value) ) { datum.set_semi_major_axis(atof(value.c_str()) * 1000); } else {failed = true; }
    key = "B_AXIS_RADIUS";
    if ( resource.query(key,value) ) { datum.set_semi_minor_axis(atof(value.c_str()) * 1000); } else {failed = true; }
    georef.set_datum(datum);

    // Collect projection information
    key = "MAP_PROJECTION_TYPE";
    if ( resource.query(key,value) ) {
      if (value != "\"SIMPLE CYLINDRICAL\"") {
        vw_out(ErrorMessage, "console") << "Unsupported map projection type in PDS header.";
        vw_out(ErrorMessage, "cartography") << "Unsupported map projection type in PDS header.";
        failed = true;
      }
      datum.set_semi_minor_axis(atof(value.c_str()) * 1000);
    } else {failed = true; }

    // Set affine transform
    Matrix<double,3,3> transform;
    transform.set_identity();

    key = "WESTERNMOST_LONGITUDE";
    if ( resource.query(key,value) ) { transform(0,2) = atof(value.c_str()); } else {failed = true; }
    key = "MAXIMUM_LATITUDE";
    if ( resource.query(key,value) ) { transform(1,2) = atof(value.c_str()); } else {failed = true; }
    key = "MAP_RESOLUTION";
    if ( resource.query(key,value) ) {
      transform(0,0) = 1/atof(value.c_str());
      transform(1,1) = -1/atof(value.c_str());
    } else {failed = true; }

    georef.set_transform(transform);

    return !failed;
  }

}} // namespace vw::cartography


// Example test program for 2D LIDAR alignment
// using the FRSM library. It uses pretty brute force
// settings
//
// Example with attached data sample:
// testRegister2D -a scan_00.csv  -b scan_01.csv  -c scan_02.csv

#include <zlib.h>
#include <lcm/lcm-cpp.hpp>

#include <lcmtypes/bot_core.hpp>
#include <boost/shared_ptr.hpp>
#include <lidar-odom/lidar-odometry.hpp>
#include <lcmtypes/bot_core.hpp>
#include <ConciseArgs>

#include <iterator>
#include <sstream>

struct CommandLineConfig
{
  std::string filenameA;
  std::string filenameB;
  std::string filenameC;
};

class App{
  public:
    App(boost::shared_ptr<lcm::LCM> &lcm_, const CommandLineConfig& cl_cfg_);

    ~App(){
    }

    void doWork();

  private:
    boost::shared_ptr<lcm::LCM> lcm_;
    const CommandLineConfig cl_cfg_;
    LidarOdom* lidarOdom_;
};

App::App(boost::shared_ptr<lcm::LCM> &lcm_, const CommandLineConfig& cl_cfg_) :
       lcm_(lcm_), cl_cfg_(cl_cfg_){

  LidarOdomConfig lo_cfg = LidarOdomConfig();

  // Overwrite default config values with more brute force search:
  lo_cfg.matchingMode = FRSM_GRID_COORD;
  lo_cfg.initialSearchRangeXY = 1.7;
  lo_cfg.initialSearchRangeTheta = 1.7;
  lo_cfg.maxSearchRangeXY = 2.0;
  lo_cfg.maxSearchRangeTheta = 2.0;

  lidarOdom_ = new LidarOdom(lcm_, lo_cfg);
}

void readCSVFile(std::string filename, std::vector<float> &x, std::vector<float> &y){

    std::vector<std::vector<double> > values;
    std::ifstream fin(filename.c_str());
    for (std::string line; std::getline(fin, line); )
    {
        std::replace(line.begin(), line.end(), ',', ' ');
        std::istringstream in(line);
        values.push_back(
            std::vector<double>(std::istream_iterator<double>(in),
                                std::istream_iterator<double>()));
    }

   for (size_t i=0; i<values.size(); i++){
     x.push_back(values[i][0]);
     y.push_back(values[i][1]);
   }
}

void App::doWork(){

  std::vector<float> xA;
  std::vector<float> yA;
  readCSVFile(cl_cfg_.filenameA, xA, yA);
  std::cout << xA.size() << " points in File A\n";
  lidarOdom_->doOdometry(xA, yA, xA.size(), 0);

  // Match second scan without a prior
  std::string i;
  cout << "Match B: Continue? ";
  cin >> i;

  std::vector<float> xB;
  std::vector<float> yB;
  readCSVFile(cl_cfg_.filenameB, xB, yB);
  std::cout << xB.size() << " points in File B\n";
  lidarOdom_->doOdometry(xB, yB, xB.size(), 1);

  // Match third scan giving a prior rotation for the heading
  // this alignment would otherwise fail:
  cout << "Match C: Continue? ";
  cin >> i;

  std::vector<float> xC;
  std::vector<float> yC;
  readCSVFile(cl_cfg_.filenameC, xC, yC);
  std::cout << xC.size() << " points in File C\n";

  ScanTransform prior;
  prior.x = 0;
  prior.y = 0;
  prior.theta = 1.7;
  lidarOdom_->doOdometry(xC, yC, xC.size(), 2, &prior);

  // 2. Determine the body position using the LIDAR motion estimate:
  Eigen::Isometry3d pose = lidarOdom_->getCurrentPose();
  Eigen::Quaterniond orientation(pose.rotation());

  Eigen::Vector3d rpy = orientation.matrix().eulerAngles(0,1,2);

  std::cout << "\n";
  std::cout << "x,y,yaw (m,m,deg): "<< pose.translation().x() << ", " << pose.translation().y()
            << ", "
            << rpy[2]*180/M_PI << "\n";
}

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

  CommandLineConfig cl_cfg;
  ConciseArgs parser(argc, argv, "simple-fusion");
  parser.add(cl_cfg.filenameA, "a", "filenameA", "FilenameA.csv");
  parser.add(cl_cfg.filenameB, "b", "filenameB", "FilenameB.csv");
  parser.add(cl_cfg.filenameC, "c", "filenameC", "FilenameC.csv");
  parser.parse();

  boost::shared_ptr<lcm::LCM> lcm(new lcm::LCM);
  if(!lcm->good()){
    std::cerr <<"ERROR: lcm is not good()" <<std::endl;
  }
  App app= App(lcm, cl_cfg);

  app.doWork();
}