#include <stella_vslam_ros.h>
#include <stella_vslam/publish/map_publisher.h>

#include <chrono>

#include <tf2_eigen/tf2_eigen.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <geometry_msgs/msg/transform_stamped.h>
#include <opencv2/core/core.hpp>
#include <opencv2/imgcodecs.hpp>
#include <Eigen/Geometry>

namespace stella_vslam_ros {
system::system(const std::shared_ptr<stella_vslam::config>& cfg, const std::string& vocab_file_path, const std::string& mask_img_path)
    : SLAM_(cfg, vocab_file_path), cfg_(cfg), node_(std::make_shared<rclcpp::Node>("run_slam")), custom_qos_(rmw_qos_profile_default),
      mask_(mask_img_path.empty() ? cv::Mat{} : cv::imread(mask_img_path, cv::IMREAD_GRAYSCALE)),
      pose_pub_(node_->create_publisher<nav_msgs::msg::Odometry>("~/camera_pose", 1)),
      map_to_odom_broadcaster_(std::make_shared<tf2_ros::TransformBroadcaster>(node_)),
      tf_(std::make_unique<tf2_ros::Buffer>(node_->get_clock())),
      transform_listener_(std::make_shared<tf2_ros::TransformListener>(*tf_)) {
    custom_qos_.depth = 1;
    exec_.add_node(node_);
    init_pose_sub_ = node_->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
        "/initialpose", 1,
        std::bind(&system::init_pose_callback,
                  this, std::placeholders::_1));
    setParams();
}

void system::publish_pose(const Eigen::Matrix4d& cam_pose_wc, const rclcpp::Time& stamp) {
    // Extract rotation matrix and translation vector from
    Eigen::Matrix3d rot(cam_pose_wc.block<3, 3>(0, 0));
    Eigen::Translation3d trans(cam_pose_wc.block<3, 1>(0, 3));
    Eigen::Affine3d map_to_camera_affine(trans * rot);
    Eigen::AngleAxisd rot_ros_to_cv_map_frame(
        (Eigen::Matrix3d() << 0, 0, 1,
         -1, 0, 0,
         0, -1, 0)
            .finished());

    // Transform map frame from CV coordinate system to ROS coordinate system
    map_to_camera_affine.prerotate(rot_ros_to_cv_map_frame);

    // Create odometry message and update it with current camera pose
    nav_msgs::msg::Odometry pose_msg;
    pose_msg.header.stamp = stamp;
    pose_msg.header.frame_id = map_frame_;
    pose_msg.child_frame_id = camera_frame_;
    pose_msg.pose.pose = tf2::toMsg(map_to_camera_affine * rot_ros_to_cv_map_frame.inverse());
    pose_pub_->publish(pose_msg);

    // Send map->odom transform. Set publish_tf to false if not using TF
    if (publish_tf_) {
        try {
            auto camera_to_odom = tf_->lookupTransform(
                camera_optical_frame_, odom_frame_, tf2_ros::fromMsg(builtin_interfaces::msg::Time(stamp)),
                tf2::durationFromSec(0.0));
            Eigen::Affine3d camera_to_odom_affine = tf2::transformToEigen(camera_to_odom.transform);

            auto map_to_odom_msg = tf2::eigenToTransform(map_to_camera_affine * camera_to_odom_affine);
            tf2::TimePoint transform_timestamp = tf2_ros::fromMsg(stamp) + tf2::durationFromSec(transform_tolerance_);
            map_to_odom_msg.header.stamp = tf2_ros::toMsg(transform_timestamp);
            map_to_odom_msg.header.frame_id = map_frame_;
            map_to_odom_msg.child_frame_id = odom_frame_;
            map_to_odom_broadcaster_->sendTransform(map_to_odom_msg);
        }
        catch (tf2::TransformException& ex) {
            RCLCPP_ERROR_STREAM_THROTTLE(node_->get_logger(), *node_->get_clock(), 1000, "Transform failed: " << ex.what());
        }
    }
}

void system::setParams() {
    odom_frame_ = std::string("odom");
    odom_frame_ = node_->declare_parameter("odom_frame", odom_frame_);

    map_frame_ = std::string("map");
    map_frame_ = node_->declare_parameter("map_frame", map_frame_);

    base_link_ = std::string("base_footprint");
    base_link_ = node_->declare_parameter("base_link", base_link_);

    camera_frame_ = std::string("camera_frame");
    camera_frame_ = node_->declare_parameter("camera_frame", camera_frame_);

    // Set publish_tf to false if not using TF
    publish_tf_ = true;
    publish_tf_ = node_->declare_parameter("publish_tf", publish_tf_);

    // Publish pose's timestamp in the future
    transform_tolerance_ = 0.5;
    transform_tolerance_ = node_->declare_parameter("transform_tolerance", transform_tolerance_);
}

void system::init_pose_callback(
    const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) {
    if (camera_optical_frame_.empty()) {
        RCLCPP_ERROR(node_->get_logger(),
                     "Camera link is not set: no images were received yet");
        return;
    }

    Eigen::Translation3d trans(
        msg->pose.pose.position.x,
        msg->pose.pose.position.y,
        msg->pose.pose.position.z);
    Eigen::Quaterniond rot_q(
        msg->pose.pose.orientation.w,
        msg->pose.pose.orientation.x,
        msg->pose.pose.orientation.y,
        msg->pose.pose.orientation.z);
    Eigen::Affine3d initialpose_affine(trans * rot_q);

    Eigen::Matrix3d rot_cv_to_ros_map_frame;
    rot_cv_to_ros_map_frame << 0, -1, 0,
        0, 0, -1,
        1, 0, 0;

    Eigen::Affine3d map_to_initialpose_frame_affine;
    try {
        auto map_to_initialpose_frame = tf_->lookupTransform(
            map_frame_, msg->header.frame_id, tf2_ros::fromMsg(msg->header.stamp),
            tf2::durationFromSec(0.0));
        map_to_initialpose_frame_affine = tf2::transformToEigen(
            map_to_initialpose_frame.transform);
    }
    catch (tf2::TransformException& ex) {
        RCLCPP_ERROR_STREAM_THROTTLE(node_->get_logger(), *node_->get_clock(), 1000, "Transform failed: " << ex.what());
        return;
    }

    Eigen::Affine3d base_link_to_camera_affine;
    try {
        auto base_link_to_camera = tf_->lookupTransform(
            base_link_, camera_optical_frame_, tf2_ros::fromMsg(msg->header.stamp),
            tf2::durationFromSec(0.0));
        base_link_to_camera_affine = tf2::transformToEigen(base_link_to_camera.transform);
    }
    catch (tf2::TransformException& ex) {
        RCLCPP_ERROR_STREAM_THROTTLE(node_->get_logger(), *node_->get_clock(), 1000, "Transform failed: " << ex.what());
        return;
    }

    // Target transform is map_cv -> camera_link and known parameters are following:
    //   rot_cv_to_ros_map_frame: T(map_cv -> map)
    //   map_to_initialpose_frame_affine: T(map -> `msg->header.frame_id`)
    //   initialpose_affine: T(`msg->header.frame_id` -> base_link)
    //   base_link_to_camera_affine: T(base_link -> camera_link)
    // The flow of the transformation is as follows:
    //   map_cv -> map -> `msg->header.frame_id` -> base_link -> camera_link
    Eigen::Matrix4d cam_pose_cv = (rot_cv_to_ros_map_frame * map_to_initialpose_frame_affine
                                   * initialpose_affine * base_link_to_camera_affine)
                                      .matrix();

    const Eigen::Vector3d normal_vector = (Eigen::Vector3d() << 0., 1., 0.).finished();
    if (!SLAM_.relocalize_by_pose_2d(cam_pose_cv, normal_vector)) {
        RCLCPP_ERROR(node_->get_logger(), "Can not set initial pose");
    }
}

mono::mono(const std::shared_ptr<stella_vslam::config>& cfg, const std::string& vocab_file_path, const std::string& mask_img_path)
    : system(cfg, vocab_file_path, mask_img_path) {
    sub_ = image_transport::create_subscription(
        node_.get(), "camera/image_raw", [this](const sensor_msgs::msg::Image::ConstSharedPtr& msg) { callback(msg); }, "raw", custom_qos_);
}
void mono::callback(const sensor_msgs::msg::Image::ConstSharedPtr& msg) {
    if (camera_optical_frame_.empty()) {
        camera_optical_frame_ = msg->header.frame_id;
    }
    const rclcpp::Time tp_1 = node_->now();
    const double timestamp = tp_1.seconds();

    // input the current frame and estimate the camera pose
    auto cam_pose_wc = SLAM_.feed_monocular_frame(cv_bridge::toCvShare(msg)->image, timestamp, mask_);

    const rclcpp::Time tp_2 = node_->now();
    const double track_time = (tp_2 - tp_1).seconds();

    //track times in seconds
    track_times_.push_back(track_time);

    if (cam_pose_wc) {
        publish_pose(*cam_pose_wc, msg->header.stamp);
    }
}

stereo::stereo(const std::shared_ptr<stella_vslam::config>& cfg, const std::string& vocab_file_path, const std::string& mask_img_path,
               const bool rectify)
    : system(cfg, vocab_file_path, mask_img_path),
      rectifier_(rectify ? std::make_shared<stella_vslam::util::stereo_rectifier>(cfg) : nullptr),
      left_sf_(node_, "camera/left/image_raw"),
      right_sf_(node_, "camera/right/image_raw") {
    use_exact_time_ = false;
    use_exact_time_ = node_->declare_parameter("use_exact_time", use_exact_time_);
    if (use_exact_time_) {
        exact_time_sync_ = std::make_shared<ExactTimeSyncPolicy::Sync>(2, left_sf_, right_sf_);
        exact_time_sync_->registerCallback(&stereo::callback, this);
    }
    else {
        approx_time_sync_ = std::make_shared<ApproximateTimeSyncPolicy::Sync>(10, left_sf_, right_sf_);
        approx_time_sync_->registerCallback(&stereo::callback, this);
    }
}

void stereo::callback(const sensor_msgs::msg::Image::ConstSharedPtr& left, const sensor_msgs::msg::Image::ConstSharedPtr& right) {
    if (camera_optical_frame_.empty()) {
        camera_optical_frame_ = left->header.frame_id;
    }
    auto leftcv = cv_bridge::toCvShare(left)->image;
    auto rightcv = cv_bridge::toCvShare(right)->image;
    if (leftcv.empty() || rightcv.empty()) {
        return;
    }

    if (rectifier_) {
        rectifier_->rectify(leftcv, rightcv, leftcv, rightcv);
    }

    const rclcpp::Time tp_1 = node_->now();
    const double timestamp = tp_1.seconds();

    // input the current frame and estimate the camera pose
    auto cam_pose_wc = SLAM_.feed_stereo_frame(leftcv, rightcv, timestamp, mask_);

    const rclcpp::Time tp_2 = node_->now();
    const double track_time = (tp_2 - tp_1).seconds();

    //track times in seconds
    track_times_.push_back(track_time);

    if (cam_pose_wc) {
        publish_pose(*cam_pose_wc, left->header.stamp);
    }
}

rgbd::rgbd(const std::shared_ptr<stella_vslam::config>& cfg, const std::string& vocab_file_path, const std::string& mask_img_path)
    : system(cfg, vocab_file_path, mask_img_path),
      color_sf_(node_, "camera/color/image_raw"),
      depth_sf_(node_, "camera/depth/image_raw") {
    use_exact_time_ = false;
    use_exact_time_ = node_->declare_parameter("use_exact_time", use_exact_time_);
    if (use_exact_time_) {
        exact_time_sync_ = std::make_shared<ExactTimeSyncPolicy::Sync>(2, color_sf_, depth_sf_);
        exact_time_sync_->registerCallback(&rgbd::callback, this);
    }
    else {
        approx_time_sync_ = std::make_shared<ApproximateTimeSyncPolicy::Sync>(10, color_sf_, depth_sf_);
        approx_time_sync_->registerCallback(&rgbd::callback, this);
    }
}

void rgbd::callback(const sensor_msgs::msg::Image::ConstSharedPtr& color, const sensor_msgs::msg::Image::ConstSharedPtr& depth) {
    if (camera_optical_frame_.empty()) {
        camera_optical_frame_ = color->header.frame_id;
    }
    auto colorcv = cv_bridge::toCvShare(color)->image;
    auto depthcv = cv_bridge::toCvShare(depth)->image;
    if (colorcv.empty() || depthcv.empty()) {
        return;
    }
    if (depthcv.type() == CV_32FC1) {
        cv::patchNaNs(depthcv);
    }

    const rclcpp::Time tp_1 = node_->now();
    const double timestamp = tp_1.seconds();

    // input the current frame and estimate the camera pose
    auto cam_pose_wc = SLAM_.feed_RGBD_frame(colorcv, depthcv, timestamp, mask_);

    const rclcpp::Time tp_2 = node_->now();
    const double track_time = (tp_2 - tp_1).seconds();

    // track time in seconds
    track_times_.push_back(track_time);

    if (cam_pose_wc) {
        publish_pose(*cam_pose_wc, color->header.stamp);
    }
}

} // namespace stella_vslam_ros


//---------------------------------------------------------------------------//
// Copyright (c) 2018-2020 Mikhail Komarov <nemo@nil.foundation>
//
// 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 CRYPTO3_BLOCK_AES_HPP
#define CRYPTO3_BLOCK_AES_HPP

#include <boost/crypto3/block/rijndael.hpp>

namespace boost {
    namespace crypto3 {
        namespace block {
            /*!
             * @brief AES block cipher. Equals to Rijndael block cipher with 128 bit block length.
             */
            template<std::size_t KeyBits>
            using aes = rijndael<KeyBits, 128>;
        }    // namespace block
    }        // namespace crypto3
}    // namespace boost

#endif    // CRYPTO3_AES_HPP


#include "profile_2d.hpp"
//#include "profile_python.hpp"
#include <boost/python.hpp>
#include <pyublas/numpy.hpp>

namespace gd {
using namespace boost::python;

template<class ProfileType, class Ctor>
	void py_export_profile_potential_axi(string name) {
		class_<ProfileType, bases<Potential2d> >(name.c_str(), Ctor())
			.def("potentialrhoz", (&ProfileType::potentialxy))
			.def("potentialxy_eff", (&ProfileType::potentialxy_eff))
			;
	}
template<class ProfileType, class Ctor>
	void py_export_profile_profile_axi(string name) {
		class_<ProfileType, bases<Profile2d> >(name.c_str(), Ctor())
			.def("densityxy", (&ProfileType::densityxy))
			.def("potentialxy", (&ProfileType::potentialxy))
			.def("potentialxy_eff", (&ProfileType::potentialxy_eff))
			.def("dphidx", (&ProfileType::dphidx))
			.def("dphidy", (&ProfileType::dphidy))
			.def("dphidx_eff", (&ProfileType::dphidx_eff))
			.def("dphidy_eff", (&ProfileType::dphidy_eff))
			;
	}

void py_export_profile_2d() {
	class_< Potential2d, boost::noncopyable >("Potential2d", no_init);
	class_< Profile2d, boost::noncopyable >("Profile2d", no_init);
	class_< ProfileModel2d, boost::noncopyable >("ProfileModel", no_init)
		;
	class_<ProfileModel2d1C, bases<ProfileModel2d>, boost::noncopyable  >("ProfileModel2d1C", init<Profile2d*>())
		.def("densityxy", (&ProfileModel2d1C::densityxy))
		.def("potentialxy", (&ProfileModel2d1C::potentialxy))
		.def("potentialxy_eff", (&ProfileModel2d1C::potentialxy_eff))
		.def("dphidx", (&ProfileModel2d1C::dphidx))
		.def("dphidy", (&ProfileModel2d1C::dphidy))
		.def("dphidx_eff", (&ProfileModel2d1C::dphidx_eff))
		.def("dphidy_eff", (&ProfileModel2d1C::dphidy_eff))
		;
	/*class_<ProfileModel2C, bases<ProfileModel>, boost::noncopyable  >("ProfileModel2C", init<Profile*,Profile*>())
		.def("densityr", (&ProfileModel2C::densityr))
		.def("dphidr", (&ProfileModel2C::dphidr))
		.def("potentialr", (&ProfileModel2C::potentialr))
		;

	*/
		//.def("densityr", pure_virtual(&Profile::densityr))
	/*class_< Density2d, boost::noncopyable >("Density2d", no_init)
		//.def("densityr", pure_virtual(&Density::densityr))
		;*/
	py_export_profile_profile_axi<Logarithmic2d, init<double, double, double, double>>("Logarithmic2d");

}

}


#include <boost/numeric/conversion/int_float_mixture.hpp>


/* Copyright (c) 2017, United States Government, as represented by the
 * Administrator of the National Aeronautics and Space Administration.
 *
 * All rights reserved.
 *
 * The Astrobee platform is 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 <dense_map_utils.h>
#include <depth_from_mesh_utils.h>
#include <localization_common/utilities.h>
#include <texture_processing.h>

#include <mve/mesh.h>

#include <boost/algorithm/string/predicate.hpp>
#include <boost/filesystem.hpp>

#include <glog/logging.h>

namespace dm = dense_map;
namespace fs = boost::filesystem;
namespace lc = localization_common;

namespace dense_map {

boost::optional<double> Depth(const Eigen::Vector3d& sensor_t_ray, const Eigen::Isometry3d& world_T_sensor,
                              const BVHTree& bvh_tree, double min_depth, double max_depth) {
  const Eigen::Vector3d world_F_sensor_t_ray = world_T_sensor.linear() * sensor_t_ray;
  // Create ray centered at world_t_sensor
  BVHTree::Ray bvh_ray;
  bvh_ray.origin = dm::eigen_to_vec3f(world_T_sensor.translation());
  bvh_ray.dir = dm::eigen_to_vec3f(world_F_sensor_t_ray);
  bvh_ray.dir.normalize();
  bvh_ray.tmin = min_depth;
  bvh_ray.tmax = max_depth;

  // Get intersection if it exists
  BVHTree::Hit hit;
  if (bvh_tree.intersect(bvh_ray, &hit))
    return hit.t;
  else
    VLOG(2) << "Failed to get mesh intersection.";
  return boost::none;
}

lc::Time LoadTimestamp(const std::string& filename, const double timestamp_offset) {
  const std::string timestamp_string = filename.substr(0, filename.find("_"));
  if (timestamp_string.empty()) LOG(FATAL) << "Failed to load timestamp from filename.";
  return std::stod(timestamp_string) + timestamp_offset;
}

void LoadTimestampsAndPoses(const std::string& directory, const std::string& sensor_name,
                            std::vector<lc::Time>& timestamps, std::vector<Eigen::Isometry3d>& poses,
                            const Eigen::Isometry3d& poses_sensor_T_sensor, const double timestamp_offset) {
  for (const auto& file : fs::recursive_directory_iterator(directory)) {
    const std::string filename = file.path().filename().string();
    if (boost::algorithm::ends_with(filename, "world.txt") && filename.find(sensor_name) != std::string::npos) {
      timestamps.emplace_back(LoadTimestamp(filename, timestamp_offset));
      Eigen::Affine3d affine_pose;
      if (!dm::readAffine(affine_pose, file.path().string())) {
        LOG(FATAL) << "Failed to read pose for filename " << filename;
      }
      const Eigen::Isometry3d world_T_poses_sensor(affine_pose.matrix());
      poses.push_back(world_T_poses_sensor * poses_sensor_T_sensor);
    }
  }
}

// Assumes each timestamp is on a newline of the file
std::vector<lc::Time> LoadTimestamps(const std::string& timestamps_filename) {
  std::vector<lc::Time> timestamps;
  std::ifstream timestamps_file(timestamps_filename);
  std::string file_line;
  lc::Time timestamp;
  while (std::getline(timestamps_file, file_line)) {
    std::istringstream line_ss(file_line);
    line_ss >> timestamp;
    timestamps.emplace_back(timestamp);
  }
  return timestamps;
}

lc::PoseInterpolater MakePoseInterpolater(const std::string& directory, const Eigen::Isometry3d& body_T_sensor,
                                          const std::vector<std::string>& groundtruth_sensor_names,
                                          const std::vector<Eigen::Isometry3d>& body_T_groundtruth_sensor_vec,
                                          const std::vector<double>& timestamp_offsets) {
  std::vector<lc::Time> timestamps;
  std::vector<Eigen::Isometry3d> poses;
  for (int i = 0; i < groundtruth_sensor_names.size(); ++i) {
    LoadTimestampsAndPoses(directory, groundtruth_sensor_names[i], timestamps, poses,
                           (body_T_groundtruth_sensor_vec[i]).inverse() * body_T_sensor, timestamp_offsets[i]);
  }
  return lc::PoseInterpolater(timestamps, poses);
}

std::vector<Eigen::Vector3d> LoadSensorRays(const std::string& sensor_rays_filename, int& rows) {
  std::vector<Eigen::Vector3d> sensor_t_rays;
  std::ifstream sensor_rays_file(sensor_rays_filename);
  std::string file_line;
  double y, z;
  int line_index = 0;
  while (std::getline(sensor_rays_file, file_line)) {
    std::istringstream line_ss(file_line);
    if (line_index == 0) {
      line_ss >> rows;
    } else {
      line_ss >> y;
      line_ss >> z;
      const Eigen::Vector3d sensor_t_ray(1.0, y, z);
      sensor_t_rays.emplace_back(sensor_t_ray.normalized());
    }
    ++line_index;
  }
  return sensor_t_rays;
}

std::shared_ptr<BVHTree> LoadMeshTree(const std::string& mesh_file) {
  mve::TriangleMesh::Ptr mesh;
  std::shared_ptr<mve::MeshInfo> mesh_info;
  std::shared_ptr<tex::Graph> graph;
  std::shared_ptr<BVHTree> bvh_tree;
  dm::loadMeshBuildTree(mesh_file, mesh, mesh_info, graph, bvh_tree);
  return bvh_tree;
}

std::vector<boost::optional<double>> GetDepthData(const std::vector<lc::Time>& timestamps,
                                                  const std::vector<Eigen::Vector3d>& sensor_t_rays,
                                                  const lc::PoseInterpolater& groundtruth_pose_interpolater,
                                                  const BVHTree& bvh_tree) {
  std::vector<boost::optional<double>> depths;
  for (const auto& timestamp : timestamps) {
    const auto world_T_sensor = groundtruth_pose_interpolater.Interpolate(timestamp);
    if (!world_T_sensor) {
      LOG(ERROR) << "Failed to get groundtruth pose at timstamp " << std::setprecision(20) << timestamp;
      depths.emplace_back(boost::none);
      continue;
    }
    for (const auto& sensor_t_ray : sensor_t_rays) {
      depths.emplace_back(Depth(sensor_t_ray, *world_T_sensor, bvh_tree));
    }
  }
  return depths;
}

void SaveDepthData(const std::vector<lc::Time>& timestamps, const std::vector<Eigen::Vector3d>& sensor_t_rays,
                   const std::vector<boost::optional<double>>& depths, const std::string& output_filename,
                   const int rows) {
  std::ofstream output_file;
  output_file.open(output_filename);
  for (const auto timestamp : timestamps) {
    output_file << std::setprecision(20) << timestamp << " ";
  }
  output_file << std::endl;
  const int cols = depths.size() / (timestamps.size() * rows);
  output_file << cols << std::endl;
  const int depths_per_row = depths.size() / timestamps.size();
  int index = 0;
  for (const auto& depth : depths) {
    if (!depth) {
      output_file << -1 << " ";
    } else {
      output_file << *depth << " ";
    }
    ++index;
    if (index >= depths_per_row) {
      output_file << std::endl;
      index = 0;
    }
  }
  output_file.close();
}
}  // namespace dense_map


#include <Core/Core.h>
#include <Functions4U/Functions4U.h>
#include <Eigen/Eigen.h>
#include "Sundials.h"

namespace Upp {

using namespace Eigen;

#include <kinsol/kinsol.h>           		  /* access to KINSOL func., consts. */
#include <ida/ida.h>                          /* prototypes for IDA fcts., consts.    */
#include <nvector/nvector_serial.h>           /* access to serial N_Vector            */
#include <sunmatrix/sunmatrix_dense.h>        /* access to dense SUNMatrix            */
#include <sunlinsol/sunlinsol_dense.h>        /* access to dense SUNLinearSolver      */
#include <sunnonlinsol/sunnonlinsol_newton.h> /* access to Newton SUNNonlinearSolver  */
#include <sundials/sundials_types.h>          /* defs. of realtype, sunindextype      */
#include <sundials/sundials_math.h>           /* defs. of SUNRabs, SUNRexp, etc.      */

static void CheckMem(void *returnvalue, const char *funcname) {
	if (returnvalue == NULL)
		throw Exc(Format(t_("SUNDIALS_ERROR: %s() failed - returned NULL pointer"), funcname));
}

static void CheckRet(int returnvalue, const char *funcname) {
	if (returnvalue < 0)
	  	throw Exc(Format(t_("SUNDIALS_ERROR: %s() failed with retval = %d"), funcname, returnvalue));
}

static void PrintFinalStats(void *mem) {
	int retval;
	long int nst, nni, nje, nre, nreLS, netf, ncfn, nge;

	retval = IDAGetNumSteps(mem, &nst);
	CheckRet(retval, "IDAGetNumSteps");
	retval = IDAGetNumResEvals(mem, &nre);
	CheckRet(retval, "IDAGetNumResEvals");
	retval = IDAGetNumJacEvals(mem, &nje);
	CheckRet(retval, "IDAGetNumJacEvals");
	retval = IDAGetNumNonlinSolvIters(mem, &nni);
	CheckRet(retval, "IDAGetNumNonlinSolvIters");
	retval = IDAGetNumErrTestFails(mem, &netf);
	CheckRet(retval, "IDAGetNumErrTestFails");
	retval = IDAGetNumNonlinSolvConvFails(mem, &ncfn);
	CheckRet(retval, "IDAGetNumNonlinSolvConvFails");
	retval = IDAGetNumLinResEvals(mem, &nreLS);
	CheckRet(retval, "IDAGetNumLinResEvals");
	retval = IDAGetNumGEvals(mem, &nge);
	CheckRet(retval, "IDAGetNumGEvals");

	printf("\nFinal Run Statistics: \n\n");
	printf("Number of steps                    = %ld\n", nst);
	printf("Number of residual evaluations     = %ld\n", nre+nreLS);
	printf("Number of Jacobian evaluations     = %ld\n", nje);
	printf("Number of nonlinear iterations     = %ld\n", nni);
	printf("Number of error test failures      = %ld\n", netf);
	printf("Number of nonlinear conv. failures = %ld\n", ncfn);
	printf("Number of root fn. evaluations     = %ld\n", nge);
}

static void PrintIteration(realtype t, realtype *y, int neq) {
	Cout() << "\n" << Format("%7.3f ", t);
	for (int i = 0; i < neq; ++i)
		Cout() << Format("%12.4e ", y[i]);
}

static void PrintRootInfo(int *roots, int nroots) {
	Cout() << "\nRoots found[] = ";
	for (int i = 0; i < nroots; ++i)
		Cout() << roots[i] << " ";
}

static String GetIdaErrorMsg(int ret) {
	switch (ret) {
	case IDA_TOO_MUCH_WORK:	return "IDASolve: IDA_TOO_MUCH_WORK";
	case IDA_TOO_MUCH_ACC:  return "IDASolve: IDA_TOO_MUCH_ACC";
	case IDA_ERR_FAIL:      return "IDASolve: IDA_ERR_FAIL";
	case IDA_MEM_NULL:		return "ida_mem is NULL";
	case IDA_NO_MALLOC:		return "ida_mem was not allocated";
	case IDA_ILL_INPUT:		return "bad value for icopt, tout1, or id";
	case IDA_LINIT_FAIL:	return "the linear solver linit routine failed";
 	case IDA_BAD_EWT:		return "zero value of some component of ewt";
 	case IDA_RES_FAIL:		return "res had a non-recoverable error";
 	case IDA_FIRST_RES_FAIL:return "res failed recoverably on the first call";
 	case IDA_LSETUP_FAIL:	return "lsetup had a non-recoverable error";
 	case IDA_LSOLVE_FAIL:	return "lsolve had a non-recoverable error";
 	case IDA_NO_RECOVERY:	return "res, lsetup, or lsolve had a recoverable error, but IDACalcIC could not recover";
 	case IDA_CONSTR_FAIL:	return "the inequality constraints could not be met";
 	case IDA_LINESEARCH_FAIL:return"the linesearch failed (either on steptol test or on the maxbacks test)";
 	case IDA_CONV_FAIL:		return "the Newton iterations failed to converge";
	default:				return Format("Unknown Sundials error %d", ret);
	}
}

void SolveDAE(const VectorXd &y, const VectorXd &dy, double dt, double maxt, Upp::Array<VectorXd> &res,
		Function <bool(double t, Eigen::Index iiter, const double y[], const double dy[], double residual[])>Residual, int numZero,
		Function <bool(double t, Eigen::Index iiter, const double y[], const double dy[], double residual[])>ResidualZero) {
	ASSERT(y.size() == dy.size());
	Eigen::Index numEq = y.size();
	res.SetCount(int(numEq));
	for (int i = 0; i < numEq; ++i) {
		res[i].resize(int(maxt/dt)+1);
		res[i](0) = y[i];
	}
	SolveDAE(y.data(), dy.data(), int(numEq), dt, maxt, Residual, numZero, ResidualZero, [&](double t, Eigen::Index iiter, const double y[], const double dy[], bool isZero, int *whichZero)->int {
		for (int i = 0; i < int(numEq); ++i)
			res[i](iiter) = y[i];
		return true;
	});
}

void SolveDAE(const VectorXd &y, const VectorXd &dy, double dt, double maxt,
		Upp::Array<VectorXd> &res, Upp::Array<VectorXd> &dres,
		Function <bool(double t, Eigen::Index iiter, const double y[], const double dy[], double residual[])>Residual, int numZero,
		Function <bool(double t, Eigen::Index iiter, const double y[], const double dy[], double residual[])>ResidualZero) {
	ASSERT(y.size() == dy.size());
	Eigen::Index numEq = y.size();
	res.SetCount(int(numEq));
	dres.SetCount(int(numEq));
	for (int i = 0; i < numEq; ++i) {
		res[i].resize(int(maxt/dt)+1);
		res[i](0) = y[i];
		dres[i].resize(int(maxt/dt)+1);
		dres[i](0) = dy[i];
	}
	SolveDAE(y.data(), dy.data(), int(numEq), dt, maxt, Residual, numZero, ResidualZero, [&](double t, Eigen::Index iiter, const double y[], const double dy[], bool isZero, int *whichZero)->int {
		for (int i = 0; i < int(numEq); ++i) {
			res[i](iiter) = y[i];
			dres[i](iiter) = dy[i];
		}
		return true;
	});
}

void SolveDAE(const double y[], const double dy[], int numEq, double dt, double maxt,
		Function <bool(double t, Eigen::Index iiter, const double y[], const double dy[], double residual[])> Residual, int numZero,
		Function <bool(double t, Eigen::Index iiter, const double y[], const double dy[], double residual[])> ResidualZero,
		Function <bool(double t, Eigen::Index iiter, const double y[], const double dy[], bool isZero, int *whichZero)>OnIteration) {
	int retval, retvalr;
	Buffer<int> rootsfound(numEq);
	SUNNonlinearSolver NLS = NULL;
	SUNLinearSolver LS = NULL;
	SUNMatrix A = NULL;
	void *mem = NULL;
	N_Vector avtol = NULL, yy = NULL, yp = NULL;

	Exc error;
	try {
		yy = N_VNew_Serial(numEq);
		CheckMem((void *)yy, "N_VNew_Serial");
		yp = N_VNew_Serial(numEq);
		CheckMem((void *)yp, "N_VNew_Serial");
		avtol = N_VNew_Serial(numEq);
		CheckMem((void *)avtol, "N_VNew_Serial");

	  	/* Create and initialize  y, y', and absolute tolerance vectors. */
		memcpy(N_VGetArrayPointer(yy), y,  numEq*sizeof(double));
		memcpy(N_VGetArrayPointer(yp), dy, numEq*sizeof(double));

		realtype rtol = 1.0e-5;

		realtype *atval = N_VGetArrayPointer(avtol);
		for (int i = 0; i < numEq; ++i)
			atval[i] = 1.0e-8;

	  	double t0 = 0;

		struct UserData {
			Function <bool(double t, Eigen::Index iiter, const double y[], const double dy[], double residual[])> Residual;
			Function <bool(double t, Eigen::Index iiter, const double y[], const double dy[], double residual[])> ResidualZero;
			int iiter = 0;
		} userData;

		userData.Residual = Residual;
		userData.ResidualZero = ResidualZero;

	    auto ResFun = [](realtype t, N_Vector _y, N_Vector _dy, N_Vector _res, void *user_data) {
			realtype *y = N_VGetArrayPointer(_y);
			realtype *dy = N_VGetArrayPointer(_dy);
			realtype *res = N_VGetArrayPointer(_res);

			UserData &userData = *(UserData *)user_data;

			return userData.Residual(t, userData.iiter, y, dy, res) ? 0 : -1;
		};
	    auto ResZeroFun = [](realtype t, N_Vector _y, N_Vector _dy, realtype *res, void *user_data) {
			realtype *y = N_VGetArrayPointer(_y);
			realtype *dy = N_VGetArrayPointer(_dy);

			UserData &userData = *(UserData *)user_data;

			return userData.ResidualZero(t, userData.iiter, y, dy, res) ? 0 : -1;
		};

		/* Call IDACreate and IDAInit to initialize IDA memory */
		mem = IDACreate();
		CheckMem((void *)mem, "IDACreate");

		retval = IDASetUserData(mem, (void *)&userData);
		CheckRet(retval, "IDASetUserData");

		retval = IDAInit(mem, ResFun, t0, yy, yp);
		CheckRet(retval, "IDAInit");

		IDASetErrHandlerFn(mem, [](int error_code, const char *module, const char *function,
                                char *msg, void *user_data) {
        	throw Exc(Format(t_("Sundials %s error (%s): %s"), module, function, msg));
                                }, nullptr);

		/* Call IDASVtolerances to set tolerances */
		retval = IDASVtolerances(mem, rtol, avtol);
		CheckRet(retval, "IDASVtolerances");

		/* Call IDARootInit to specify the root function grob with 2 components */
		if (ResidualZero && numZero > 0) {
			retval = IDARootInit(mem, numZero, ResZeroFun);
			CheckRet(retval, "IDARootInit");
		}

		/* Create dense SUNMatrix for use in linear solver */
		A = SUNDenseMatrix(numEq, numEq);
		CheckMem((void *)A, "SUNDenseMatrix");

		/* Create dense SUNLinearSolver object */
		LS = SUNLinSol_Dense(yy, A);
		CheckMem((void *)LS, "SUNLinSol_Dense");

		/* Attach the matrix and linear solver */
		retval = IDASetLinearSolver(mem, LS, A);
		CheckRet(retval, "IDASetLinearSolver");

		retval = IDASetMaxNumSteps(mem, 20000);
		CheckRet(retval, "IDASetLinearSolver");

		/* Create Newton SUNNonlinearSolver object. IDA uses a
		* Newton SUNNonlinearSolver by default, so it is unnecessary
		* to create it and attach it. It is done in this example code
		* solely for demonstration purposes. */
		NLS = SUNNonlinSol_Newton(yy);
		CheckMem((void *)NLS, "SUNNonlinSol_Newton");

		/* Attach the nonlinear solver */
		retval = IDASetNonlinearSolver(mem, NLS);
		CheckRet(retval, "IDASetNonlinearSolver");

		userData.iiter = 0;
		double tnext = 0.000001;
		double titer;
		while(tnext <= maxt && userData.iiter < int(maxt/dt)+1) {
			retval = IDASolve(mem, tnext, &titer, yy, yp, IDA_NORMAL);
			CheckRet(retval, "IDASolve");

			realtype *y  = N_VGetArrayPointer(yy);
			realtype *dy = N_VGetArrayPointer(yp);

			//PrintIteration(titer, y, numEq);

			if (retval == IDA_ROOT_RETURN) {
				retvalr = IDAGetRootInfo(mem, rootsfound);
				CheckRet(retvalr, "IDAGetRootInfo");
				//PrintRootInfo(rootsfound, numEq);
			} else if (retval == IDA_SUCCESS)
				;
			else
				throw Exc(Format(t_("Sundials IDA error: %s"), GetIdaErrorMsg(retval)));

			if(OnIteration && !OnIteration(titer, userData.iiter, y, dy, retval == IDA_ROOT_RETURN, rootsfound))
				break;

			if (retval == IDA_SUCCESS) {
				userData.iiter++;
				tnext = userData.iiter*dt;
			}
	  	}
	  	//PrintFinalStats(mem);
	} catch(Exc err) {
		error = err;
	}
	IDAFree(&mem);
	SUNNonlinSolFree(NLS);
	SUNLinSolFree(LS);
	SUNMatDestroy(A);
	N_VDestroy(avtol);
	N_VDestroy(yy);
	N_VDestroy(yp);

	if (!error.IsEmpty())
		throw Exc(error);
}

/*
0 	then no constraint is imposed on ui.
1 	then ui will be constrained to be ui 0:0.
-1 	then ui will be constrained to be ui 0:0.
2 	then ui will be constrained to be ui > 0:0.
-2 	then ui will be constrained to be ui < 0:0.
*/
void SolveNonLinearEquationsSun(double y[], int numEq,
			Function <bool(const double b[], double residuals[])> Residual, int constraints[]) {
	N_Vector s = NULL;
	void *kmem = NULL;

  	Exc error;
  	try {
  		N_Vector u = N_VNew_Serial(numEq);
		CheckMem((void *)u, "N_VNew_Serial");
  		realtype *udata = NV_DATA_S(u);
 		memcpy(udata, y, numEq*sizeof(double));

		s = N_VNew_Serial(numEq);
	  	CheckMem((void *)s, "N_VNew_Serial");

		N_VConst_Serial(1. ,s); /* no scaling */

		double fnormtol = 1.e-6;
		double scsteptol = 1.e-6;
		int numIterations = 500;

	  	kmem = KINCreate();
	  	CheckMem((void *)kmem, "KINCreate");

	  	auto ErrorFun = [](int error_code, const char *module, const char *function, char *msg, void *data) {
		  	String serror = Format("%d, module %s, function %s", error_code, module, function);
		  	if (error_code == KIN_WARNING)
		    	Cout() << Format(t_("Kinsol Warning (%s): %s"), serror, msg);
		  	else {
		  		char *str = KINGetReturnFlagName(error_code);
		  		String cerror(str);
		  		free(str);
		    	throw Exc(Format(t_("Kinsol Error %s (%s): %s"), cerror, serror, msg));
		  	}
		};

	  	int flag;

		flag = KINSetErrHandlerFn(kmem, ErrorFun, NULL);
		CheckRet(flag, "KINSetErrHandlerFn");

	  	struct UserData {
			Function <bool(const double y[], double residual[])> Residual;
		} userData;

		userData.Residual = Residual;

	  	flag = KINSetUserData(kmem, &userData);
	  	CheckRet(flag, "KINSetUserData");
		flag = KINSetFuncNormTol(kmem, fnormtol);
		CheckRet(flag, "KINSetFuncNormTol");
		flag = KINSetScaledStepTol(kmem, scsteptol);
		CheckRet(flag, "KINSetScaledStepTol");
		flag = KINSetNumMaxIters(kmem, numIterations);
	  	CheckRet(flag, "KINSetNumMaxIters");

	  	auto ResFun = [](N_Vector u, N_Vector f, void *user_data) {
			realtype *y = NV_DATA_S(u);
			realtype *res = NV_DATA_S(f);

			UserData &userData = *(UserData *)user_data;

			return userData.Residual(y, res) ? 0 : 1;
		};

		flag = KINInit(kmem, ResFun, u);
		CheckRet(flag, "KINInit");

		if (constraints != nullptr) {
			N_Vector constr = N_VNew_Serial(numEq);
			CheckMem((void *)constr, "N_VNew_Serial");
			realtype *constrdata = NV_DATA_S(constr);
			for (int i = 0; i < numEq; ++i)
				constrdata[i] = constraints[i];

		 	flag = KINSetConstraints(kmem, constr);
		  	CheckRet(flag, "KINSetConstraints");

			N_VDestroy_Serial(constr);
		}

		/* Create dense SUNMatrix */
		SUNMatrix J = SUNDenseMatrix(numEq, numEq);
		CheckMem((void *)J, "SUNDenseMatrix");

		/* Create dense SUNLinearSolver object */
		SUNLinearSolver LS = SUNLinSol_Dense(u, J);
		CheckMem((void *)LS, "SUNLinSol_Dense");

		/* Attach the matrix and linear solver to KINSOL */
		flag = KINSetLinearSolver(kmem, LS, J);
		CheckRet(flag, "KINSetLinearSolver");

		int glstr = KIN_NONE;	// KIN_NONE 		KIN_LINESEARCH	Using line search
		int mset = 1;			// 1 Exact Newton	0 Modified Newton

		flag = KINSetMaxSetupCalls(kmem, mset);
		CheckRet(flag, "KINSetMaxSetupCalls");

		flag = KINSol(kmem, u, glstr, s, s);
		CheckRet(flag, "KINSol");

		for (int i = 0; i < numEq; ++i)
			if (udata[i] != udata[i])
				throw Exc(Format(t_("Obtained NaN in value %d"), i));
		 memcpy(y, udata, numEq*sizeof(double));

  	} catch(Exc err) {
  		error = err;
  	}
 	N_VDestroy_Serial(s);
  	KINFree(&kmem);
  	if (!error.IsEmpty())
  		throw Exc(error);
}

}


#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <string.h>
#include <iostream>
#include <fstream>
#include <boost/lexical_cast.hpp>
#include "packet.h"

#define PORT 10038
#define PAKSIZE 128
#define ACK 0
#define NAK 1

using namespace std;

bool seqNum;

bool isvpack(unsigned char * p) {

  cout << endl << "=== IS VALID PACKET TESTING" << endl;

  char * sns = new char[2];
  memcpy(sns, &p[0], 1);
  sns[1] = '\0';

  char * css = new char[6];
  css[0] = 0;
  memcpy(css, &p[1], 6);

  // truncate after first 6 bytes
  css[6] = 0;
  char * db = new char[121 + 1];
  memcpy(db, &p[7], 121);
  db[121] = '\0';

  cout << "Seq. num: " << sns << endl;
  cout << "Checksum: " << css << endl;
  cout << "Message: " << db << endl;

  cout << "CSS:" << css << "$$" << endl;
  int sn = boost::lexical_cast<int>(sns);
  int cs = boost::lexical_cast<int>(css);

  Packet pk (0, db);
  pk.setSequenceNum(sn);

  // change to validate based on checksum and sequence number

  if(sn == seqNum) return false;
  cout << "PK: " << pk.getDataBuffer() << "$$" << endl;
  cout << "Help Me!" << cs << "$$" << endl;
  cout << "Generated: " << pk.generateCheckSum() << "$$" << endl;
  if(cs != pk.generateCheckSum()) return false;
  return true;
}

int main() {
  struct sockaddr_in a;
  struct sockaddr_in ca;
  socklen_t calen = sizeof(ca);
  int rlen;
  int s;
  bool ack;
  seqNum = true;

  /* Create our socket. */
  if ((s = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
    cout << "Socket creation failed. (socket s)" << endl;
    return 0;
  }

  /*
   * Bind our socket to an IP (whatever the computer decides)
   * and a specified port.
   *
   */

  memset((char *)&a, 0, sizeof(a));
  a.sin_family = AF_INET;
  a.sin_addr.s_addr = htonl(INADDR_ANY);
  a.sin_port = htons(PORT);

  if (bind(s, (struct sockaddr *)&a, sizeof(a)) < 0) {
    cout << "Socket binding failed. (socket s, address a)" << endl;
    return 0;
  }

  /* Loop forever, waiting for messages from a client. */

  cout << "Waiting on port " << PORT << "..." << endl;

  ofstream file("Dumpfile");

  bool isSeqNumSet = false;
  for (;;) {
    unsigned char packet[PAKSIZE + 1];
    unsigned char dataPull[PAKSIZE - 7 + 1];
    rlen = recvfrom(s, packet, PAKSIZE, 0, (struct sockaddr *)&ca, &calen);
    if(!isSeqNumSet) {
      isSeqNumSet = true;
      char * str = new char[1];
      memcpy(str, &packet[0], 1);
      seqNum = boost::lexical_cast<int>(str);
    }
    for(int x = 0; x < PAKSIZE - 7; x++) {
      dataPull[x] = packet[x + 7];
    }
    dataPull[PAKSIZE - 7] = '\0';
    packet[PAKSIZE] = '\0';
    if (rlen > 0) {
      char * css = new char[6];
      memcpy(css, &packet[1], 5);
      css[5] = '\0';
      cout << endl << endl << "=== RECEIPT" << endl;
      cout << "Seq. num: " << packet[0] << endl;
      cout << "Checksum: " << css << endl;
      cout << "Received message: " << dataPull << endl;
      cout << "Sent response: ";
      if(isvpack(packet)) {
        ack = ACK;
        seqNum = (seqNum) ? false : true;
        file << dataPull;
      } else {
        ack = NAK;
      }
      cout << ((ack == ACK) ? "ACK" : "NAK") << endl;
      Packet p ((seqNum) ? false : true, reinterpret_cast<const char *>(dataPull));
      cout << "CSS:" << css << "$$" << endl;
      p.setCheckSum(boost::lexical_cast<int>(css));
      p.setAckNack(ack);

      if(sendto(s, p.str(), PAKSIZE, 0, (struct sockaddr *)&ca, calen) < 0) {
        cout << "Acknowledgement failed. (socket s, acknowledgement message ack, client address ca, client address length calen)" << endl;
        return 0;
      }
      delete css;
    }
  }
  file.close();
}

COMP 4320 - Intro to Computer Networks/Networks Group Project Folder/Project1_Submission/NetworksGremlin/server.cpp


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN

#include <string>
#include <thread>

#include <limits>

#include <boost/make_shared.hpp>
#include <boost/test/unit_test.hpp>

#include "tudat/basics/testMacros.h"
#include "tudat/simulation/estimation.h"
#include "tudat/astro/orbit_determination/estimatable_parameters/directTidalTimeLag.h"

namespace tudat
{
namespace unit_tests
{
BOOST_AUTO_TEST_SUITE( test_tidal_propery_estimation )

//Using declarations.
using namespace tudat::observation_models;
using namespace tudat::orbit_determination;
using namespace tudat::estimatable_parameters;
using namespace tudat::interpolators;
using namespace tudat::numerical_integrators;
using namespace tudat::spice_interface;
using namespace tudat::simulation_setup;
using namespace tudat::orbital_element_conversions;
using namespace tudat::ephemerides;
using namespace tudat::propagators;
using namespace tudat::basic_astrodynamics;

//! Unit test to check if tidal time lag parameters are estimated correctly
BOOST_AUTO_TEST_CASE( test_DissipationParameterEstimation )
{
    //Load spice kernels
    spice_interface::loadStandardSpiceKernels( );

    //Define list of bodies
    std::vector< std::string > bodyNames;
    bodyNames.push_back( "Sun" );
    bodyNames.push_back( "Jupiter" );
    bodyNames.push_back( "Io" );
    bodyNames.push_back( "Europa" );

    std::vector< std::string > satelliteNames;
    satelliteNames.push_back( "Io" );
    satelliteNames.push_back( "Europa" );

    // Specify initial time
    double initialEphemerisTime = 0.0;
    double finalEphemerisTime = 0.25 * physical_constants::JULIAN_YEAR;
    double fixedStepSize = 450.0;
    double buffer = 10.0 * fixedStepSize;

    // Get default settings
    BodyListSettings bodySettings =
            getDefaultBodySettings( bodyNames, initialEphemerisTime - buffer, finalEphemerisTime + buffer );
    Eigen::MatrixXd cosineCoefficients = Eigen::MatrixXd::Zero( 3, 3 );

    // Update gravity field settings with sh field, required by tidal models (reference radius)
    cosineCoefficients( 0, 0 ) = 1.0;
    Eigen::MatrixXd sineCoefficients = Eigen::MatrixXd::Zero( 3, 3 );
    for( unsigned int i = 0; i < bodyNames.size( ); i++ )
    {
        if( bodyNames.at( i ) != "Sun" )
        {
            bodySettings.at( bodyNames.at( i ) )->gravityFieldSettings = std::make_shared< SphericalHarmonicsGravityFieldSettings >
                    ( getBodyGravitationalParameter( bodyNames.at( i ) ), getAverageRadius( bodyNames.at( i ) ),
                      cosineCoefficients, sineCoefficients, "IAU_" + bodyNames.at( i ) );
        }
    }

    SystemOfBodies bodies = createSystemOfBodies( bodySettings );

    // Test estimation of Jupiter dissipation without and with frequency dependence
    for( unsigned int test = 0; test < 2; test++ )
    {
        // Define accelerations acting on Io and Europa
        double satelliteLoveNumber = 1.0, satelliteTimeLag = 1000.0;
        double jupiterLoveNumber = 1.0, jupiterTimeLag = 100.0;
        SelectedAccelerationMap accelerationMap;
        for( unsigned int i = 0; i < satelliteNames.size( ); i++ )
        {
            std::map< std::string, std::vector< std::shared_ptr< AccelerationSettings > > > accelerationsOfSatellite;
            accelerationsOfSatellite[ "Jupiter" ].push_back( std::make_shared< AccelerationSettings >(
                                                                 point_mass_gravity ) );
            accelerationsOfSatellite[ "Sun" ].push_back( std::make_shared< AccelerationSettings >(
                                                             point_mass_gravity ) );
            accelerationsOfSatellite[ "Jupiter" ].push_back( std::make_shared< DirectTidalDissipationAccelerationSettings >(
                                                                 satelliteLoveNumber, satelliteTimeLag, false, false ) );
            accelerationsOfSatellite[ "Jupiter" ].push_back( std::make_shared< DirectTidalDissipationAccelerationSettings >(
                                                                 jupiterLoveNumber, jupiterTimeLag, false, true ) );
            for( unsigned int j = 0; j < satelliteNames.size( ); j++ )
            {
                if( i != j )
                {
                    accelerationsOfSatellite[ satelliteNames.at( j ) ].push_back(
                                std::make_shared< AccelerationSettings >( point_mass_gravity ) );
                }
            }
            accelerationMap[ satelliteNames.at( i ) ] = accelerationsOfSatellite;
        }

        // Set bodies for which initial state is to be estimated and integrated.
        std::vector< std::string > bodiesToEstimate;
        bodiesToEstimate.push_back( "Io" );
        bodiesToEstimate.push_back( "Europa" );
        std::vector< std::string > centralBodies;
        centralBodies.push_back( "Jupiter" );
        centralBodies.push_back( "Jupiter" );
        AccelerationMap accelerationModelMap = createAccelerationModelsMap(
                    bodies, accelerationMap, bodiesToEstimate, centralBodies );

        std::shared_ptr< PropagatorSettings< double > > propagatorSettings =
                std::make_shared< TranslationalStatePropagatorSettings< double > >
                ( centralBodies, accelerationModelMap, bodiesToEstimate,
                  getInitialStatesOfBodies( bodiesToEstimate, centralBodies, bodies, initialEphemerisTime ),
                  finalEphemerisTime );

        // Set parameters that are to be estimated.
        std::vector< std::shared_ptr< EstimatableParameterSettings > > parameterNames =
                getInitialStateParameterSettings< double >( propagatorSettings, bodies );
        parameterNames.push_back(
                    std::make_shared< tudat::estimatable_parameters::DirectTidalTimeLagEstimatableParameterSettings >(
                        "Io", "" ) );
        parameterNames.push_back(
                    std::make_shared< tudat::estimatable_parameters::DirectTidalTimeLagEstimatableParameterSettings >(
                        "Europa", "" ) );
        if( test == 0 )
        {
            parameterNames.push_back(
                        std::make_shared< tudat::estimatable_parameters::DirectTidalTimeLagEstimatableParameterSettings >(
                            "Jupiter", "" ) );
        }
        else
        {
            parameterNames.push_back(
                        std::make_shared< tudat::estimatable_parameters::DirectTidalTimeLagEstimatableParameterSettings >(
                            "Jupiter", "Io" ) );
            parameterNames.push_back(
                        std::make_shared< tudat::estimatable_parameters::DirectTidalTimeLagEstimatableParameterSettings >(
                            "Jupiter", "Europa" ) );
        }
        std::shared_ptr< tudat::estimatable_parameters::EstimatableParameterSet< double > > parametersToEstimate =
                createParametersToEstimate< double >( parameterNames, bodies, propagatorSettings );

        // Define links in simulation.
        std::vector< LinkEnds > linkEnds;
        linkEnds.resize( 2 );
        linkEnds[ 0 ][ observed_body ] = std::make_pair( "Io", "" );
        linkEnds[ 1 ][ observed_body ] = std::make_pair( "Europa", "" );
        std::vector< std::shared_ptr< ObservationModelSettings > > observationSettingsList;
        observationSettingsList.push_back( std::make_shared< ObservationModelSettings >(
                                           position_observable, linkEnds[ 0 ] ) );
        observationSettingsList.push_back( std::make_shared< ObservationModelSettings >(
                                           position_observable, linkEnds[ 1 ] ) );

        // Define integrator and propagator settings.
        std::shared_ptr< IntegratorSettings< > > integratorSettings =
                std::make_shared< RungeKuttaVariableStepSizeSettings< > >
                ( 0.0, fixedStepSize,
                  RungeKuttaCoefficients::rungeKuttaFehlberg78, fixedStepSize, fixedStepSize, 1.0, 1.0 );

        // Create orbit determination object.
        OrbitDeterminationManager< double, double > orbitDeterminationManager =
                OrbitDeterminationManager< double, double >(
                    bodies, parametersToEstimate,
                    observationSettingsList, integratorSettings, propagatorSettings );

        Eigen::VectorXd initialParameterEstimate =
                parametersToEstimate->template getFullParameterValues< double >( );

        // Test if tidal time lag parameters are correctly linked to acceleration models
        std::vector< std::shared_ptr< EstimatableParameter< double > > > dissipationEstimatedParameters =
                parametersToEstimate->getEstimatedDoubleParameters( );
        if( test == 0 )
        {
            BOOST_CHECK_EQUAL( dissipationEstimatedParameters.size( ), 3 );
        }
        else
        {
            BOOST_CHECK_EQUAL( dissipationEstimatedParameters.size( ), 4 );
        }
        for( unsigned int i = 0; i < dissipationEstimatedParameters.size( ); i++ )
        {
            std::shared_ptr< DirectTidalTimeLag > currentTidalTimeLagParameter =
                    std::dynamic_pointer_cast< DirectTidalTimeLag >( dissipationEstimatedParameters.at( i ) );
            BOOST_CHECK_EQUAL( ( currentTidalTimeLagParameter == nullptr ), false );
            int numberOfAccelerationModels = currentTidalTimeLagParameter->getTidalAccelerationModels( ).size( );

            if( ( i == 2 ) && ( test == 0 ) )
            {
                BOOST_CHECK_EQUAL( numberOfAccelerationModels, 2 );
            }
            else
            {
                BOOST_CHECK_EQUAL( numberOfAccelerationModels, 1 );
            }

        }

        // Define observatoion simulation times
        std::vector< double > observationTimes;
        double observationTime = initialEphemerisTime + 10.0 * fixedStepSize;
        while( observationTime < finalEphemerisTime - 10.0 * fixedStepSize  )
        {
            observationTimes.push_back( observationTime );
            observationTime += 6.0 * 3600.0;;
        }

        std::vector< std::shared_ptr< ObservationSimulationSettings< > > > measurementSimulationInput;
        measurementSimulationInput.push_back(
                    std::make_shared< TabulatedObservationSimulationSettings< > >(
                        position_observable, linkEnds[ 0 ], observationTimes, observed_body ) );
        measurementSimulationInput.push_back(
                    std::make_shared< TabulatedObservationSimulationSettings< > >(
                        position_observable, linkEnds[ 1 ], observationTimes, observed_body ) );

        // Simulate observations
        std::shared_ptr< ObservationCollection< > >  observationsAndTimes = simulateObservations< double, double >(
                    measurementSimulationInput, orbitDeterminationManager.getObservationSimulators( ), bodies );

        // Perturb parameter values
        Eigen::VectorXd truthParameters = initialParameterEstimate;
        for( unsigned int i = 0; i < bodiesToEstimate.size( ); i++ )
        {
            initialParameterEstimate[ 0 + 6 * i ] += 1.0E0;
            initialParameterEstimate[ 1 + 6 * i ] += 1.0E0;
            initialParameterEstimate[ 2 + 6 * i ] += 1.0E0;
            initialParameterEstimate[ 3 + 6 * i ] += 1.0E-5;
            initialParameterEstimate[ 4 + 6 * i ] += 1.0E-5;
            initialParameterEstimate[ 5 + 6 * i ] += 1.0E-5;
        }
        for( unsigned int i = 6 * bodiesToEstimate.size( );
             i < static_cast< unsigned int >( initialParameterEstimate.rows( ) ); i++ )
        {
            initialParameterEstimate[ i ] *= 10.0;
        }
        parametersToEstimate->resetParameterValues( initialParameterEstimate );

        // Estimate initial states and tidal parameters
        std::shared_ptr< PodInput< double, double > > podInput =
                std::make_shared< PodInput< double, double > >(
                    observationsAndTimes, ( initialParameterEstimate ).rows( ) );
        std::shared_ptr< PodOutput< double > > podOutput = orbitDeterminationManager.estimateParameters(
                    podInput, std::make_shared< EstimationConvergenceChecker >( 3 ) );

        // Check if parameters are correctly estimated
        Eigen::VectorXd estimatedParametervalues = podOutput->parameterEstimate_;
        for( unsigned int i = 0; i < 3; i++ )
        {
            BOOST_CHECK_SMALL( std::fabs( truthParameters( i ) - podOutput->parameterEstimate_( i ) ), 0.1 );
            BOOST_CHECK_SMALL( std::fabs( truthParameters( i + 6 ) - podOutput->parameterEstimate_( i + 6 ) ), 0.1 );
            BOOST_CHECK_SMALL( std::fabs( truthParameters( i + 3 ) - podOutput->parameterEstimate_( i + 3 ) ), 1.0E-6 );
            BOOST_CHECK_SMALL( std::fabs( truthParameters( i + 9 ) - podOutput->parameterEstimate_( i + 9 ) ), 1.0E-6 );
        }
        BOOST_CHECK_SMALL( std::fabs( truthParameters( 12 ) - podOutput->parameterEstimate_( 12 ) ), 0.1 );
        BOOST_CHECK_SMALL( std::fabs( truthParameters( 13 ) - podOutput->parameterEstimate_( 13 ) ), 10.0 );
        BOOST_CHECK_SMALL( std::fabs( truthParameters( 14 ) - podOutput->parameterEstimate_( 14 ) ), 1.0E-3 );
        if( test == 1 )
        {
            BOOST_CHECK_SMALL( std::fabs( truthParameters( 15 ) - podOutput->parameterEstimate_( 15 ) ), 1.0E-2 );
        }

        std::cout << "Parameter error: " << ( truthParameters -  podOutput->parameterEstimate_ ).transpose( ) << std::endl;
    }
}

//! Function to get tidal deformation model for Earth
std::vector< std::shared_ptr< GravityFieldVariationSettings > > getEarthGravityFieldVariationSettings( )
{
    std::vector< std::shared_ptr< GravityFieldVariationSettings > > gravityFieldVariations;

    std::vector< std::string > deformingBodies;
    deformingBodies.push_back( "Moon" );

    std::map< int, std::vector< std::complex< double > > > loveNumbers;

    std::vector< std::complex< double > > degreeTwoLoveNumbers_;
    degreeTwoLoveNumbers_.push_back( std::complex< double >( 0.29525, -0.00087 ) );
    degreeTwoLoveNumbers_.push_back( std::complex< double >( 0.29525, -0.00087 ) );
    degreeTwoLoveNumbers_.push_back( std::complex< double >( 0.29525, -0.00087 ) );
    std::vector< std::complex< double > > degreeThreeLoveNumbers_;
    degreeThreeLoveNumbers_.push_back( std::complex< double >( 0.093, 0.0 ) );
    degreeThreeLoveNumbers_.push_back( std::complex< double >( 0.093, 0.0 ) );
    degreeThreeLoveNumbers_.push_back( std::complex< double >( 0.093, 0.0 ) );
    degreeThreeLoveNumbers_.push_back( std::complex< double >( 0.093, 0.0 ) );
    loveNumbers[ 2 ] = degreeTwoLoveNumbers_;
    loveNumbers[ 3 ] = degreeThreeLoveNumbers_;

    std::shared_ptr< GravityFieldVariationSettings > moonGravityFieldVariation =
            std::make_shared< BasicSolidBodyGravityFieldVariationSettings >(
                deformingBodies, loveNumbers );
    gravityFieldVariations.push_back( moonGravityFieldVariation );

    deformingBodies[ 0 ] = "Sun";
    std::shared_ptr< GravityFieldVariationSettings > sunSingleGravityFieldVariation =
            std::make_shared< BasicSolidBodyGravityFieldVariationSettings >(
                deformingBodies, loveNumbers );
    gravityFieldVariations.push_back( sunSingleGravityFieldVariation );

    return gravityFieldVariations;
}

//! Unit test to check if real/complex Love numbers at different degrees/orders/forcing frequencies are being correctly estimated.
BOOST_AUTO_TEST_CASE( test_LoveNumberEstimationFromOrbiterData )
{

    //Load spice kernels.
    spice_interface::loadStandardSpiceKernels( );

    // Define bodies in simulation
    std::vector< std::string > bodyNames;
    bodyNames.push_back( "Earth" );
    bodyNames.push_back( "Sun" );
    bodyNames.push_back( "Moon" );
    bodyNames.push_back( "Mars" );

    // Specify initial time
    int numberOfDaysOfData = 5;
    double initialEphemerisTime = 1.0E7;
    double finalEphemerisTime = initialEphemerisTime + numberOfDaysOfData * 86400.0;

    // Create bodies needed in simulation
    BodyListSettings bodySettings =
            getDefaultBodySettings( bodyNames, "Earth", "ECLIPJ2000"  );
    bodySettings.at( "Earth" )->gravityFieldVariationSettings = getEarthGravityFieldVariationSettings( );
    SystemOfBodies bodies = createSystemOfBodies( bodySettings );
    bodies.createEmptyBody( "Vehicle" );

    // Set accelerations on Vehicle that are to be taken into account.
    SelectedAccelerationMap accelerationMap;
    std::map< std::string, std::vector< std::shared_ptr< AccelerationSettings > > > accelerationsOfVehicle;
    accelerationsOfVehicle[ "Earth" ].push_back( std::make_shared< SphericalHarmonicAccelerationSettings >( 3, 3 ) );
    accelerationsOfVehicle[ "Sun" ].push_back( std::make_shared< AccelerationSettings >(
                                                   basic_astrodynamics::point_mass_gravity ) );
    accelerationsOfVehicle[ "Moon" ].push_back( std::make_shared< AccelerationSettings >(
                                                    basic_astrodynamics::point_mass_gravity ) );
    accelerationMap[ "Vehicle" ] = accelerationsOfVehicle;

    // Set bodies for which initial state is to be estimated and integrated.
    std::vector< std::string > bodiesToIntegrate;
    std::vector< std::string > centralBodies;
    bodiesToIntegrate.push_back( "Vehicle" );
    centralBodies.push_back( "Earth" );

    // Create acceleration models
    AccelerationMap accelerationModelMap = createAccelerationModelsMap(
                bodies, accelerationMap, bodiesToIntegrate, centralBodies );

    // Set Keplerian and Cartesian elements for spacecraft.
    Eigen::Vector6d asterixInitialStateInKeplerianElements;
    asterixInitialStateInKeplerianElements( semiMajorAxisIndex ) = 6600.0E3;
    asterixInitialStateInKeplerianElements( eccentricityIndex ) = 0.05;
    asterixInitialStateInKeplerianElements( inclinationIndex ) = unit_conversions::convertDegreesToRadians( 85.3 );
    asterixInitialStateInKeplerianElements( argumentOfPeriapsisIndex )
            = unit_conversions::convertDegreesToRadians( 235.7 );
    asterixInitialStateInKeplerianElements( longitudeOfAscendingNodeIndex )
            = unit_conversions::convertDegreesToRadians( 23.4 );
    asterixInitialStateInKeplerianElements( trueAnomalyIndex ) = unit_conversions::convertDegreesToRadians( 139.87 );
    double earthGravitationalParameter = bodies.at( "Earth" )->getGravityFieldModel( )->getGravitationalParameter( );
    Eigen::Vector6d systemInitialState = convertKeplerianToCartesianElements(
                asterixInitialStateInKeplerianElements, earthGravitationalParameter );

    // Create propagator settings
    std::shared_ptr< TranslationalStatePropagatorSettings< double > > propagatorSettings =
            std::make_shared< TranslationalStatePropagatorSettings< double > >
            ( centralBodies, accelerationModelMap, bodiesToIntegrate, systemInitialState,
              double( finalEphemerisTime ), cowell );

    // Create integrator settings
    std::shared_ptr< IntegratorSettings< double > > integratorSettings =
            std::make_shared< RungeKuttaVariableStepSizeSettings< double > >
            ( initialEphemerisTime, 60.0,
              RungeKuttaCoefficients::CoefficientSets::rungeKuttaFehlberg78,
              60.0, 60.0, 1.0, 1.0 );

    // Define link ends to use
    LinkEnds linkEnds;
    linkEnds[ observed_body ] = std::make_pair( "Vehicle", "" );
    std::map< ObservableType, std::vector< LinkEnds > > linkEndsPerObservable;
    linkEndsPerObservable[ position_observable ].push_back( linkEnds );

    // Define parameters to estimate (vehicle initial state and various Love number combinations)
    std::vector< std::shared_ptr< EstimatableParameterSettings > > parameterNames =
            getInitialStateParameterSettings< double >( propagatorSettings, bodies );
    parameterNames.push_back( std::make_shared< SingleDegreeVariableTidalLoveNumberEstimatableParameterSettings >(
                                  "Earth", 2, std::vector< int >{ 2 }, "Moon", true ) );
    parameterNames.push_back( std::make_shared< SingleDegreeVariableTidalLoveNumberEstimatableParameterSettings >(
                                  "Earth", 2, std::vector< int >{ 2 }, "Sun", true ) );
    parameterNames.push_back( std::make_shared< SingleDegreeVariableTidalLoveNumberEstimatableParameterSettings >(
                                  "Earth", 2, std::vector< int >{ 0, 1 }, "Moon", false ) );
    parameterNames.push_back( std::make_shared< FullDegreeTidalLoveNumberEstimatableParameterSettings >(
                                  "Earth", 3, "Moon", true ) );

    // Create parameters
    std::shared_ptr< estimatable_parameters::EstimatableParameterSet< double > > parametersToEstimate =
            createParametersToEstimate( parameterNames, bodies );

    // Define observation settings
    std::vector< std::shared_ptr< ObservationModelSettings > > observationSettingsList;
    observationSettingsList.push_back( std::make_shared< ObservationModelSettings >(
                                       position_observable, linkEnds ) );
    // Create orbit determination object.
    OrbitDeterminationManager< double, double > orbitDeterminationManager =
            OrbitDeterminationManager< double, double >(
                bodies, parametersToEstimate, observationSettingsList,
                integratorSettings, propagatorSettings );

    // define observation simulation times
    std::vector< double > baseTimeList;
    double currentObservationTime = initialEphemerisTime + 1000.0;
    double observationInterval = 60.0;
    while( currentObservationTime < finalEphemerisTime - 1000.0 )
    {
        baseTimeList.push_back( currentObservationTime );
        currentObservationTime += observationInterval;
    }

    std::vector< std::shared_ptr< ObservationSimulationSettings< > > > measurementSimulationInput;
    measurementSimulationInput.push_back(
                std::make_shared< TabulatedObservationSimulationSettings< > >(
                    position_observable, linkEnds, baseTimeList, observed_body ) );

    // Simulate observations
    std::shared_ptr< ObservationCollection< > >  observationsAndTimes = simulateObservations< double, double >(
                measurementSimulationInput, orbitDeterminationManager.getObservationSimulators( ), bodies );

    // Perturb parameter estimate
    Eigen::VectorXd initialParameterEstimate =
            parametersToEstimate->template getFullParameterValues< double >( );
    Eigen::VectorXd truthParameters = initialParameterEstimate;
    Eigen::VectorXd parameterPerturbation =
            Eigen::VectorXd::Zero( truthParameters.rows( ) );
    parameterPerturbation.segment( 0, 3 ) = Eigen::Vector3d::Constant( 1.0 );
    parameterPerturbation.segment( 3, 3 ) = Eigen::Vector3d::Constant( 1.E-3 );

    for( int i = 6; i < parameterPerturbation.rows( ); i++ )
    {
        parameterPerturbation( i ) += 0.001;
    }
    initialParameterEstimate += parameterPerturbation;

    // Define estimation input
    std::shared_ptr< PodInput< double, double  > > podInput =
            std::make_shared< PodInput< double, double > >(
                observationsAndTimes, initialParameterEstimate.rows( ),
                Eigen::MatrixXd::Zero( truthParameters.rows( ), truthParameters.rows( ) ),
                initialParameterEstimate - truthParameters );

    // Perform estimation
    std::shared_ptr< PodOutput< double > > podOutput = orbitDeterminationManager.estimateParameters(
                podInput, std::make_shared< EstimationConvergenceChecker >( 4 ) );

    // Check estimation results
    Eigen::VectorXd estimationError = podOutput->parameterEstimate_ - truthParameters;
    for( int i = 0; i < 3; i++ )
    {
        BOOST_CHECK_SMALL( std::fabs( estimationError( i ) ), 1.0E-4 );
        BOOST_CHECK_SMALL( std::fabs( estimationError( i + 3 ) ), 1.0E-8 );
    }

    for( int i = 0; i < estimationError.rows( ) - 6; i++ )
    {
        BOOST_CHECK_SMALL( std::fabs( estimationError( i + 6 ) ), 5.0E-6 );
    }

    std::cout << "Parameter error"<< ( estimationError ).transpose( ) << std::endl;

}

BOOST_AUTO_TEST_SUITE_END( )

}

}


#include <cstdlib>
#include <iostream>
#include <vector>
#include <fstream>
#include <cmath>
#include <chrono>
#include <ctime>
#include <unordered_set>
#include <unordered_map>

#include <execinfo.h>
#include <unistd.h>
#include <signal.h>

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

#include "cnn/training.h"
#include "cnn/cnn.h"
#include "cnn/expr.h"
#include "cnn/nodes.h"
#include "cnn/lstm.h"
#include "cnn/rnn.h"
#include "cnn/dict.h"
#include "cnn/cfsm-builder.h"

#include "nt-parser/oracle.h"
#include "nt-parser/pretrained.h"
#include "nt-parser/compressed-fstream.h"
#include "nt-parser/eval.h"

// dictionaries
cnn::Dict termdict, ntermdict, adict, posdict;

volatile bool requested_stop = false;
unsigned kSOS = 0;
unsigned IMPLICIT_REDUCE_AFTER_SHIFT = 0;
unsigned LAYERS = 2;
unsigned INPUT_DIM = 40;
unsigned HIDDEN_DIM = 60;
unsigned ACTION_DIM = 36;
unsigned PRETRAINED_DIM = 50;
unsigned LSTM_INPUT_DIM = 60;

unsigned ACTION_SIZE = 0;
unsigned VOCAB_SIZE = 0;
unsigned NT_SIZE = 0;
float DROPOUT = 0.0f;
std::map<int,int> action2NTindex;  // pass in index of action NT(X), return index of X

using namespace cnn::expr;
using namespace cnn;
using namespace std;
namespace po = boost::program_options;

vector<unsigned> possible_actions;
unordered_map<unsigned, vector<float>> pretrained;

ClassFactoredSoftmaxBuilder *cfsm = nullptr;

void InitCommandLine(int argc, char** argv, po::variables_map* conf) {
  po::options_description opts("Configuration options");
  opts.add_options()
        ("training_data,T", po::value<string>(), "List of Transitions - Training corpus")
        ("explicit_terminal_reduce,x", "[not recommended] If set, the parser must explicitly process a REDUCE operation to complete a preterminal constituent")
        ("dropout,D", po::value<float>(), "Use dropout")
        ("clusters,c", po::value<string>(), "Clusters word clusters file")
        ("dev_data,d", po::value<string>(), "Development corpus")
        ("test_data,p", po::value<string>(), "Test corpus")
        ("eta_decay,e", po::value<float>(), "Start decaying eta after this many epochs")
        ("model,m", po::value<string>(), "Load saved model from this file")
        ("layers", po::value<unsigned>()->default_value(2), "number of LSTM layers")
        ("action_dim", po::value<unsigned>()->default_value(16), "action embedding size")
        ("input_dim", po::value<unsigned>()->default_value(32), "input embedding size")
        ("hidden_dim", po::value<unsigned>()->default_value(64), "hidden dimension")
        ("pretrained_dim", po::value<unsigned>()->default_value(50), "pretrained input dimension")
        ("lstm_input_dim", po::value<unsigned>()->default_value(60), "LSTM input dimension")
        ("train,t", "Should training be run?")
        ("words,w", po::value<string>(), "Pretrained word embeddings")
        ("help,h", "Help");
  po::options_description dcmdline_options;
  dcmdline_options.add(opts);
  po::store(parse_command_line(argc, argv, dcmdline_options), *conf);
  if (conf->count("help")) {
    cerr << dcmdline_options << endl;
    exit(1);
  }
  if (conf->count("training_data") == 0) {
    cerr << "Please specify --traing_data (-T): this is required to determine the vocabulary mapping, even if the parser is used in prediction mode.\n";
    exit(1);
  }
}

struct ParserBuilder {
  LSTMBuilder stack_lstm; // (layers, input, hidden, trainer)
  LSTMBuilder term_lstm; // (layers, input, hidden, trainer)
  LSTMBuilder action_lstm;
  LSTMBuilder const_lstm_fwd;
  LSTMBuilder const_lstm_rev;
  LookupParameters* p_w; // word embeddings
  LookupParameters* p_t; // pretrained word embeddings (not updated)
  LookupParameters* p_nt; // nonterminal embeddings
  LookupParameters* p_ntup; // nonterminal embeddings when used in a composed representation
  LookupParameters* p_a; // input action embeddings
  Parameters* p_pbias; // parser state bias
  Parameters* p_A; // action lstm to parser state
  Parameters* p_S; // stack lstm to parser state
  Parameters* p_T; // term lstm to parser state
  //Parameters* p_pbias2; // parser state bias
  //Parameters* p_A2; // action lstm to parser state
  //Parameters* p_S2; // stack lstm to parser state
  //Parameters* p_w2l; // word to LSTM input
  //Parameters* p_t2l; // pretrained word embeddings to LSTM input
  //Parameters* p_ib; // LSTM input bias
  Parameters* p_cbias; // composition function bias
  Parameters* p_p2a;   // parser state to action
  Parameters* p_action_start;  // action bias
  Parameters* p_abias;  // action bias
  Parameters* p_stack_guard;  // end of stack

  Parameters* p_cW;

  explicit ParserBuilder(Model* model, const unordered_map<unsigned, vector<float>>& pretrained) :
      stack_lstm(LAYERS, LSTM_INPUT_DIM, HIDDEN_DIM, model),
      term_lstm(LAYERS, INPUT_DIM, HIDDEN_DIM, model),  // sequence of generated terminals
      action_lstm(LAYERS, ACTION_DIM, HIDDEN_DIM, model),
      const_lstm_fwd(1, LSTM_INPUT_DIM, LSTM_INPUT_DIM, model), // used to compose children of a node into a representation of the node
      const_lstm_rev(1, LSTM_INPUT_DIM, LSTM_INPUT_DIM, model), // used to compose children of a node into a representation of the node
      p_w(model->add_lookup_parameters(VOCAB_SIZE, {INPUT_DIM})),
      p_t(model->add_lookup_parameters(VOCAB_SIZE, {INPUT_DIM})),
      p_nt(model->add_lookup_parameters(NT_SIZE, {LSTM_INPUT_DIM})),
      p_ntup(model->add_lookup_parameters(NT_SIZE, {LSTM_INPUT_DIM})),
      p_a(model->add_lookup_parameters(ACTION_SIZE, {ACTION_DIM})),
      p_pbias(model->add_parameters({HIDDEN_DIM})),
      p_A(model->add_parameters({HIDDEN_DIM, HIDDEN_DIM})),
      p_S(model->add_parameters({HIDDEN_DIM, HIDDEN_DIM})),
      p_T(model->add_parameters({HIDDEN_DIM, HIDDEN_DIM})),
      //p_pbias2(model->add_parameters({HIDDEN_DIM})),
      //p_A2(model->add_parameters({HIDDEN_DIM, HIDDEN_DIM})),
      //p_S2(model->add_parameters({HIDDEN_DIM, HIDDEN_DIM})),
      //p_w2l(model->add_parameters({LSTM_INPUT_DIM, INPUT_DIM})),
      //p_ib(model->add_parameters({LSTM_INPUT_DIM})),
      p_cbias(model->add_parameters({LSTM_INPUT_DIM})),
      p_p2a(model->add_parameters({ACTION_SIZE, HIDDEN_DIM})),
      p_action_start(model->add_parameters({ACTION_DIM})),
      p_abias(model->add_parameters({ACTION_SIZE})),

      p_stack_guard(model->add_parameters({LSTM_INPUT_DIM})),

      p_cW(model->add_parameters({LSTM_INPUT_DIM, LSTM_INPUT_DIM * 2})) {
    if (pretrained.size() > 0) {
      cerr << "Pretrained embeddings not implemented\n";
      abort();
    }
  }

// checks to see if a proposed action is valid in discriminative models
static bool IsActionForbidden_Generative(const string& a, char prev_a, unsigned tsize, unsigned ssize, unsigned nopen_parens) {
  bool is_shift = (a[0] == 'S' && a[1]=='H');
  bool is_reduce = (a[0] == 'R' && a[1]=='E');
  bool is_nt = (a[0] == 'N');
  assert(is_shift || is_reduce || is_nt);
  static const unsigned MAX_OPEN_NTS = 100;
  if (is_nt && nopen_parens > MAX_OPEN_NTS) return true;
  if (ssize == 1) {
    if (!is_nt) return true;
    return false;
  }
  // you can't reduce after an NT action
  if (is_reduce && prev_a == 'N') return true;
  return false;
}

// returns parse actions for input sentence (in training just returns the reference)
// this lets us use pretrained embeddings, when available, for words that were OOV in the
// parser training data
// if sent is empty, generate a sentence
vector<unsigned> log_prob_parser(ComputationGraph* hg,
                     const parser::Sentence& sent,
                     const vector<int>& correct_actions,
                     double *right,
                     bool is_evaluation) {
    vector<unsigned> results;
    vector<string> stack_content;
    stack_content.push_back("ROOT_GUARD");
    const bool sample = sent.size() == 0;
    const bool build_training_graph = correct_actions.size() > 0;
    assert(sample || build_training_graph);
    bool apply_dropout = (DROPOUT && !is_evaluation);
    if (sample) apply_dropout = false;

    if (apply_dropout) {
      stack_lstm.set_dropout(DROPOUT);
      term_lstm.set_dropout(DROPOUT);
      action_lstm.set_dropout(DROPOUT);
      const_lstm_fwd.set_dropout(DROPOUT);
      const_lstm_rev.set_dropout(DROPOUT);
    } else {
      stack_lstm.disable_dropout();
      term_lstm.disable_dropout();
      action_lstm.disable_dropout();
      const_lstm_fwd.disable_dropout();
      const_lstm_rev.disable_dropout();
    }
    term_lstm.new_graph(*hg);
    stack_lstm.new_graph(*hg);
    action_lstm.new_graph(*hg);
    const_lstm_fwd.new_graph(*hg);
    const_lstm_rev.new_graph(*hg);
    cfsm->new_graph(*hg);
    term_lstm.start_new_sequence();
    stack_lstm.start_new_sequence();
    action_lstm.start_new_sequence();
    // variables in the computation graph representing the parameters
    Expression pbias = parameter(*hg, p_pbias);
    Expression S = parameter(*hg, p_S);
    Expression A = parameter(*hg, p_A);
    Expression T = parameter(*hg, p_T);
    //Expression pbias2 = parameter(*hg, p_pbias2);
    //Expression S2 = parameter(*hg, p_S2);
    //Expression A2 = parameter(*hg, p_A2);

    //Expression ib = parameter(*hg, p_ib);
    Expression cbias = parameter(*hg, p_cbias);
    //Expression w2l = parameter(*hg, p_w2l);
    Expression p2a = parameter(*hg, p_p2a);
    Expression abias = parameter(*hg, p_abias);
    Expression action_start = parameter(*hg, p_action_start);
    Expression cW = parameter(*hg, p_cW);

    action_lstm.add_input(action_start);

    vector<Expression> terms(1, lookup(*hg, p_w, kSOS));
    term_lstm.add_input(terms.back());

    vector<Expression> stack;  // variables representing subtree embeddings
    stack.push_back(parameter(*hg, p_stack_guard));
    // drive dummy symbol on stack through LSTM
    stack_lstm.add_input(stack.back());
    vector<int> is_open_paren; // -1 if no nonterminal has a parenthesis open, otherwise index of NT
    is_open_paren.push_back(-1); // corresponds to dummy symbol
    vector<Expression> log_probs;
    string rootword;
    unsigned action_count = 0;  // incremented at each prediction
    unsigned nt_count = 0; // number of times an NT has been introduced
    vector<unsigned> current_valid_actions;
    int nopen_parens = 0;
    char prev_a = '0';
    unsigned termc = 0;
    while(stack.size() > 2 || termc == 0) {
      assert (stack.size() == stack_content.size());
      // get list of possible actions for the current parser state
      current_valid_actions.clear();
      for (auto a: possible_actions) {
        if (IsActionForbidden_Generative(adict.Convert(a), prev_a, terms.size(), stack.size(), nopen_parens))
          continue;
        current_valid_actions.push_back(a);
      }
      //cerr << "valid actions = " << current_valid_actions.size() << endl;

      //onerep
      Expression stack_summary = stack_lstm.back();
      Expression action_summary = action_lstm.back();
      Expression term_summary = term_lstm.back();
      if (apply_dropout) {
        stack_summary = dropout(stack_summary, DROPOUT);
        action_summary = dropout(action_summary, DROPOUT);
        term_summary = dropout(term_summary, DROPOUT);
      }
      Expression p_t = affine_transform({pbias, S, stack_summary, A, action_summary, T, term_summary});
      Expression nlp_t = rectify(p_t);
      //tworep*
      //Expression p_t = affine_transform({pbias, S, stack_lstm.back(), A, action_lstm.back()});
      //Expression nlp_t = rectify(p_t);
      //if (build_training_graph) nlp_t = dropout(nlp_t, 0.4);
      // r_t = abias + p2a * nlp
      Expression r_t = affine_transform({abias, p2a, nlp_t});

      // adist = log_softmax(r_t, current_valid_actions)
      Expression adiste = log_softmax(r_t, current_valid_actions);
      unsigned action = 0;
      if (sample) {
        auto dist = as_vector(hg->incremental_forward());
        double p = rand01();
        assert(current_valid_actions.size() > 0);
        unsigned w = 0;
        for (; w < current_valid_actions.size(); ++w) {
          p -= exp(dist[current_valid_actions[w]]);
          if (p < 0.0) { break; }
        }
        if (w == current_valid_actions.size()) w--;
        action = current_valid_actions[w];
        const string& a = adict.Convert(action);
        if (a[0] == 'R') cerr << ")";
        if (a[0] == 'N') {
          int nt = action2NTindex[action];
          cerr << " (" << ntermdict.Convert(nt);
        }
      } else {
        if (action_count >= correct_actions.size()) {
          cerr << "Correct action list exhausted, but not in final parser state.\n";
          abort();
        }
        action = correct_actions[action_count];
        //cerr << "prob ="; for (unsigned i = 0; i < adist.size(); ++i) { cerr << ' ' << adict.Convert(i) << ':' << adist[i]; }
        //cerr << endl;
        ++action_count;
        log_probs.push_back(pick(adiste, action));
      }
      results.push_back(action);

      // add current action to action LSTM
      Expression actione = lookup(*hg, p_a, action);
      action_lstm.add_input(actione);

      // do action
      const string& actionString=adict.Convert(action);
      //cerr << "ACT: " << actionString << endl;
      const char ac = actionString[0];
      const char ac2 = actionString[1];
      prev_a = ac;

      if (ac =='S' && ac2=='H') {  // SHIFT
        unsigned wordid = 0;
        //tworep
        //Expression p_t = affine_transform({pbias2, S2, stack_lstm.back(), A2, action_lstm.back(), T, term_lstm.back()});
        //Expression nlp_t = rectify(p_t);
        //tworep-oneact:
        //Expression p_t = affine_transform({pbias2, S2, stack_lstm.back(), T, term_lstm.back()});
        //Expression nlp_t = rectify(p_t);
        if (sample) {
          wordid = cfsm->sample(nlp_t);
          cerr << " " << termdict.Convert(wordid);
        } else {
          assert(termc < sent.size());
          wordid = sent.raw[termc];
          log_probs.push_back(-cfsm->neg_log_softmax(nlp_t, wordid));
        }
        assert (wordid != 0);
        stack_content.push_back(termdict.Convert(wordid)); //add the string of the word to the stack
        ++termc;
        Expression word = lookup(*hg, p_w, wordid);
        terms.push_back(word);
        term_lstm.add_input(word);
        stack.push_back(word);
        stack_lstm.add_input(word);
        is_open_paren.push_back(-1);
      } else if (ac == 'N') { // NT
        ++nopen_parens;
        auto it = action2NTindex.find(action);
        assert(it != action2NTindex.end());
        int nt_index = it->second;
        nt_count++;
        stack_content.push_back(ntermdict.Convert(nt_index));
        Expression nt_embedding = lookup(*hg, p_nt, nt_index);
        stack.push_back(nt_embedding);
        stack_lstm.add_input(nt_embedding);
        is_open_paren.push_back(nt_index);
      } else { // REDUCE
        --nopen_parens;
        assert(stack.size() > 2); // dummy symbol means > 2 (not >= 2)
        assert(stack_content.size() > 2 && stack.size() == stack_content.size());
        // find what paren we are closing
        int i = is_open_paren.size() - 1; //get the last thing on the stack
        while(is_open_paren[i] < 0) { --i; assert(i >= 0); } //iteratively decide whether or not it's a non-terminal
        Expression nonterminal = lookup(*hg, p_ntup, is_open_paren[i]);
        int nchildren = is_open_paren.size() - i - 1;
        assert(nchildren > 0);
        //cerr << "  number of children to reduce: " << nchildren << endl;
        vector<Expression> children(nchildren);
        const_lstm_fwd.start_new_sequence();
        const_lstm_rev.start_new_sequence();

        // REMOVE EVERYTHING FROM THE STACK THAT IS GOING
        // TO BE COMPOSED INTO A TREE EMBEDDING
        string curr_word;
        //cerr << "--------------------------------" << endl;
        //cerr << "Now printing the children" << endl;
        //cerr << "--------------------------------" << endl;
        for (i = 0; i < nchildren; ++i) {
          assert (stack_content.size() == stack.size());
          children[i] = stack.back();
          stack.pop_back();
          stack_lstm.rewind_one_step();
          is_open_paren.pop_back();
          curr_word = stack_content.back();
          //cerr << "At the back of the stack (supposed to be one of the children): " << curr_word << endl;
          stack_content.pop_back();
        }
        assert (stack_content.size() == stack.size());
        //cerr << "Doing REDUCE operation" << endl;
        is_open_paren.pop_back(); // nt symbol
        stack.pop_back(); // nonterminal dummy
        stack_lstm.rewind_one_step(); // nt symbol
        curr_word = stack_content.back();
        //cerr << "--------------------------------" << endl;
        //cerr << "At the back of the stack (supposed to be the non-terminal symbol) : " << curr_word << endl;
        stack_content.pop_back();
        assert (stack.size() == stack_content.size());
        //cerr << "Done reducing" << endl;

        // BUILD TREE EMBEDDING USING BIDIR LSTM
        const_lstm_fwd.add_input(nonterminal);
        const_lstm_rev.add_input(nonterminal);
        for (i = 0; i < nchildren; ++i) {
          const_lstm_fwd.add_input(children[i]);
          const_lstm_rev.add_input(children[nchildren - i - 1]);
        }
        Expression cfwd = const_lstm_fwd.back();
        Expression crev = const_lstm_rev.back();
        if (apply_dropout) {
          cfwd = dropout(cfwd, DROPOUT);
          crev = dropout(crev, DROPOUT);
        }
        Expression c = concatenate({cfwd, crev});
        Expression composed = rectify(affine_transform({cbias, cW, c}));
        stack_lstm.add_input(composed);
        stack.push_back(composed);
        stack_content.push_back(curr_word);
        //cerr << curr_word << endl;
        is_open_paren.push_back(-1); // we just closed a paren at this position
      }
    }
    if (action_count != correct_actions.size()) {
      cerr << "Unexecuted actions remain but final state reached!\n";
      abort();
    }
    assert(stack.size() == 2); // guard symbol, root
    if (!sample) {
      Expression tot_neglogprob = -sum(log_probs);
      assert(tot_neglogprob.pg != nullptr);
    }
    if (sample) cerr << "\n";
    return results;
  }
};

void signal_callback_handler(int /* signum */) {
  if (requested_stop) {
    cerr << "\nReceived SIGINT again, quitting.\n";
    _exit(1);
  }
  cerr << "\nReceived SIGINT terminating optimization early...\n";
  requested_stop = true;
}

int main(int argc, char** argv) {
  cnn::Initialize(argc, argv);

  cerr << "COMMAND LINE:";
  for (unsigned i = 0; i < static_cast<unsigned>(argc); ++i) cerr << ' ' << argv[i];
  cerr << endl;
  unsigned status_every_i_iterations = 100;

  po::variables_map conf;
  InitCommandLine(argc, argv, &conf);
  if (conf.count("clusters") == 0) {
    cerr << "Please specify vocabulary clustering with --clusters FILE when training generative model\n";
    return 1;
  }
  if (conf.count("dropout"))
    DROPOUT = conf["dropout"].as<float>();
  LAYERS = conf["layers"].as<unsigned>();
  INPUT_DIM = conf["input_dim"].as<unsigned>();
  PRETRAINED_DIM = conf["pretrained_dim"].as<unsigned>();
  HIDDEN_DIM = conf["hidden_dim"].as<unsigned>();
  ACTION_DIM = conf["action_dim"].as<unsigned>();
  LSTM_INPUT_DIM = conf["lstm_input_dim"].as<unsigned>();
  if (conf.count("train") && conf.count("dev_data") == 0) {
    cerr << "You specified --train but did not specify --dev_data FILE\n";
    return 1;
  }
  ostringstream os;
  os << "ntparse_gen"
     << "_D" << DROPOUT
     << '_' << LAYERS
     << '_' << INPUT_DIM
     << '_' << HIDDEN_DIM
     << '_' << ACTION_DIM
     << '_' << LSTM_INPUT_DIM
     << "-pid" << getpid() << ".params";
  const string fname = os.str();
  cerr << "PARAMETER FILE: " << fname << endl;
  bool softlinkCreated = false;

  kSOS = termdict.Convert("<s>");
  Model model;
  cfsm = new ClassFactoredSoftmaxBuilder(HIDDEN_DIM, conf["clusters"].as<string>(), &termdict, &model);

  parser::TopDownOracleGen corpus(&termdict, &adict, &posdict, &ntermdict);
  parser::TopDownOracleGen dev_corpus(&termdict, &adict, &posdict, &ntermdict);
  parser::TopDownOracleGen2 test_corpus(&termdict, &adict, &posdict, &ntermdict);
  corpus.load_oracle(conf["training_data"].as<string>());

  if (conf.count("words"))
    parser::ReadEmbeddings_word2vec(conf["words"].as<string>(), &termdict, &pretrained);

  // freeze dictionaries so we don't accidentaly load OOVs
  termdict.Freeze();
  adict.Freeze();
  ntermdict.Freeze();
  posdict.Freeze();

  if (conf.count("dev_data")) {
    cerr << "Loading validation set\n";
    dev_corpus.load_oracle(conf["dev_data"].as<string>());
  }

  if (conf.count("test_data")) {
    cerr << "Loading test set\n";
    test_corpus.load_oracle(conf["test_data"].as<string>());
  }

  for (unsigned i = 0; i < adict.size(); ++i) {
    const string& a = adict.Convert(i);
    if (a[0] != 'N') continue;
    size_t start = a.find('(') + 1;
    size_t end = a.rfind(')');
    int nt = ntermdict.Convert(a.substr(start, end - start));
    action2NTindex[i] = nt;
  }

  NT_SIZE = ntermdict.size();
  VOCAB_SIZE = termdict.size();
  ACTION_SIZE = adict.size();
  possible_actions.resize(adict.size());
  for (unsigned i = 0; i < adict.size(); ++i)
    possible_actions[i] = i;

  ParserBuilder parser(&model, pretrained);
  if (conf.count("model")) {
    ifstream in(conf["model"].as<string>().c_str());
    boost::archive::text_iarchive ia(in);
    ia >> model;
  }

  //TRAINING
  if (conf.count("train")) {
    signal(SIGINT, signal_callback_handler);
    //AdamTrainer sgd(&model);
    SimpleSGDTrainer sgd(&model);
    //sgd.eta = 0.01;
    //sgd.eta0 = 0.01;
    //MomentumSGDTrainer sgd(&model);
    sgd.eta_decay = 0.08;
    //sgd.eta_decay = 0.05;
    vector<unsigned> order(corpus.sents.size());
    for (unsigned i = 0; i < corpus.sents.size(); ++i)
      order[i] = i;
    double tot_seen = 0;
    status_every_i_iterations = min((int)status_every_i_iterations, (int)corpus.sents.size());
    unsigned si = corpus.sents.size();
    cerr << "NUMBER OF TRAINING SENTENCES: " << corpus.sents.size() << endl;
    unsigned trs = 0;
    unsigned words = 0;
    double right = 0;
    double llh = 0;
    bool first = true;
    int iter = -1;
    double best_dev_llh = 9e99;
    //cerr << "TRAINING STARTED AT: " << put_time(localtime(&time_start), "%c %Z") << endl;
    while(!requested_stop) {
      ++iter;
      auto time_start = chrono::system_clock::now();
      for (unsigned sii = 0; sii < status_every_i_iterations; ++sii) {
           if (si == corpus.sents.size()) {
             si = 0;
             if (first) { first = false; } else {
                  sgd.update_epoch();
                  //sgd.eta /= 2;
              }
             //cerr << "NO SHUFFLE" << endl;
             cerr << "**SHUFFLE\n";
             random_shuffle(order.begin(), order.end());
             //sgd.eta /= 2;
           }
           tot_seen += 1;
           auto& sentence = corpus.sents[order[si]];
	   const vector<int>& actions=corpus.actions[order[si]];
           ComputationGraph hg;
           parser.log_prob_parser(&hg,sentence,actions,&right,false);
           double lp = as_scalar(hg.incremental_forward());
           if (lp < 0) {
             cerr << "Log prob < 0 on sentence " << order[si] << ": lp=" << lp << endl;
             assert(lp >= 0.0);
           }
           hg.backward();
           sgd.update(1.0);
           llh += lp;
           ++si;
           trs += actions.size();
           words += sentence.size();
      }
      sgd.status();
      auto time_now = chrono::system_clock::now();
      auto dur = chrono::duration_cast<chrono::milliseconds>(time_now - time_start);
      cerr << "update #" << iter << " (epoch " << (tot_seen / corpus.sents.size()) <<
         /*" |time=" << put_time(localtime(&time_now), "%c %Z") << ")\tllh: "<< */
        ") per-action-ppl: " << exp(llh / trs) << " per-input-ppl: " << exp(llh / words) << " per-sent-ppl: " << exp(llh / status_every_i_iterations) << " err: " << (trs - right) / trs << " [" << dur.count() / (double)status_every_i_iterations << "ms per instance]" << endl;
      llh = trs = right = words = 0;
      static int logc = 0;
      ++logc;
      if (logc > 50) {
        // generate random sample
        ComputationGraph cg;
        double x;
        parser.log_prob_parser(&cg, parser::Sentence(), vector<int>(),&x,true);
      }
      if (logc % 100 == 0) { // report on dev set
        unsigned dev_size = dev_corpus.size();
        double llh = 0;
        double trs = 0;
        double right = 0;
        double dwords = 0;
        auto t_start = chrono::high_resolution_clock::now();
        for (unsigned sii = 0; sii < dev_size; ++sii) {
           const auto& sentence=dev_corpus.sents[sii];
	   const vector<int>& actions=dev_corpus.actions[sii];
           dwords += sentence.size();
           {  ComputationGraph hg;
              parser.log_prob_parser(&hg,sentence,actions,&right,true);
              double lp = as_scalar(hg.incremental_forward());
              llh += lp;
           }
           double lp = 0;
           trs += actions.size();
        }
        auto t_end = chrono::high_resolution_clock::now();
        double err = (trs - right) / trs;
        //parser::EvalBResults res = parser::Evaluate("foo", pfx);
        cerr << "  **dev (iter=" << iter << " epoch=" << (tot_seen / corpus.size()) << ")\tllh=" << llh << " ppl: " << exp(llh / dwords) << " err: " << err << "\t[" << dev_size << " sents in " << chrono::duration<double, milli>(t_end-t_start).count() << " ms]" << endl;
        if (llh < best_dev_llh && (tot_seen / corpus.size()) > 1.0) {
          cerr << "  new best...writing model to " << fname << " ...\n";
          best_dev_llh = llh;
          ofstream out(fname);
          boost::archive::text_oarchive oa(out);
          oa << model;
          // Create a soft link to the most recent model in order to make it
          // easier to refer to it in a shell script.
          if (!softlinkCreated) {
            string softlink = " latest_model";
            if (system((string("rm -f ") + softlink).c_str()) == 0 &&
                system((string("ln -s ") + fname + softlink).c_str()) == 0) {
              cerr << "Created " << softlink << " as a soft link to " << fname
                   << " for convenience." << endl;
            }
            softlinkCreated = true;
          }
        }
      }
    }
  } // should do training?
  if (test_corpus.size() > 0) {
    // if rescoring, we may have many repeats, cache them
    unordered_map<vector<int>, unordered_map<vector<int>,double,boost::hash<vector<int>>>, boost::hash<vector<int>>> s2a2p;
    unsigned test_size = test_corpus.size();
    double llh = 0;
    double right = 0;
    double dwords = 0;
    for (unsigned sii = 0; sii < test_size; ++sii) {
      const auto& sentence=test_corpus.sents[sii];
      const vector<int>& actions=test_corpus.actions[sii];
      dwords += sentence.size();
      double& lp = s2a2p[sentence.raw][actions];
      if (!lp) {
        ComputationGraph hg;
        parser.log_prob_parser(&hg,sentence,actions,&right,true);
        lp = as_scalar(hg.incremental_forward());
      }
      cout << sentence.size() << '\t' << lp << endl;
      llh += lp;
    }
    cerr << "test     total -llh=" << llh << endl;
    cerr << "test ppl (per word)=" << exp(llh / dwords) << endl;
  }
}