/*
 * Copyright (C) 2019-2021 LEIDOS.
 *
 * 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 <inlanecruising_plugin/inlanecruising_plugin.h>
#include <carma_wm/WMTestLibForGuidance.h>
#include <gtest/gtest.h>
#include <ros/ros.h>
#include <lanelet2_core/primitives/Lanelet.h>
#include <lanelet2_io/Io.h>
#include <lanelet2_io/io_handlers/Factory.h>
#include <lanelet2_io/io_handlers/Writer.h>
#include <lanelet2_projection/UTM.h>
#include <lanelet2_routing/RoutingGraph.h>
#include <lanelet2_core/Attribute.h>
#include <lanelet2_core/primitives/Traits.h>
#include <lanelet2_extension/traffic_rules/CarmaUSTrafficRules.h>
#include <lanelet2_extension/utility/query.h>
#include <lanelet2_extension/projection/local_frame_projector.h>
#include <lanelet2_extension/io/autoware_osm_parser.h>
#include <carma_wm/MapConformer.h>
#include <carma_wm/CARMAWorldModel.h>
#include <ros/console.h>
#include <unsupported/Eigen/Splines>
#include <carma_utils/containers/containers.h>
#include <tf/LinearMath/Vector3.h>

#include <ros/ros.h>
#include <string>
#include <algorithm>
#include <memory>
#include <boost/uuid/uuid_generators.hpp>
#include <boost/uuid/uuid_io.hpp>
#include <lanelet2_core/geometry/Point.h>
#include <trajectory_utils/trajectory_utils.h>
#include <trajectory_utils/conversions/conversions.h>
#include <sstream>
#include <carma_utils/containers/containers.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/LU>
#include <Eigen/SVD>
#include <inlanecruising_plugin/smoothing/SplineI.h>
#include <inlanecruising_plugin/smoothing/BSpline.h>
#include <inlanecruising_plugin/inlanecruising_plugin.h>
#include <inlanecruising_plugin/log/log.h>
#include <inlanecruising_plugin/smoothing/filters.h>
#include <unordered_set>

typedef Eigen::Spline<double, 2> Spline2d;

using namespace lanelet::units::literals;
namespace inlanecruising_plugin
{

TEST(InLaneCruisingPluginTest, testPlanningCallback)
{
  InLaneCruisingPluginConfig config;
  config.default_downsample_ratio = 1;
  std::shared_ptr<carma_wm::CARMAWorldModel> wm = std::make_shared<carma_wm::CARMAWorldModel>();
  InLaneCruisingPlugin plugin(wm, config, [&](auto msg) {});

  auto map = carma_wm::test::buildGuidanceTestMap(3.7, 10);

  wm->setMap(map);
  carma_wm::test::setSpeedLimit(15_mph, wm);

  /**
   * Total route length should be 100m
   *
   *        |1203|1213|1223|
   *        | _  _  _  _  _|
   *        |1202| Ob |1222|
   *        | _  _  _  _  _|
   *        |1201|1211|1221|    num   = lanelet id hardcoded for easier testing
   *        | _  _  _  _  _|    |     = lane lines
   *        |1200|1210|1220|    - - - = Lanelet boundary
   *        |              |    O     = Default Obstacle
   *        ****************
   *           START_LINE
   */

  carma_wm::test::setRouteByIds({ 1200, 1201, 1202, 1203 }, wm);

  cav_srvs::PlanTrajectoryRequest req;
  req.vehicle_state.X_pos_global = 1.5;
  req.vehicle_state.Y_pos_global = 5;
  req.vehicle_state.orientation = 0;
  req.vehicle_state.longitudinal_vel = 0.0;

  cav_msgs::Maneuver maneuver;
  maneuver.type = cav_msgs::Maneuver::LANE_FOLLOWING;
  maneuver.lane_following_maneuver.lane_ids = {"1200"};
  maneuver.lane_following_maneuver.start_dist = 5.0;
  maneuver.lane_following_maneuver.start_time = ros::Time(0.0);
  maneuver.lane_following_maneuver.start_speed = 0.0;

  maneuver.lane_following_maneuver.end_dist = 14.98835712;
  maneuver.lane_following_maneuver.end_speed = 6.7056;
  maneuver.lane_following_maneuver.end_time = ros::Time(4.4704);

  cav_msgs::Maneuver maneuver2;
  maneuver2.type = cav_msgs::Maneuver::LANE_FOLLOWING;
  maneuver2.lane_following_maneuver.lane_ids = {"1200"};
  maneuver2.lane_following_maneuver.start_dist = 14.98835712;
  maneuver2.lane_following_maneuver.start_speed = 6.7056;
  maneuver2.lane_following_maneuver.start_time = ros::Time(4.4704);

  maneuver2.lane_following_maneuver.end_dist = 14.98835712 + 50.0;
  maneuver2.lane_following_maneuver.end_speed = 6.7056;
  maneuver2.lane_following_maneuver.end_time = ros::Time(4.4704 + 7.45645430685);

  // Create a third maneuver of a different type to test the final element in resp.related_maneuvers
  cav_msgs::Maneuver maneuver3;
  maneuver3.type = cav_msgs::Maneuver::LANE_CHANGE;

  req.maneuver_plan.maneuvers.push_back(maneuver);
  req.maneuver_plan.maneuvers.push_back(maneuver2);
  req.maneuver_plan.maneuvers.push_back(maneuver3);

  req.maneuver_index_to_plan = 0;

  cav_srvs::PlanTrajectoryResponse resp;

  plugin.plan_trajectory_cb(req, resp);

  EXPECT_EQ(1, resp.related_maneuvers.back());

}

/*
Using this file:
    1) Set the file path to your OSM file
    2) Set the route_ids to the route you wish to test

    NOTE: The test is disabled by default. Enable it by removing the DISABLED_ prefix from the test name
*/

TEST(WaypointGeneratorTest, DISABLED_test_full_generation)
{

 int projector_type = 0;
  std::string target_frame;
  lanelet::ErrorMessages load_errors;

  // File location of osm file
  std::string file = "/workspaces/carma/AOI_1_TFHRC_faster_pretty.osm";
  // The route ids that will form the route used
  std::vector<lanelet::Id> route_ids = { 130, 111, 110, 113, 135, 138 };

  // The parsing in this file was copied from https://github.com/usdot-fhwa-stol/carma-platform/blob/develop/carma_wm_ctrl/test/MapToolsTest.cpp
  lanelet::io_handlers::AutowareOsmParser::parseMapParams(file, &projector_type, &target_frame);
  lanelet::projection::LocalFrameProjector local_projector(target_frame.c_str());
  lanelet::LaneletMapPtr map = lanelet::load(file, local_projector, &load_errors);

  lanelet::MapConformer::ensureCompliance(map, 80_mph);

  InLaneCruisingPluginConfig config;
  config.lateral_accel_limit = 1.5;
  std::shared_ptr<carma_wm::CARMAWorldModel> wm = std::make_shared<carma_wm::CARMAWorldModel>();
  wm->setMap(map);

  InLaneCruisingPlugin inlc(wm, config, [&](auto msg) {});

  auto routing_graph = wm->getMapRoutingGraph();

  // Output graph for debugging
  //routing_graph->exportGraphViz("/workspaces/carma_ws/carma/src/carma-platform/waypoint_generator/routing.viz");

  carma_wm::test::setRouteByIds(route_ids, wm);

  auto p = wm->getMap()->laneletLayer.get(130).centerline()[3];
  ROS_WARN_STREAM("Start Point: " << p.x() << ", " << p.y());

  // -159.666, 521.683

  cav_srvs::PlanTrajectoryRequest req;

  req.vehicle_state.X_pos_global = -107;
  req.vehicle_state.Y_pos_global = 311.904;
  req.vehicle_state.orientation = -2.7570977;
  req.vehicle_state.longitudinal_vel = 0.0;

  cav_msgs::Maneuver maneuver;
  maneuver.type = cav_msgs::Maneuver::LANE_FOLLOWING;
  maneuver.lane_following_maneuver.lane_ids = {"110"};
  maneuver.lane_following_maneuver.start_dist = 14.98835712 + 45+ 180;
  maneuver.lane_following_maneuver.start_time = ros::Time(0.0);
  maneuver.lane_following_maneuver.start_speed = 0.0;

  maneuver.lane_following_maneuver.end_dist = 14.98835712 + 50.0 + 45 + 200;
  maneuver.lane_following_maneuver.end_speed = 6.7056;
  maneuver.lane_following_maneuver.end_time = ros::Time(8);

  cav_msgs::Maneuver maneuver2;
  maneuver2.type = cav_msgs::Maneuver::LANE_FOLLOWING;
  maneuver2.lane_following_maneuver.lane_ids = {"110"};
  maneuver2.lane_following_maneuver.start_dist = 14.98835712 + 45+ 202;
  maneuver2.lane_following_maneuver.start_speed = 6.7056;
  maneuver2.lane_following_maneuver.start_time = ros::Time(4.4704);

  maneuver2.lane_following_maneuver.end_dist = 14.98835712 + 50.0 + 45 + 250;
  maneuver2.lane_following_maneuver.end_speed = 6.7056;
  maneuver2.lane_following_maneuver.end_time = ros::Time(4.4704 + 7.45645430685 + 37.31);

  req.maneuver_plan.maneuvers.push_back(maneuver);
  req.maneuver_plan.maneuvers.push_back(maneuver2);

  cav_srvs::PlanTrajectoryResponse resp;

  inlc.plan_trajectory_cb(req, resp);

}

TEST(WaypointGeneratorTest, DISABLED_test_compute_fit_full_generation)
{
  int projector_type = 0;
  std::string target_frame;
  lanelet::ErrorMessages load_errors;

  // File location of osm file
  std::string file = "/workspaces/carma/AOI_1_TFHRC_faster_pretty.osm";
  // The route ids that will form the route used
  std::vector<lanelet::Id> route_ids = { 130, 129 };

  // The parsing in this file was copied from https://github.com/usdot-fhwa-stol/carma-platform/blob/develop/carma_wm_ctrl/test/MapToolsTest.cpp
  lanelet::io_handlers::AutowareOsmParser::parseMapParams(file, &projector_type, &target_frame);
  lanelet::projection::LocalFrameProjector local_projector(target_frame.c_str());
  lanelet::LaneletMapPtr map = lanelet::load(file, local_projector, &load_errors);

  lanelet::MapConformer::ensureCompliance(map, 80_mph);

  InLaneCruisingPluginConfig config;
  config.lateral_accel_limit = 1.5;
  std::shared_ptr<carma_wm::CARMAWorldModel> wm = std::make_shared<carma_wm::CARMAWorldModel>();
  wm->setMap(map);

  InLaneCruisingPlugin inlc(wm, config, [&](auto msg) {});

  auto routing_graph = wm->getMapRoutingGraph();

  carma_wm::test::setRouteByIds(route_ids, wm);

  auto route = wm->getRoute()->shortestPath();
  std::vector<lanelet::ConstLanelet> lanelets;
  for (auto llt : route)
  {
    lanelets.push_back(llt);
  }
  // Get centerline
  lanelet::BasicLineString2d route_geometry = carma_wm::geometry::concatenate_lanelets(lanelets);

  // Downsample
  std::vector<lanelet::BasicPoint2d> downsampled_points;
  downsampled_points.reserve((route_geometry.size() / config.default_downsample_ratio) + 1);

  for (int i = 0; i < route_geometry.size(); i += config.default_downsample_ratio)
  {
    downsampled_points.push_back(route_geometry[i]);
    // Uncomment to print and check if this original map matches with the generated one below
    // ROS_INFO_STREAM("Original point: x: " << route_geometry[i].x() << "y: " << route_geometry[i].y());
  }

  std::unique_ptr<smoothing::SplineI> fit_curve = inlc.compute_fit(downsampled_points);

  std::vector<lanelet::BasicPoint2d> spline_points;

  // Following logic is written for BSpline library. Switch with appropriate call of the new library if different.
  double parameter = 0.0;
  for(int i=0; i< downsampled_points.size(); i++){
    lanelet::BasicPoint2d pt = (*fit_curve)(parameter);
    // Uncomment to print and check if this generated map matches with the original one above
    // ROS_INFO_STREAM("BSpline point: x: " << values.x() << "y: " << values.y());
    spline_points.push_back(pt);
    parameter += 1.0/(downsampled_points.size()*1.0);
  }

  ASSERT_EQ(spline_points.size(), downsampled_points.size());
  int error_count = 0;

  for(int i = 1; i < downsampled_points.size(); i ++)
  {
    // tag as erroneous if directions of generated points are not within 5 degree of those of original points
    tf::Vector3 original_vector(downsampled_points[i].x() - downsampled_points[i-1].x(),
                      downsampled_points[i].y() - downsampled_points[i-1].y(), 0);
    original_vector.setZ(0);
    tf::Vector3 spline_vector(spline_points[i].x() - spline_points[i-1].x(),
                      spline_points[i].y() - spline_points[i-1].y(), 0);
    spline_vector.setZ(0);
    double angle_in_rad = std::fabs(tf::tfAngle(original_vector, spline_vector));
    if (angle_in_rad > 0.08)  error_count ++;
  }

  // We say it is passing if there is less than 10% error in total number of points
  ROS_INFO_STREAM("Total points above 5 degree difference in their direction:" << error_count
    << ", which is " << (double)error_count/(double)downsampled_points.size()*100 << "% of total");
  ASSERT_TRUE((double)error_count/(double)downsampled_points.size() < 0.1);
}

};  // namespace inlanecruising_plugin

// Run all the tests
int main(int argc, char **argv)
{
    testing::InitGoogleTest(&argc, argv);
    ros::Time::init();
    ROSCONSOLE_AUTOINIT;
    if( ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug) ) {
      ros::console::notifyLoggerLevelsChanged();
    }
    return RUN_ALL_TESTS();
}


/***********************************************/
//STANDARD HEADERS
/************************************************/
#include <iostream>
#include <cmath>
#include <vector>
#include <utility>
#include <chrono>
#include <unordered_map>
#include <queue>
#include <fstream>
#include <thread>
#include <ctime>

#include "pclviewer.h"
#include "ui_pclviewer.h"
/*********************************************/
//PCL HEADERS
/**********************************************/
#include <pcl/io/ply_io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/impl/point_types.hpp>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/features/normal_3d.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/registration/icp.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/common/transforms.h>
#include <pcl/features/normal_3d_omp.h>
/*********************************************/
//OTHER HEADERS
/**********************************************/
#include <Eigen/Dense>
#include <Eigen/Core>
#include <boost/property_tree/xml_parser.hpp>
#include <boost/property_tree/ptree.hpp>
#include <omp.h>
// #include "nabo/nabo.h"
#include "algorithms.hpp"

using namespace std;
using namespace Eigen;
using namespace TransformationUtilities;
using namespace InputUtilities;
using namespace InterfaceUtilities;
// using namespace Nabo;
using namespace pcl;

namespace PointCloudProcessing
{

    constexpr int degree(double radian){return int((radian*180)/3.14159);};
    constexpr double magnitude(float normal[3]){return normal[0]*normal[0]+normal[1]*normal[1]+normal[2]*normal[2];};
    constexpr int angle(float normal[3]){return degree(acos(-normal[2]/magnitude(normal)));};
    template <typename T> constexpr int sgn(T x) {
        return (T(0) < x) - (x < T(0));
    }

    constexpr double k_AngleMin = 0;
    constexpr double k_AngleMax = 45;
    constexpr double k_ZMin = 0.20;
    constexpr double k_ZMax = 1.0;

    void classifyPointCloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud,pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_classified)
    {
        PointCloud<PointXYZ>::Ptr cloud_bw (new PointCloud<PointXYZ>);
        for(int i=0;i<cloud->points.size();i++)
        {
            PointXYZ ptxyz;
            PointXYZRGB ptxyzrgb = cloud->points[i];
            ptxyz.x=ptxyzrgb.x;
            ptxyz.y=ptxyzrgb.y;
            ptxyz.z=ptxyzrgb.z*sgn(ptxyzrgb.z);
            cloud_bw->points.push_back(ptxyz);
        }
        pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
        pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> ne;
        ne.setInputCloud (cloud_bw);
        pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
        ne.setSearchMethod (tree);
        pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
        ne.setRadiusSearch (0.005);
        ne.setViewPoint(0,0,0);
        ne.compute (*normals);

        for(int i=0;i<normals->points.size();i++)
        {
            Normal ptn = normals->points[i];
            PointXYZ pt = cloud_bw->points[i];
            int angle_z = angle(ptn.normal);
            PointXYZRGB pt_new;
            pt_new.x = pt.x;
            pt_new.y = pt.y;
            pt_new.z = pt.z;
            pt_new.g = 0;
            pt_new.r = 255;
            pt_new.b = 0;
            if(angle_z>=k_AngleMin&&angle_z<=k_AngleMax&&pt_new.z>=k_ZMin&&pt_new.z<=k_ZMax)
            {
                pt_new.b = 0;
                pt_new.r = 0;
                pt_new.g = 255;
            }
            cloud_classified->points.push_back(pt_new);
        }
    }

    void filterPointCloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud,pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered)
    {
        PointCloud<PointXYZ>::Ptr cloud_bw (new PointCloud<PointXYZ>);
        for(int i=0;i<cloud->points.size();i++)
        {
            PointXYZ ptxyz;
            PointXYZRGB ptxyzrgb = cloud->points[i];
            ptxyz.x=ptxyzrgb.x;
            ptxyz.y=ptxyzrgb.y;
            ptxyz.z=ptxyzrgb.z*sgn(ptxyzrgb.z);
            cloud_bw->points.push_back(ptxyz);
        }
        pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
        pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
        ne.setInputCloud (cloud_bw);
        pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
        ne.setSearchMethod (tree);
        pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
        ne.setRadiusSearch (0.005);
        ne.setViewPoint(0,0,0);
        ne.compute (*normals);

        for(int i=0;i<normals->points.size();i++)
        {
            Normal ptn = normals->points[i];
            PointXYZRGB pt = cloud->points[i];
            int angle_z = angle(ptn.normal);
            PointXYZRGB pt_new;
            pt_new.x = pt.x;
            pt_new.y = pt.y;
            pt_new.z = pt.z*sgn(pt.z);
            pt_new.g = pt.g;
            pt_new.r = pt.r;
            pt_new.b = pt.b;
            if(angle_z>=k_AngleMin&&angle_z<=k_AngleMax&&pt_new.z>=k_ZMin&&pt_new.z<=k_ZMax)
            {
                cloud_filtered->points.push_back(pt_new);
            }
        }
    }
};

void PCLViewer::findSeedPoints()
{
    int K = 1;

    MatrixXf M = MatrixXf::Zero(3, cloud_->points.size());
    for(int i=0;i<cloud_->points.size();i++)
    {
        auto pt = cloud_->points[i];
        M(0,i) = pt.x;
        M(1,i) = pt.y;
        M(2,i) = pt.z;
    }
    // NNSearchF* nns = NNSearchF::createKDTreeLinearHeap(M);

    vector<double> error_avg(clouds_.size(),1.0/0.0);
    vector<double> error_max(clouds_.size(),1.0/0.0);
    double max_error = -1e9;
    Eigen::MatrixXd pts=Eigen::MatrixXd::Zero(1,3);
    // #pragma omp parallel
    // #pragma omp for
    for(int j=0;j<clouds_.size();j++)
    {
        if(selected_clouds_[j]==false)
        {
            continue;
        }
        float average = 0.0;
        float maximum = -1e9;
        int counter = 0;
        Eigen::MatrixXd world_T_object = vectorToTransformationMatrix(transformation_);
        Eigen::MatrixXd cam_T_flange = vectorToTransformationMatrix(flange_transformation_);
        Eigen::MatrixXd transformation = inverse_kinematics_[mapping_[j]]*cam_T_flange;
        world_T_object = world_T_object.inverse();
        MatrixXf N = MatrixXf::Zero(3, cloud_downsampled_[j]->points.size());
        Eigen::Affine3d trans;
        for(int a=0;a<3;a++)
            for(int b=0;b<4;b++)
                trans(a,b) = transformation(a,b);
        Eigen::Affine3d transW;
        for(int a=0;a<3;a++)
            for(int b=0;b<4;b++)
                transW(a,b) = world_T_object(a,b);
        for(int i=0;i<cloud_downsampled_[j]->points.size();i++)
        {
            auto point = cloud_downsampled_[j]->points[i];
#if 1
            float src[3];
            float out[3];
            src[0] = point.x;
            src[1] = point.y;
            src[2] = point.z;
            apply_transformation_optimized(src,out,trans);
            src[0] = out[0];
            src[1] = out[1];
            src[2] = out[2];
            apply_transformation_optimized(src,out,transW);
            N(0,i) = out[0];
            N(1,i) = out[1];
            N(2,i) = out[2];
#else
            pts(0,0)=point.x;
            pts(0,1)=point.y;
            pts(0,2)=point.z;
            pts=apply_transformation(pts,transformation);
            pts=apply_transformation(pts,world_T_object);
            N(0,i) = pts(0,0);
            N(1,i) = pts(0,1);
            N(2,i) = pts(0,2);
#endif
            counter++;
        }
        MatrixXi indices;
        MatrixXf dists2;
        indices.resize(1, N.cols());
        dists2.resize(1, N.cols());
        // nns->knn(N, indices, dists2, 1, 0, NNSearchF::SORT_RESULTS);
        for(int i=0;i<counter;i++)
        {
            double distance = sqrt(dists2(0,i));
            if(maximum<distance)
                maximum=distance;
            average+=distance;
        }
        average=average/counter;
        std::cout<<average<<" "<<counter<<endl;
        cout<<"Counted "<<counter<<" points."<<endl;
        // cout<<"Max Error : "<<max_error/1000.0<<endl;
        error_avg[j]=average;
        error_max[j]=maximum;
    }

    cout<<"Errors: "<<endl;
    for(int i=0;i<error_avg.size();i++)
    {
        cout<<"Error Average: "<<error_avg[i]<<" Error Max: "<<error_max[i]<<endl;
    }
}

double pointToPlaneDistanceD(vector<double> plane, vector<double> pt)
{
    return fabs(plane[0]*pt[0]+plane[1]*pt[1]+plane[2]*pt[2]+plane[3])/(sqrt(pow(plane[0],2)+pow(plane[1],2)+pow(plane[2],2)));
}

void PCLViewer::showErrorsInPoints()
{
    if(use_plane_==false)
    {
        int K = 1;
        MatrixXf M = MatrixXf::Zero(3, cloud_->points.size());
        for(int i=0;i<cloud_->points.size();i++)
        {
            auto pt = cloud_->points[i];
            M(0,i) = pt.x;
            M(1,i) = pt.y;
            M(2,i) = pt.z;
        }
        // NNSearchF* nns = NNSearchF::createKDTreeLinearHeap(M);
        Eigen::MatrixXd pts=Eigen::MatrixXd::Zero(1,3);
        vector<PointCloudT::Ptr> processed;
        for(int j=0;j<clouds_.size();j++)
        {
            PointCloudT::Ptr cloud_temp(new PointCloudT);
            if(selected_clouds_[j]==false)
            {
                processed.push_back(cloud_temp);
                continue;
            }
            float average = 0.0;
            float maximum = -1e9;
            int counter = 0;
            Eigen::MatrixXd world_T_object = vectorToTransformationMatrix(transformation_);
            Eigen::MatrixXd cam_T_flange = vectorToTransformationMatrix(flange_transformation_);
            Eigen::MatrixXd transformation = inverse_kinematics_[mapping_[j]]*cam_T_flange;
            world_T_object = world_T_object.inverse();
            MatrixXf N = MatrixXf::Zero(3, clouds_[j]->points.size());
            Eigen::Affine3d trans;
            for(int a=0;a<3;a++)
                for(int b=0;b<4;b++)
                    trans(a,b) = transformation(a,b);
            Eigen::Affine3d transW;
            for(int a=0;a<3;a++)
                for(int b=0;b<4;b++)
                    transW(a,b) = world_T_object(a,b);
            for(int i=0;i<clouds_[j]->points.size();i++)
            {
                auto point = clouds_[j]->points[i];
#if 1
                float src[3];
                float out[3];
                src[0] = point.x;
                src[1] = point.y;
                src[2] = point.z;
                apply_transformation_optimized(src,out,trans);
                src[0] = out[0];
                src[1] = out[1];
                src[2] = out[2];
                apply_transformation_optimized(src,out,transW);
                N(0,i) = out[0];
                N(1,i) = out[1];
                N(2,i) = out[2];
#else
                pts(0,0)=point.x;
                pts(0,1)=point.y;
                pts(0,2)=point.z;
                pts=apply_transformation(pts,transformation);
                pts=apply_transformation(pts,world_T_object);
                N(0,i) = pts(0,0);
                N(1,i) = pts(0,1);
                N(2,i) = pts(0,2);
#endif
                counter++;
            }
            MatrixXi indices;
            MatrixXf dists2;
            indices.resize(1, N.cols());
            dists2.resize(1, N.cols());
            // nns->knn(N, indices, dists2, 1, 0, NNSearchF::SORT_RESULTS);
            for(int i=0;i<counter;i++)
            {
                double distance = sqrt(dists2(0,i));
                auto point = clouds_[j]->points[i];
                point.r = 0;
                point.g = 0;
                point.b = 0;
                if(distance*1000.0<2.5)
                    point.g=255;
                else if(distance*1000.0<5.0)
                    point.b=255;
                else if(distance*1000.0<10.0)
                {
                    point.r = 100;
                    point.g = 100;
                }
                else
                    point.r=255;
                cloud_temp->points.push_back(point);
            }
            processed.push_back(cloud_temp);
            cout<<"Counted "<<counter<<" points."<<endl;
        }
        updateClouds(processed);
    }
    else
    {
        vector<PointCloudT::Ptr> processed;
        Eigen::MatrixXd pts=Eigen::MatrixXd::Zero(1,3);
        for(int j=0;j<clouds_.size();j++)
        {
            PointCloudT::Ptr cloud_temp(new PointCloudT);
            if(selected_clouds_[j]==false)
            {
                processed.push_back(cloud_temp);
                continue;
            }
            Eigen::MatrixXd cam_T_flange = vectorToTransformationMatrix(flange_transformation_);
            Eigen::MatrixXd transformation = inverse_kinematics_[mapping_[j]]*cam_T_flange;
            Eigen::Affine3d trans;
            for(int a=0;a<3;a++)
                for(int b=0;b<4;b++)
                    trans(a,b) = transformation(a,b);
            for(int i=0;i<clouds_[j]->points.size();i++)
            {
                auto pt = clouds_[j]->points[i];
                pts(0,0)=pt.x;
                pts(0,1)=pt.y;
                pts(0,2)=pt.z;
                pts=apply_transformation(pts,transformation);
                double distance = pointToPlaneDistanceD(plane_,{pts(0,0),pts(0,1),pts(0,2)});
                auto point = clouds_[j]->points[i];
                point.r = 0;
                point.g = 0;
                point.b = 0;
                if(distance*1000.0<2.5)
                    point.g=255;
                else if(distance*1000.0<5.0)
                    point.b=255;
                else if(distance*1000.0<10.0)
                {
                    point.r = 100;
                    point.g = 100;
                }
                else
                    point.r=255;
                cloud_temp->points.push_back(point);
            }
            processed.push_back(cloud_temp);
        }
        updateClouds(processed);
    }
}


/*
 * bow_histogram.hpp
 *
 *  Created on: Feb 8, 2012
 *      Author: lbossard
 */

#ifndef VISION_FEATURES_BOW_HISTOGRAM_HPP_
#define VISION_FEATURES_BOW_HISTOGRAM_HPP_

#include <cmath>

#include <boost/noncopyable.hpp>
#include <boost/serialization/access.hpp>

#include <opencv2/core/core.hpp>

#include <glog/logging.h>

#include "low_level_features.hpp"

#include "bow_extractor.hpp"

namespace vision
{
namespace features
{

template <typename T>
class BowHistogram_
{
public:
    typedef typename cv::Mat_<T> Histogram;

    BowHistogram_();

    BowHistogram_(Histogram& histogram, feature_type::T feature_type, std::size_t bow_count);
    virtual ~BowHistogram_();

    void set(Histogram& histogram, feature_type::T feature_type, std::size_t bow_count);

    /**
     * Returns the histograms for the specified level. One Histogram per row.
     * @param spm_level 0, ..., spm_levels() - 1
     * @return
     */
    Histogram histogram_view(unsigned int spm_level) const;

    /**
     * Returns one histogram with the given part_id. part_id goes from 0...part_count()-1
     * @param part_id 0...part_count()-1
     * @return
     */
    Histogram histogram_part(unsigned int part_id) const;

    inline const Histogram& histogram() const;
    inline Histogram& histogram();
    inline const feature_type::T fature_type() const;
    inline const int bow_count() const;
    inline unsigned int spm_levels() const;
    inline unsigned int part_count() const;

private:

    // members
    feature_type::T feature_type_;
    std::size_t bow_count_;
    Histogram histogram_ ;

    // derived members
    unsigned int spm_levels_;

    friend class boost::serialization::access;
    template <class Archive>
    void serialize(Archive& archive, const unsigned int /*version*/)
    {
        archive & feature_type_;
        archive & bow_count_;
        archive & spm_levels_;
        archive & histogram_;
    }
};

typedef BowHistogram_<float> BowHistogramFloat;
typedef BowHistogram_<double> BowHistogramDouble;
typedef BowHistogram_<int> BowHistogram;

////////////////////////////////////////////////////////////////////////////////
// Implementation

template <typename T>
/*inline*/ const
typename BowHistogram_<T>::Histogram&
BowHistogram_<T>::histogram() const
{
    return histogram_;
}

template <typename T>
/*inline*/
typename BowHistogram_<T>::Histogram&
BowHistogram_<T>::histogram()
{
    return histogram_;
}

template <typename T>
inline const feature_type::T BowHistogram_<T>::fature_type() const
{
    return feature_type_;
}

template <typename T>
inline const int BowHistogram_<T>::bow_count() const
{
    return bow_count_;
}

template <typename T>
inline unsigned int BowHistogram_<T>::spm_levels() const
{
    return spm_levels_;
}

template <typename T>
inline unsigned int BowHistogram_<T>::part_count() const
{
    return histogram_.cols / bow_count_;
}

template <typename T>
BowHistogram_<T>::BowHistogram_(Histogram& histogram, feature_type::T feature_type, std::size_t bow_count)
{
    set(histogram, feature_type, bow_count);
}

template <typename T>
BowHistogram_<T>::BowHistogram_()
{

}

template <typename T>
BowHistogram_<T>::~BowHistogram_()
{
    histogram_.release();
}

template <typename T>
void BowHistogram_<T>::set(Histogram& histogram, feature_type::T feature_type, std::size_t bow_count)
{
    histogram_ = histogram.reshape(1);
    assert(histogram_.rows == 1);
    feature_type_ = feature_type;
    bow_count_ = bow_count;
    spm_levels_ = static_cast<unsigned int>(
            log2f(3.f* histogram_.cols / bow_count + 1.f) / 2.f);

    if (BowExtractor::getHistogramCount(spm_levels_) * bow_count != histogram_.cols)
    {
        LOG(ERROR) << "dimensions of histogram does not match spm levels" << std::endl;
    }

}

template <typename T>
typename BowHistogram_<T>::Histogram
BowHistogram_<T>::histogram_view(unsigned int level) const
{
    if (level >= spm_levels_)
    {
        LOG(INFO) << "requested level (" << level
                << ") exceeds saved levels ("<< spm_levels_ << ")" << std::endl;
        return Histogram();
    }

    int histogram_count = std::pow(2, 2 * level);
    int offset = BowExtractor::getHistogramCount(level) * bow_count_;
    int width = histogram_count * bow_count_;

    // reshape http://code.opencv.org/issues/1942
    return histogram_(cv::Rect(offset,0 , width, 1)).reshape(0, histogram_count);
}

template <typename T>
typename BowHistogram_<T>::Histogram
BowHistogram_<T>::histogram_part(unsigned int part) const
{
    if (part > histogram_.cols / bow_count_)
    {
        LOG(INFO) << "requested part (" << part
                << ") exceeds saved parts ("<< histogram_.cols / bow_count_ << ")" << std::endl;
        return Histogram();
    }

    const int offset = part * bow_count_;
    const int width = bow_count_;

    return histogram_(cv::Rect(offset, 0, width, 1));

}

} /* namespace features */
} /* namespace vision */
#endif /* VISION_FEATURES_BOW_HISTOGRAM_HPP_ */


#include <puppet/compiler/evaluation/operators/unary/negate.hpp>
#include <puppet/compiler/evaluation/operators/unary/call_context.hpp>
#include <puppet/compiler/exceptions.hpp>
#include <boost/format.hpp>

using namespace std;
using namespace puppet::runtime;

namespace puppet { namespace compiler { namespace evaluation { namespace operators { namespace unary {

    descriptor negate::create_descriptor()
    {
        unary::descriptor descriptor{ ast::unary_operator::negate };

        descriptor.add("Integer", [](call_context& context) {
            auto operand = context.operand().require<int64_t>();
            if (operand == numeric_limits<int64_t>::min()) {
                throw evaluation_exception(
                    (boost::format("negation of %1% results in an arithmetic overflow.") %
                     operand
                    ).str(),
                    context.operand_context(),
                    context.context().backtrace()
                );
            }
            return -operand;
        });
        descriptor.add("Float", [](call_context& context) {
            return -context.operand().require<double>();
        });
        return descriptor;
    }

}}}}}  // namespace puppet::compiler::evaluation::operators::unary


/* Copyright (C) 2010-2019, The Regents of The University of Michigan.
 All rights reserved.

 This software was developed as part of the The Vulcan project in the Intelligent Robotics Lab
 under the direction of Benjamin Kuipers, kuipers@umich.edu. Use of this code is governed by an
 MIT-style License that can be found at "https://github.com/h2ssh/Vulcan".
*/

/**
 * \file     isovist_voronoi_gateway_generator.cpp
 * \author   Collin Johnson
 *
 * Definition of IsovistVoronoiGatewayGenerator.
 */

#include "hssh/local_topological/area_detection/gateways/isovist_voronoi_gateway_generator.h"
#include "hssh/local_topological/area_detection/gateways/endpoint_validator.h"
#include "hssh/local_topological/area_detection/gateways/gateway_utils.h"
#include "hssh/local_topological/area_detection/voronoi/voronoi_utils.h"
#include "hssh/local_topological/gateway.h"
#include "utils/algorithm_ext.h"
#include "utils/stub.h"
#include <boost/range/algorithm_ext.hpp>
#include <boost/range/as_array.hpp>
#include <boost/range/iterator_range.hpp>

// #define DEBUG_GATEWAYS

namespace vulcan
{
namespace hssh
{

std::pair<cell_t, cell_t> select_straightest_gateway_boundary(cell_t source,
                                                              cell_t skeleton,
                                                              const std::vector<cell_t>& skeletonCells,
                                                              const EndpointValidator& validator,
                                                              const VoronoiSkeletonGrid& grid);
std::pair<cell_t, cell_t> select_straightest_gateway_from_sources(cell_t skeleton,
                                                                  const std::vector<cell_t>& sourceCells,
                                                                  const VoronoiSkeletonGrid& grid);
std::pair<cell_t, double> source_for_straightest_gateway(cell_t start,
                                                         cell_t skeleton,
                                                         CellConstIter beginSources,
                                                         CellConstIter endSources,
                                                         double originalAngle,
                                                         const EndpointValidator* validator);
double gateway_score(cell_t start, cell_t end, cell_t skeleton, double originalAngle);

IsovistVoronoiGatewayGenerator::IsovistVoronoiGatewayGenerator(const isovist_voronoi_gateway_generator_params_t& params)
: nextGatewayId_(0)
, params_(params)
{
}

std::vector<WeightedGateway>
  IsovistVoronoiGatewayGenerator::generateGateways(const std::vector<WeightedGateway>& priorGateways,
                                                   const VoronoiIsovistField& isovists,
                                                   const VoronoiSkeletonGrid& grid,
                                                   const EndpointValidator& validator)
{
    VoronoiEdges edges(grid, SKELETON_CELL_REDUCED_SKELETON);
    VoronoiIsovistGradients gradients(edges);
    gradients.calculateGradients(utils::Isovist::kShapeEccentricity, isovists);

    VoronoiIsovistMaxima maxima(gradients, edges, grid, params_.numAboveMean, params_.saveGradientData);

    initialGateways_.clear();
    isovists_ = &isovists;
    grid_ = &grid;
    validator_ = &validator;

    sourceToCells_ = extract_source_cells(grid, SKELETON_CELL_REDUCED_SKELETON);

    for (auto& maximum : maxima) {
        createGatewayForMaximumIfValid(maximum);
    }

    std::cout << "INFO: IsovistVoronoiGatewayGenerator: Maxima:" << maxima.size()
              << " Num generated:" << initialGateways_.size() << '\n';

    return initialGateways_;
}

bool IsovistVoronoiGatewayGenerator::createGatewayForMaximumIfValid(const isovist_local_maximum_t& maximum)
{
    proposeGatewaysForMaximum(maximum);

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

    // There was at least one proposal, so select the smallest proposed gateway and call it good
    auto bestProposalIt = std::max_element(proposals_.begin(), proposals_.end());
    initialGateways_.emplace_back(WeightedGateway{toGateway(*bestProposalIt), bestProposalIt->gradient, false});

#ifdef DEBUG_GATEWAYS
    std::cout << "DEBUG: GatewayGenerator: Best gateway for: " << maximum.maximum.position << "->"
              << initialGateways_.back().gateway.boundary() << '\n';
#endif

    return true;
}

void IsovistVoronoiGatewayGenerator::proposeGatewaysForMaximum(const isovist_local_maximum_t& maximum)
{
    proposals_.clear();
    sourceCells_.clear();
    edgeCells_.clear();

    // Find all valid source cells for gateways and all valid skeleton edge cells

    for (auto& cell : maximum.skeletonCells) {
        // Find valid source cells associated with this skeleton cell
        std::copy_if(grid_->beginSourceCells(cell.position),
                     grid_->endSourceCells(cell.position),
                     std::back_inserter(sourceCells_),
                     [this](cell_t source) {
                         return validator_->isValidEndpoint(source);
                     });

        // Store the cell itself for further gateway creation
        edgeCells_.push_back(cell.position);
    }
    std::sort(sourceCells_.begin(), sourceCells_.end());
    utils::erase_unique(sourceCells_);

    // For each skeleton cell, find the best possible gateway
    for (auto& cell : maximum.skeletonCells) {
        // Ignore any cells that fall on a junction because
        if (num_neighbor_cells_with_classification(cell.position,
                                                   SKELETON_CELL_REDUCED_SKELETON,
                                                   *grid_,
                                                   FOUR_THEN_EIGHT_WAY)
            > 2) {
            continue;
        }

        auto gateway = proposeGatewayForSkeletonCell(cell.position);

        // A gateway isn't guaranteed to be found
        if (gateway) {
            gateway->gradient = cell.value;
            proposals_.push_back(*gateway);
        }
    }

#ifdef DEBUG_GATEWAYS
    if (proposals_.empty()) {
        std::cout << "Found no gateways for maximum centered at " << maximum.maximum.position << '\n';
    }
#endif   // DEBUG_GATEWAYS
}

boost::optional<IsovistVoronoiGatewayGenerator::proposed_gateway_t>
  IsovistVoronoiGatewayGenerator::proposeGatewayForSkeletonCell(cell_t skeletonCell)
{
    if ((sourceCells_.size() < 2) || isInvalidSkeletonCell(skeletonCell)) {
        return boost::none;
    }

    if (params_.useSkeletonBasedGateways) {
        auto bestGateway = select_straightest_gateway_from_sources(skeletonCell, sourceCells_, *grid_);

        if (isValidProposal(skeletonCell, bestGateway.first, bestGateway.second)) {
            return proposed_gateway_t(bestGateway, skeletonCell);
        }
    } else if (params_.useSourceBasedGateways) {
        // Go through each gateway and find the one with the highest score
        auto bestGateway = createProposalForSource(*(grid_->beginSourceCells(skeletonCell)), skeletonCell);
        for (auto source : boost::make_iterator_range(grid_->beginSourceCells(skeletonCell) + 1,
                                                      grid_->endSourceCells(skeletonCell))) {
            auto sourceGateway = createProposalForSource(source, skeletonCell);

            if (sourceGateway) {
                if (!bestGateway || (sourceGateway->score() > bestGateway->score())) {
                    bestGateway = sourceGateway;
                }
            }
        }

        return bestGateway;
    } else {
        std::cerr << "ERROR: IsovistVoronoiGatewayGenerator: Invalid parameters. Must have one of gateway selection "
                     "flags set to true.\n";
        assert(false);
    }

    return boost::none;
}

boost::optional<IsovistVoronoiGatewayGenerator::proposed_gateway_t>
  IsovistVoronoiGatewayGenerator::createProposalForSource(cell_t source, cell_t skeleton)
{
    auto bestGateway = select_straightest_gateway_boundary(source, skeleton, edgeCells_, *validator_, *grid_);

    if (isValidProposal(bestGateway.first, source, bestGateway.second)) {
        return proposed_gateway_t(std::make_pair(source, bestGateway.second), bestGateway.first);
    }

    return boost::none;
}

bool IsovistVoronoiGatewayGenerator::isInvalidSkeletonCell(cell_t skeleton)
{
    // A skeleton cell is invalid if:
    //  - it is at the junction of the underlying skeleton
    //  - no sources on difference sides of the skeleton (angle between them and skeleton < M_PI_2), or
    //  - no sources both valid endpoints

    if (num_neighbor_cells_with_classification(skeleton, SKELETON_CELL_SKELETON, *grid_, FOUR_THEN_EIGHT_WAY) > 2) {
        return true;
    }

    for (auto sourceIt = grid_->beginSourceCells(skeleton), endIt = grid_->endSourceCells(skeleton); sourceIt != endIt;
         ++sourceIt) {
        // Skip any sources that aren't valid. No point in searching the next ones
        if (!validator_->isValidEndpoint(*sourceIt)) {
            continue;
        }

        for (auto nextIt = sourceIt + 1; nextIt != endIt; ++nextIt) {
            if (!validator_->isValidEndpoint(*nextIt)) {
                continue;
            }

            // Both sources were valid, but are they on separate sides of the skeleton?
            if (angle_between_points(*sourceIt, *nextIt, skeleton) > M_PI_2) {
                return false;
            }
        }
    }

    // No valid pair of sources was found
    return true;
}

bool IsovistVoronoiGatewayGenerator::isValidProposal(cell_t skeleton, cell_t sourceA, cell_t sourceB)
{
    return (std::abs(angle_between_points(sourceA, sourceB, skeleton)) > params_.minGatewayStraightness)
      && validator_->isValidEndpoint(sourceA) && validator_->isValidEndpoint(sourceB);
}

Gateway IsovistVoronoiGatewayGenerator::toGateway(const proposed_gateway_t& proposed)
{
    return Gateway(grid_->getTimestamp(),
                   nextGatewayId_++,
                   Line<int>(proposed.sources.first, proposed.sources.second),
                   proposed.skeleton,
                   *isovists_,
                   *grid_);
}

std::pair<cell_t, cell_t> select_straightest_gateway_boundary(cell_t source,
                                                              cell_t skeleton,
                                                              const std::vector<cell_t>& skeletonCells,
                                                              const EndpointValidator& validator,
                                                              const VoronoiSkeletonGrid& grid)
{
    double originalAngle = angle_to_point(source, skeleton);

    auto straightestBoundary = std::make_pair(source, 0.0);
    cell_t straightestSkeleton;

    for (auto cell : skeletonCells) {
        // Skip all junctions. They aren't valid gateway boundaries
        if (num_neighbor_cells_with_classification(cell, SKELETON_CELL_REDUCED_SKELETON, grid, FOUR_THEN_EIGHT_WAY)
            > 2) {
            continue;
        }

        auto sourceBoundary = source_for_straightest_gateway(source,
                                                             cell,
                                                             grid.beginSourceCells(cell),
                                                             grid.endSourceCells(cell),
                                                             originalAngle,
                                                             &validator);

        //         (sourceBoundary.second > straightestBoundary.second) ||
        //         (absolute_fuzzy_equal(sourceBoundary.second, straightestBoundary.second)
        //         && distance_between_points(source, sourceBoundary.first) < distance_between_points(source,
        //         straightestBoundary.first)))

        if (sourceBoundary.second > straightestBoundary.second) {
            straightestBoundary = sourceBoundary;
            straightestSkeleton = cell;
        }
    }

    return std::make_pair(straightestSkeleton, straightestBoundary.first);
}

std::pair<cell_t, cell_t> select_straightest_gateway_from_sources(cell_t skeleton,
                                                                  const std::vector<cell_t>& sourceCells,
                                                                  const VoronoiSkeletonGrid& grid)
{
    assert(sourceCells.size() > 1);

    std::pair<cell_t, cell_t> straightestBoundary;
    double maxScore = 0.0;

    for (std::size_t n = 0; n < sourceCells.size(); ++n) {
        auto sourceBoundary = source_for_straightest_gateway(sourceCells[n],
                                                             skeleton,
                                                             sourceCells.begin() + n + 1,
                                                             sourceCells.end(),
                                                             angle_to_point(sourceCells[n], skeleton),
                                                             nullptr);

        if (sourceBoundary.second > maxScore) {
            straightestBoundary = std::make_pair(sourceCells[n], sourceBoundary.first);
            maxScore = sourceBoundary.second;
        }
    }

    return straightestBoundary;
}

std::pair<cell_t, double> source_for_straightest_gateway(cell_t start,
                                                         cell_t skeleton,
                                                         CellConstIter beginSources,
                                                         CellConstIter endSources,
                                                         double originalAngle,
                                                         const EndpointValidator* validator)
{
    double maxScore = 0.0;
    cell_t maxCell;

    for (auto source : boost::make_iterator_range(beginSources, endSources)) {
        if (validator && !validator->isValidEndpoint(source)) {
            continue;
        }

        double sourceScore = gateway_score(start, source, skeleton, originalAngle);

        if (sourceScore > maxScore) {
            maxScore = sourceScore;
            maxCell = source;
        }
    }

    return std::make_pair(maxCell, maxScore);
}

double gateway_score(cell_t start, cell_t end, cell_t skeleton, double originalAngle)
{
    double skeletonDirection = angle_to_point(start, skeleton);
    double length = distance_between_points(start, skeleton) + distance_between_points(end, skeleton);
    double angle = angle_between_points(start, end, skeleton);
    return (length > 0.0) ? (1.0 - angle_diff_abs(originalAngle, skeletonDirection) / M_PI) * angle / length : 0.0;
}

}   // namespace hssh
}   // namespace vulcan


#ifndef TARJAN_SCC_HPP
#define TARJAN_SCC_HPP

#include "extractor/node_based_edge.hpp"
#include "extractor/query_node.hpp"
#include "util/deallocating_vector.hpp"
#include "util/percent.hpp"
#include "util/typedefs.hpp"

#include "util/integer_range.hpp"
#include "util/log.hpp"
#include "util/std_hash.hpp"
#include "util/timing_util.hpp"

#include "osrm/coordinate.hpp"
#include <boost/assert.hpp>
#include <cstdint>

#include <algorithm>
#include <climits>
#include <memory>
#include <stack>
#include <vector>

namespace osrm
{
namespace extractor
{

template <typename GraphT> class TarjanSCC
{
    struct TarjanStackFrame
    {
        explicit TarjanStackFrame(NodeID v, NodeID parent) : v(v), parent(parent) {}
        NodeID v;
        NodeID parent;
    };

    struct TarjanNode
    {
        TarjanNode() : index(SPECIAL_NODEID), low_link(SPECIAL_NODEID), on_stack(false) {}
        unsigned index;
        unsigned low_link;
        bool on_stack;
    };

    std::vector<unsigned> components_index;
    std::vector<NodeID> component_size_vector;
    const GraphT &m_graph;
    std::size_t size_one_counter;

  public:
    TarjanSCC(const GraphT &graph)
        : components_index(graph.GetNumberOfNodes(), SPECIAL_NODEID), m_graph(graph),
          size_one_counter(0)
    {
        BOOST_ASSERT(m_graph.GetNumberOfNodes() > 0);
    }

    void Run()
    {
        TIMER_START(SCC_RUN);
        const NodeID max_node_id = m_graph.GetNumberOfNodes();

        // The following is a hack to distinguish between stuff that happens
        // before the recursive call and stuff that happens after
        std::stack<TarjanStackFrame> recursion_stack;
        // true = stuff before, false = stuff after call
        std::stack<NodeID> tarjan_stack;
        std::vector<TarjanNode> tarjan_node_list(max_node_id);
        unsigned component_index = 0, size_of_current_component = 0;
        unsigned large_component_count = 0;
        unsigned index = 0;
        std::vector<bool> processing_node_before_recursion(max_node_id, true);
        for (const NodeID node : util::irange(0u, max_node_id))
        {
            if (SPECIAL_NODEID == components_index[node])
            {
                recursion_stack.emplace(TarjanStackFrame(node, node));
            }

            while (!recursion_stack.empty())
            {
                TarjanStackFrame currentFrame = recursion_stack.top();
                const NodeID u = currentFrame.parent;
                const NodeID v = currentFrame.v;
                recursion_stack.pop();

                const bool before_recursion = processing_node_before_recursion[v];

                if (before_recursion && tarjan_node_list[v].index != UINT_MAX)
                {
                    continue;
                }

                if (before_recursion)
                {
                    // Mark frame to handle tail of recursion
                    recursion_stack.emplace(currentFrame);
                    processing_node_before_recursion[v] = false;

                    // Mark essential information for SCC
                    tarjan_node_list[v].index = index;
                    tarjan_node_list[v].low_link = index;
                    tarjan_stack.push(v);
                    tarjan_node_list[v].on_stack = true;
                    ++index;

                    for (const auto current_edge : m_graph.GetAdjacentEdgeRange(v))
                    {
                        const auto vprime = m_graph.GetTarget(current_edge);

                        if (SPECIAL_NODEID == tarjan_node_list[vprime].index)
                        {
                            recursion_stack.emplace(TarjanStackFrame(vprime, v));
                        }
                        else
                        {
                            if (tarjan_node_list[vprime].on_stack &&
                                tarjan_node_list[vprime].index < tarjan_node_list[v].low_link)
                            {
                                tarjan_node_list[v].low_link = tarjan_node_list[vprime].index;
                            }
                        }
                    }
                }
                else
                {
                    processing_node_before_recursion[v] = true;
                    tarjan_node_list[u].low_link =
                        std::min(tarjan_node_list[u].low_link, tarjan_node_list[v].low_link);
                    // after recursion, lets do cycle checking
                    // Check if we found a cycle. This is the bottom part of the recursion
                    if (tarjan_node_list[v].low_link == tarjan_node_list[v].index)
                    {
                        NodeID vprime;
                        do
                        {
                            vprime = tarjan_stack.top();
                            tarjan_stack.pop();
                            tarjan_node_list[vprime].on_stack = false;
                            components_index[vprime] = component_index;
                            ++size_of_current_component;
                        } while (v != vprime);

                        component_size_vector.emplace_back(size_of_current_component);

                        if (size_of_current_component > 1000)
                        {
                            ++large_component_count;
                            util::Log(logDEBUG) << "large component [" << component_index
                                                << "]=" << size_of_current_component;
                        }

                        ++component_index;
                        size_of_current_component = 0;
                    }
                }
            }
        }

        TIMER_STOP(SCC_RUN);
        util::Log() << "Found " << component_index << " SCC (" << large_component_count
                    << " large, " << (component_index - large_component_count) << " small)";
        util::Log() << "SCC run took: " << TIMER_MSEC(SCC_RUN) / 1000. << "s";

        size_one_counter = std::count_if(component_size_vector.begin(),
                                         component_size_vector.end(),
                                         [](unsigned value) { return 1 == value; });
    }

    std::size_t GetNumberOfComponents() const { return component_size_vector.size(); }

    std::size_t GetSizeOneCount() const { return size_one_counter; }

    unsigned GetComponentSize(const unsigned component_id) const
    {
        return component_size_vector[component_id];
    }

    unsigned GetComponentID(const NodeID node) const { return components_index[node]; }
};
} // namespace extractor
} // namespace osrm

#endif /* TARJAN_SCC_HPP */


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

  @copyright 2015 NumScale SAS
  @copyright 2015 J.T. Lapreste

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

#include <boost/simd/function/is_eqz.hpp>
#include <boost/simd/meta/as_logical.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  BOOST_DISPATCH_OVERLOAD ( is_imag_
                          , (typename A0)
                          , bd::cpu_
                          , bd::generic_< bd::arithmetic_<A0> >
                          )
  {
    BOOST_FORCEINLINE bs::as_logical_t<A0> operator() ( A0 const &a0) const BOOST_NOEXCEPT
    {
      return is_eqz(a0);
    }
  };
} } }

#endif


//---------------------------------------------------------------------------//
//!
//! \file   DataGen_AdjointElectroionizationSubshellCrossSectionEvaluator.hpp
//! \author Luke Kersting
//! \brief  Adjoint electroionization subshell cross section evaluator definition
//!
//---------------------------------------------------------------------------//

#ifndef DATA_GEN_ADJOINT_ELECTROIONIZATION_SUBSHELL_CROSS_SECTION_EVALUATOR_HPP
#define DATA_GEN_ADJOINT_ELECTROIONIZATION_SUBSHELL_CROSS_SECTION_EVALUATOR_HPP

// Boost Includes
#include <boost/scoped_ptr.hpp>
/*
// Std Lib Includes
#include <limits>

// Boost Includes
#include <boost/distribution.hpp>
#include <boost/bind.hpp>
*/
// Trilinos Includes
#include <Teuchos_Array.hpp>
#include <Teuchos_RCP.hpp>

// FRENSIE Includes
#include "MonteCarlo_ElectroatomicReaction.hpp"
#include "Utility_OneDDistribution.hpp"
#include "Utility_TabularOneDDistribution.hpp"
#include "MonteCarlo_ElectroionizationSubshellElectronScatteringDistribution.hpp"

namespace DataGen{

//! The adjoint electroionization subshell cross section class
class AdjointElectroionizationSubshellCrossSectionEvaluator
{

public:

  //! Typedef for the electroionization subshell distribution
  typedef Teuchos::Array<Utility::Pair<double,
		       Teuchos::RCP<const Utility::TabularOneDDistribution> > >
  ElectroionizationSubshellDistribution;

  //! Constructor
  AdjointElectroionizationSubshellCrossSectionEvaluator(
    const double& binding_energy,
    const Teuchos::RCP<MonteCarlo::ElectroatomicReaction>&
                               electroionization_subshell_reaction,
    const Teuchos::RCP<const MonteCarlo::ElectroionizationSubshellElectronScatteringDistribution>&
                               knock_on_distribution );

  //! Destructor
  ~AdjointElectroionizationSubshellCrossSectionEvaluator()
  { /* ... */ }

  //! Evaluate the differential adjoint electroionization subshell cross section (dc/dx)
  double evaluateDifferentialCrossSection(
            const double incoming_energy,
            const double outgoing_energy ) const;

  //! Return the cross section value at a given energy
  double evaluateCrossSection( const double energy,
                               const double precision = 1e-6 ) const;

  //! Return the max outgoing adjoint energy for a given energy
  double getMaxOutgoingEnergyAtEnergy( const double energy );

private:

  // The forward reaction data
  Teuchos::RCP<MonteCarlo::ElectroatomicReaction> d_electroionization_subshell_reaction;

  // The knock on energy distribution
  Teuchos::RCP<const MonteCarlo::ElectroionizationSubshellElectronScatteringDistribution>
                                         d_knock_on_distribution;

  // The subshell binding energy
  double d_binding_energy;
};

} // end DataGen namespace

#endif // end DATA_GEN_ADJOINT_ELECTROIONIZATION_SUBSHELL_CROSS_SECTION_EVALUATOR_HPP

//---------------------------------------------------------------------------//
// end DataGen_AdjointElectroionizationSubshellCrossSectionEvaluator.hpp
//---------------------------------------------------------------------------//


#include <cctbx/boost_python/flex_fwd.h>

#include <boost/python/def.hpp>
#include <boost/python/class.hpp>
#include <boost/python/args.hpp>
#include <boost/python/return_value_policy.hpp>
#include <boost/python/copy_const_reference.hpp>
#include <boost/python/return_by_value.hpp>
#include <scitbx/array_family/boost_python/shared_wrapper.h>
#include <scitbx/array_family/selections.h>
#include <scitbx/stl/map_wrapper.h>
#include <cctbx/geometry_restraints/bond_similarity.h>

namespace cctbx { namespace geometry_restraints {
namespace {

  struct bond_similarity_proxy_wrappers : boost::python::pickle_suite
  {
    typedef bond_similarity_proxy w_t;

    static boost::python::tuple
      getinitargs(w_t const& self)
    {
      return boost::python::make_tuple(self.i_seqs,
        self.sym_ops,
        self.weights);
    }

    static void
    wrap()
    {
      using namespace boost::python;
      typedef return_value_policy<return_by_value> rbv;
      class_<w_t>("bond_similarity_proxy", no_init)
        .def(init<
          af::shared<af::tiny<std::size_t, 2> >,
          af::shared<double> const&>((
            arg("i_seqs"),
            arg("weights"))))
        .def(init<
          af::shared<af::tiny<std::size_t, 2> >,
          af::shared<sgtbx::rt_mx>,
          af::shared<double> const&>((
            arg("i_seqs"),
            arg("sym_ops"),
            arg("weights"))))
        .add_property("i_seqs", make_getter(&w_t::i_seqs, rbv()))
        .add_property("weights", make_getter(&w_t::weights, rbv()))
        .add_property("sym_ops", make_getter(&w_t::sym_ops, rbv()))
        .def_pickle(bond_similarity_proxy_wrappers())
        ;
      {
        typedef return_internal_reference<> rir;
        scitbx::af::boost_python::shared_wrapper<
          bond_similarity_proxy, rir>::wrap(
          "shared_bond_similarity_proxy");
      }
    }
  };

  struct bond_similarity_wrappers : boost::python::pickle_suite
  {
    typedef bond_similarity w_t;

    static boost::python::tuple
      getinitargs(w_t const& self)
    {
      return boost::python::make_tuple(self.sites_array,
        self.weights);
    }

    static void
    wrap()
    {
      using namespace boost::python;
      typedef return_value_policy<copy_const_reference> ccr;
      typedef return_value_policy<return_by_value> rbv;
      scitbx::af::boost_python::shared_wrapper<
        af::tiny<scitbx::vec3<double>, 2>, rbv>::wrap(
        "sites_array");
      class_<w_t>("bond_similarity", no_init)
        .def(init<
          af::shared<af::tiny<scitbx::vec3<double>, 2> > const&,
          af::shared<double> const&>((
            arg("sites_array"),
            arg("weights"))))
        .def(init<uctbx::unit_cell const&,
                  af::const_ref<scitbx::vec3<double> > const&,
                  bond_similarity_proxy const&>(
          (arg("unit_cell"), arg("sites_cart"), arg("proxy"))))
        .def(init<af::const_ref<scitbx::vec3<double> > const&,
                  bond_similarity_proxy const&>(
          (arg("sites_cart"), arg("proxy"))))
        .add_property("sites_array", make_getter(
          &w_t::sites_array, rbv()))
        .add_property("weights", make_getter(&w_t::weights, rbv()))
        .def("deltas", &w_t::deltas, ccr())
        .def("rms_deltas", &w_t::rms_deltas)
        .def("residual", &w_t::residual)
        .def("gradients", &w_t::gradients)
        .def("mean_distance", &w_t::mean_distance)
        .def_pickle(bond_similarity_wrappers())
        ;
    }
  };

  void
  wrap_all()
  {
    using namespace boost::python;
    bond_similarity_proxy_wrappers::wrap();
    bond_similarity_wrappers::wrap();
    def("bond_similarity_deltas_rms",
      (af::shared<double>(*)(
        af::const_ref<scitbx::vec3<double> > const&,
        af::const_ref<bond_similarity_proxy> const&))
      bond_similarity_deltas_rms,
      (arg("sites_cart"), arg("proxies")));
    def("bond_similarity_residuals",
      (af::shared<double>(*)(
        af::const_ref<scitbx::vec3<double> > const&,
        af::const_ref<bond_similarity_proxy> const&))
      bond_similarity_residuals,
      (arg("sites_cart"), arg("proxies")));
    def("bond_similarity_residual_sum",
      (double(*)(
        af::const_ref<scitbx::vec3<double> > const&,
        af::const_ref<bond_similarity_proxy> const&,
        af::ref<scitbx::vec3<double> > const&))
      bond_similarity_residual_sum,
      (arg("sites_cart"),
       arg("proxies"),
       arg("gradient_array")));
    def("bond_similarity_deltas_rms",
      (af::shared<double>(*)(
        uctbx::unit_cell const&,
        af::const_ref<scitbx::vec3<double> > const&,
        af::const_ref<bond_similarity_proxy> const&))
      bond_similarity_deltas_rms,
      (arg("unit_cell"), arg("sites_cart"), arg("proxies")));
    def("bond_similarity_residuals",
      (af::shared<double>(*)(
        uctbx::unit_cell const&,
        af::const_ref<scitbx::vec3<double> > const&,
        af::const_ref<bond_similarity_proxy> const&))
      bond_similarity_residuals,
      (arg("unit_cell"), arg("sites_cart"), arg("proxies")));
    def("bond_similarity_residual_sum",
      (double(*)(
        uctbx::unit_cell const&,
        af::const_ref<scitbx::vec3<double> > const&,
        af::const_ref<bond_similarity_proxy> const&,
        af::ref<scitbx::vec3<double> > const&))
      bond_similarity_residual_sum,
      (arg("unit_cell"),
       arg("sites_cart"),
       arg("proxies"),
       arg("gradient_array")));
  }

} // namespace <anonymous>

namespace boost_python {

  void
  wrap_bond_similarity() { wrap_all(); }

}}} // namespace cctbx::geometry_restraints::boost_python


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

#ifndef CRYPTO3_HASH_DETAIL_H2F_FUNCTIONS_HPP
#define CRYPTO3_HASH_DETAIL_H2F_FUNCTIONS_HPP

#include <cstdint>
#include <array>
#include <vector>
#include <iterator>
#include <type_traits>

#include <boost/assert.hpp>
#include <boost/static_assert.hpp>
#include <boost/concept/assert.hpp>

#include <nil/marshalling/endianness.hpp>
#include <nil/marshalling/algorithms/pack.hpp>
#include <nil/crypto3/marshalling/multiprecision/types/integral.hpp>

#include <nil/crypto3/algebra/type_traits.hpp>

#include <nil/crypto3/detail/strxor.hpp>

#include <nil/crypto3/hash/algorithm/hash.hpp>

#include <nil/crypto3/hash/detail/h2c/h2c_policy.hpp>

namespace nil {
    namespace crypto3 {
        namespace hashes {
            namespace detail {
                template<std::size_t k, std::size_t len_in_bytes, typename Hash,
                         /// Hash::digest_type is required to be uint8_t[]
                         typename = typename std::enable_if<std::is_same<
                             std::uint8_t,
                             typename std::iterator_traits<typename Hash::digest_type>::value_type>::value>::type>
                class expand_message_xmd {
                    // https://tools.ietf.org/html/draft-irtf-cfrg-hash-to-curve-10#section-5.4.1
                    static_assert(Hash::block_bits % 8 == 0, "r_in_bytes is not a multiple of 8");
                    static_assert(Hash::digest_bits % 8 == 0, "b_in_bytes is not a multiple of 8");
                    static_assert(Hash::digest_bits >= 2 * k, "k-bit collision resistance is not fulfilled");
                    static_assert(len_in_bytes < 0x10000, "len_in_bytes should be less than 0x10000");

                    constexpr static std::size_t b_in_bytes = Hash::digest_bits / 8;
                    constexpr static std::size_t r_in_bytes = Hash::block_bits / 8;
                    constexpr static std::array<std::uint8_t, 2> l_i_b_str = {
                        static_cast<std::uint8_t>(len_in_bytes >> 8u), static_cast<std::uint8_t>(len_in_bytes % 0x100)};
                    constexpr static std::size_t ell = static_cast<std::size_t>(len_in_bytes / b_in_bytes) +
                                                       static_cast<std::size_t>(len_in_bytes % b_in_bytes != 0);
                    constexpr static const std::array<std::uint8_t, r_in_bytes> Z_pad {0};

                    // https://tools.ietf.org/html/draft-irtf-cfrg-hash-to-curve-10#section-5.4.1
                    static_assert(ell <= 255, "ell should be less than 256");

                public:
                    typedef std::array<std::uint8_t, len_in_bytes> result_type;
                    typedef accumulator_set<Hash> internal_accumulator_type;

                    static inline void init_accumulator(internal_accumulator_type &acc) {
                        hash<Hash>(Z_pad, acc);
                    }

                    template<typename InputRange>
                    static inline void update(internal_accumulator_type &acc, const InputRange &range) {
                        BOOST_CONCEPT_ASSERT((boost::SinglePassRangeConcept<InputRange>));

                        hash<Hash>(range, acc);
                    }

                    template<typename InputIterator>
                    static inline void update(internal_accumulator_type &acc, InputIterator first, InputIterator last) {
                        BOOST_CONCEPT_ASSERT((boost::InputIteratorConcept<InputIterator>));

                        hash<Hash>(first, last, acc);
                    }

                    template<typename DstRange>
                    static inline typename std::enable_if<
                        std::is_same<std::uint8_t,
                                     typename std::iterator_traits<typename DstRange::iterator>::value_type>::value,
                        result_type>::type
                        process(internal_accumulator_type &b0_acc, const DstRange &dst) {

                        auto dst_size = std::distance(std::cbegin(dst), std::cend(dst));
                        assert(dst_size >= 16 && dst_size <= 255);

                        hash<Hash>(l_i_b_str, b0_acc);
                        hash<Hash>(std::array<std::uint8_t, 1> {0}, b0_acc);
                        hash<Hash>(dst, b0_acc);
                        hash<Hash>(std::array<std::uint8_t, 1> {static_cast<std::uint8_t>(dst_size)}, b0_acc);
                        typename Hash::digest_type b0 = ::nil::crypto3::accumulators::extract::hash<Hash>(b0_acc);

                        result_type uniform_bytes;
                        internal_accumulator_type bi_acc;
                        hash<Hash>(b0, bi_acc);
                        hash<Hash>(std::array<std::uint8_t, 1> {1}, bi_acc);
                        hash<Hash>(dst, bi_acc);
                        hash<Hash>(std::array<std::uint8_t, 1> {static_cast<std::uint8_t>(dst_size)}, bi_acc);
                        typename Hash::digest_type bi = ::nil::crypto3::accumulators::extract::hash<Hash>(bi_acc);
                        std::copy(bi.begin(), bi.end(), uniform_bytes.begin());

                        typename Hash::digest_type xored_b;
                        for (std::size_t i = 2; i <= ell; i++) {
                            internal_accumulator_type bi_acc;
                            ::nil::crypto3::detail::strxor(b0, bi, xored_b.begin());
                            hash<Hash>(xored_b, bi_acc);
                            hash<Hash>(std::array<std::uint8_t, 1> {static_cast<std::uint8_t>(i)}, bi_acc);
                            hash<Hash>(dst, bi_acc);
                            hash<Hash>(std::array<std::uint8_t, 1> {static_cast<std::uint8_t>(dst_size)}, bi_acc);
                            bi = ::nil::crypto3::accumulators::extract::hash<Hash>(bi_acc);
                            std::copy(bi.begin(), bi.end(), uniform_bytes.begin() + (i - 1) * b_in_bytes);
                        }
                        return uniform_bytes;
                    }
                };
            }    // namespace detail
        }        // namespace hashes
    }            // namespace crypto3
}    // namespace nil

#endif    // CRYPTO3_HASH_DETAIL_H2F_FUNCTIONS_HPP


#include "APSimulator.hpp"

#include "RegularStimulus.hpp"
#include "SimpleStimulus.hpp"
#include "MultiStimulus.hpp"
#include "AbstractIvpOdeSolver.hpp"
#include <boost/shared_ptr.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/tokenizer.hpp>
#include "BoostFilesystem.hpp"
#include "RandomNumberGenerator.hpp"

#include "CheckpointArchiveTypes.hpp"
#include "ArchiveLocationInfo.hpp"
#include "SteadyStateRunner.hpp"

#include "hodgkin_huxley_squid_axon_model_1952_modifiedCvode.hpp"
#include "beeler_reuter_model_1977Cvode.hpp"
#include "luo_rudy_1991Cvode.hpp"
#include "ten_tusscher_model_2004_epiCvode.hpp"
#include "decker_2009Cvode.hpp"
//#include "ten_tusscher_model_2004_epiCvodeOpt.hpp" // trying to catch blip "davies_isap_2012CvodeDataClampOpt.hpp"
#include "ohara_rudy_2011_endoCvodeDataClamp.hpp"
#include "davies_isap_2012CvodeDataClamp.hpp"
//#include "davies_isap_2012Cvode.hpp"
#include "paci_hyttinen_aaltosetala_severi_ventricularVersionCvodeDataClamp.hpp"
#include "gokhale_ex293_2017Cvode.hpp"

APSimulator::APSimulator()
    : mModelNumber(1),
      mNumberOfFailedSolves(0),
      mHowManySolves(1),
      mSolveStart(0),
      mSolveEnd(500),
      mSolveTimestep(0.2),
      mNumTimePts(2501),
      mUseDataClamp(false),
      mDataClampOn(0),
      mDataClampOff(1),
      mHaveRunToSteadyState(false)
{
    //RedirectStdErr();
    std::cerr << "*** INSIDE CONSTRUCTOR (nothing should happen here) ***" << std::endl << std::flush;
}

APSimulator::~APSimulator()
{
    //fclose(stderr);
}

/*
void APSimulator::RedirectStdErr()
{
    boost::filesystem::path stderr_dir = "/home/rossj/stderr/"; // should find a better location for this
    std::string log_file_path = stderr_dir.string() + "stderr.log";

    boost::filesystem::create_directories(stderr_dir);

    std::cerr << stderr_dir << std::endl << std::flush;

    freopen( log_file_path.c_str(), "w", stderr );
}
*/

void APSimulator::DefineStimulus(double stimulus_magnitude, double stimulus_duration, double stimulus_period, double stimulus_start_time)
{
    mpStimulus.reset(new RegularStimulus(stimulus_magnitude,stimulus_duration,stimulus_period,stimulus_start_time));
    mStimPeriod = stimulus_period;
}

void APSimulator::DefineSolveTimes(double solve_start, double solve_end, double solve_timestep)
{
    std::cerr << "About to define solve times" << std::endl << std::flush;
    mSolveStart = solve_start;
    mSolveEnd = solve_end;
    mSolveTimestep = solve_timestep;
    mNumTimePts = (solve_end - solve_start)/solve_timestep + 1;
    std::cerr << "Solve times defined" << std::endl << std::flush;
}

void APSimulator::DefineModel(unsigned model_number)
{
    mModelNumber = model_number;
    boost::shared_ptr<AbstractIvpOdeSolver> p_solver;
    if ( model_number == 1u )
    {
        mpModel.reset(new Cellhodgkin_huxley_squid_axon_model_1952_modifiedFromCellMLCvode(p_solver, mpStimulus));
        mParameterMetanames.push_back("membrane_fast_sodium_current_conductance"); // 120
        mParameterMetanames.push_back("membrane_potassium_current_conductance");   // 36
        mParameterMetanames.push_back("membrane_leakage_current_conductance");     // 0.3
    }
    else if ( model_number == 2u )
    {
        mpModel.reset(new Cellbeeler_reuter_model_1977FromCellMLCvode(p_solver, mpStimulus));
        mParameterMetanames.push_back("membrane_fast_sodium_current_conductance");                        // 4e-2
        mParameterMetanames.push_back("membrane_inward_rectifier_potassium_current_conductance");         // 0.0035
        mParameterMetanames.push_back("membrane_rapid_delayed_rectifier_potassium_current_conductance");  // 0.008
        mParameterMetanames.push_back("membrane_L_type_calcium_current_conductance");                     // 9e-4
    }
    else if ( model_number == 3u )
    {
        mpModel.reset(new Cellluo_rudy_1991FromCellMLCvode(p_solver, mpStimulus));
        mParameterMetanames.push_back("membrane_fast_sodium_current_conductance");                  // 23
        mParameterMetanames.push_back("membrane_delayed_rectifier_potassium_current_conductance");  // 0.282
        mParameterMetanames.push_back("membrane_inward_rectifier_potassium_current_conductance");   // 0.6047
        mParameterMetanames.push_back("membrane_L_type_calcium_current_conductance");               // 0.09
        mParameterMetanames.push_back("membrane_leakage_current_conductance");                      // 0.03921
        mParameterMetanames.push_back("membrane_plateau_potassium_current_conductance");            // 0.0183
    }
    else if ( model_number == 4u )
    {
        mpModel.reset(new Cellten_tusscher_model_2004_epiFromCellMLCvode(p_solver, mpStimulus));
        mParameterMetanames.push_back("membrane_fast_sodium_current_conductance");                        // 14.838
        mParameterMetanames.push_back("membrane_L_type_calcium_current_conductance");                     // 0.000175
        mParameterMetanames.push_back("membrane_inward_rectifier_potassium_current_conductance");         // 5.405
        mParameterMetanames.push_back("membrane_rapid_delayed_rectifier_potassium_current_conductance");  // 0.096
        mParameterMetanames.push_back("membrane_slow_delayed_rectifier_potassium_current_conductance");   // 0.245
        mParameterMetanames.push_back("membrane_sodium_calcium_exchanger_current_conductance");           // 1000
        mParameterMetanames.push_back("membrane_transient_outward_current_conductance");                  // 0.294
        mParameterMetanames.push_back("membrane_background_calcium_current_conductance");                 // 0.000592
        mParameterMetanames.push_back("membrane_background_sodium_current_conductance");                  // 0.00029
        mParameterMetanames.push_back("membrane_calcium_pump_current_conductance");                       // 0.825
        mParameterMetanames.push_back("membrane_potassium_pump_current_conductance");                     // 0.0146
        mParameterMetanames.push_back("membrane_sodium_potassium_pump_current_permeability");             // 1.362
    }
    else if ( model_number == 5u )
    {
        mpModel.reset(new Cellohara_rudy_2011_endoFromCellMLCvodeDataClamp(p_solver, mpStimulus));
        mParameterMetanames.push_back("membrane_fast_sodium_current_conductance");                        // 75
        mParameterMetanames.push_back("membrane_L_type_calcium_current_conductance");                     // 0.0001
        mParameterMetanames.push_back("membrane_background_potassium_current_conductance");               // 0.003
        mParameterMetanames.push_back("membrane_inward_rectifier_potassium_current_conductance");         // 0.1908
        mParameterMetanames.push_back("membrane_rapid_delayed_rectifier_potassium_current_conductance");  // 0.046
        mParameterMetanames.push_back("membrane_slow_delayed_rectifier_potassium_current_conductance");   // 0.0034
        mParameterMetanames.push_back("membrane_sodium_calcium_exchanger_current_conductance");           // 0.0008
        mParameterMetanames.push_back("membrane_sodium_potassium_pump_current_permeability");             // 30
        mParameterMetanames.push_back("membrane_transient_outward_current_conductance");                  // 0.02
        mParameterMetanames.push_back("membrane_background_calcium_current_conductance");                 // 2.5e-8
        mParameterMetanames.push_back("membrane_background_sodium_current_conductance");                  // 3.75e-10
        mParameterMetanames.push_back("membrane_calcium_pump_current_conductance");                       // 0.0005
        mParameterMetanames.push_back("membrane_persistent_sodium_current_conductance");                  // 0.0075
    }
    else if ( model_number == 6u ) // Davies 2012 (with data clamp)
    {
        mpModel.reset(new Celldavies_isap_2012FromCellMLCvodeDataClamp(p_solver, mpStimulus));
        //mpModel.reset(new Celldavies_isap_2012FromCellMLCvode(p_solver, mpStimulus));
        mParameterMetanames.push_back("membrane_fast_sodium_current_conductance");                        // 8.25
        mParameterMetanames.push_back("membrane_L_type_calcium_current_conductance");                     // 0.000243
        mParameterMetanames.push_back("membrane_inward_rectifier_potassium_current_conductance");         // 0.5
        mParameterMetanames.push_back("membrane_potassium_pump_current_conductance");                     // 0.00276
        mParameterMetanames.push_back("membrane_slow_delayed_rectifier_potassium_current_conductance");   // 0.00746925
        mParameterMetanames.push_back("membrane_rapid_delayed_rectifier_potassium_current_conductance");  // 0.0138542
        mParameterMetanames.push_back("membrane_calcium_pump_current_conductance");                       // 0.0575
        mParameterMetanames.push_back("membrane_background_calcium_current_conductance");                 // 0.0000007980336
        mParameterMetanames.push_back("membrane_sodium_calcium_exchanger_current_conductance");           // 5.85
        mParameterMetanames.push_back("membrane_sodium_potassium_pump_current_permeability");             // 0.61875
        mParameterMetanames.push_back("membrane_transient_outward_current_conductance");                  // 0.1805
        mParameterMetanames.push_back("membrane_transient_outward_chloride_current_conductance");         // 4e-7
        mParameterMetanames.push_back("membrane_background_chloride_current_conductance");                // 0.000225
        mParameterMetanames.push_back("membrane_persistent_sodium_current_conductance");                  // 0.011
    }
    else if ( model_number == 7u ) // Paci ventricular
    {
        mpModel.reset(new Cellpaci_hyttinen_aaltosetala_severi_ventricularVersionFromCellMLCvodeDataClamp(p_solver, mpStimulus));
        mParameterMetanames.push_back("membrane_fast_sodium_current_conductance");                                            // 3671.2302
        mParameterMetanames.push_back("membrane_L_type_calcium_current_conductance");                                         // 8.635702e-5
        mParameterMetanames.push_back("membrane_inward_rectifier_potassium_current_conductance");                             // 28.1492
        mParameterMetanames.push_back("membrane_slow_delayed_rectifier_potassium_current_conductance");                       // 2.041
        mParameterMetanames.push_back("membrane_rapid_delayed_rectifier_potassium_current_conductance");                      // 29.8667
        mParameterMetanames.push_back("membrane_calcium_pump_current_conductance");                                           // 0.4125
        mParameterMetanames.push_back("membrane_sodium_calcium_exchanger_current_conductance");                               // 4900
        mParameterMetanames.push_back("membrane_background_calcium_current_conductance");                                     // 0.69264
        mParameterMetanames.push_back("membrane_sodium_potassium_pump_current_permeability");                                 // 1.841424
        mParameterMetanames.push_back("membrane_transient_outward_current_conductance");                                      // 29.9038
        mParameterMetanames.push_back("membrane_background_sodium_current_conductance");                                      // 0.9
        mParameterMetanames.push_back("membrane_hyperpolarisation_activated_funny_current_potassium_component_conductance");  // 30.10312
    }
    else if ( model_number == 8u ) // Decker dog
    {
        mpModel.reset(new Celldecker_2009FromCellMLCvode(p_solver, mpStimulus));
        mParameterMetanames.push_back("membrane_fast_sodium_current_conductance");  // 9.075
        mParameterMetanames.push_back("membrane_L_type_calcium_current_conductance");  //  0.00015552
        mParameterMetanames.push_back("membrane_inward_rectifier_potassium_current_conductance");  // 0.5
        mParameterMetanames.push_back("membrane_slow_delayed_rectifier_potassium_current_conductance");  // 0.0826
        mParameterMetanames.push_back("membrane_rapid_delayed_rectifier_potassium_current_conductance");  // 0.0138542
        mParameterMetanames.push_back("membrane_transient_outward_current_conductance");  // 0.497458
    }
    else if ( model_number == 9u ) // Gokhale 2017
    {
        mpModel.reset(new Cellgokhale_ex293_2017FromCellMLCvode(p_solver, mpStimulus));
        mParameterMetanames.push_back("membrane_fast_sodium_current_conductance");  // 9.075
        mParameterMetanames.push_back("membrane_L_type_calcium_current_conductance");  //  0.00015552
        mParameterMetanames.push_back("membrane_inward_rectifier_potassium_current_conductance");  // 0.5
        mParameterMetanames.push_back("membrane_slow_delayed_rectifier_potassium_current_conductance");  // 0.0826
        mParameterMetanames.push_back("membrane_rapid_delayed_rectifier_potassium_current_conductance");  // 0.0138542
        mParameterMetanames.push_back("membrane_transient_outward_current_conductance");  // 0.497458
    }
    mpModel->SetMaxSteps(10000);
}

std::vector<std::string> APSimulator::GetParameterMetanames()
{
    return mParameterMetanames;
}

void APSimulator::SetToModelInitialConditions()
{
    mpModel->SetStateVariables(mpModel->GetInitialConditions());
}

std::vector<double> APSimulator::SolveForVoltageTraceWithParamsNoDataClamp(const std::vector<double>& rParams)
{
    //std::cerr << "About to try and solve" << std::endl << std::flush;
    //mpModel->SetStateVariables(mpModel->GetInitialConditions());
    std::vector<double> voltage_trace;
    for (unsigned j=0; j<rParams.size(); j++)
    {
        mpModel->SetParameter(mParameterMetanames[j], rParams[j]);
    }
    //std::cerr << "Just set(ted) parameters" << std::endl << std::flush;
    try
    {
        if (mHowManySolves > 1)
        {
            for (unsigned i=0; i<mHowManySolves-1; i++)
            {
                mpModel->Compute(mSolveStart, mSolveStart+mStimPeriod, mSolveTimestep);
            }
        }
        //std::cerr << "About to actually try and solve" << std::endl << std::flush;
        OdeSolution sol1 = mpModel->Compute(mSolveStart, mSolveEnd, mSolveTimestep);
        //std::cerr << "Just solved" << std::endl << std::flush;
        voltage_trace = sol1.GetAnyVariable("membrane_voltage");
    }
    catch (Exception &e)
    {
        std::cerr << "WARNING: CVODE failed to solve with these parameters" << std::endl << std::flush;
        std::cerr << "error was " << e.GetShortMessage() << std::endl << std::flush;
        voltage_trace = std::vector<double>(mNumTimePts, 0.0);
        mNumberOfFailedSolves++;
    }
    return voltage_trace;
}

std::vector<double> APSimulator::SolveForVoltageTraceWithParamsWithDataClamp(const std::vector<double>& rParams)
{
    mpModel->SetStateVariables(mpModel->GetInitialConditions());
    std::vector<double> voltage_trace;

    for (unsigned j=0; j<rParams.size(); j++)
    {
        mpModel->SetParameter(mParameterMetanames[j], rParams[j]);
    }
    mpModel->SetStateVariable("membrane_voltage",mExptTrace[0]);

    boost::static_pointer_cast<AbstractCvodeCellWithDataClamp>(mpModel)->TurnOffDataClamp();

    //mpModel->ResetSolver();

    if (mHowManySolves > 1)
    {
        for (unsigned i=0; i<mHowManySolves-1; i++)
        {
            mpModel->Compute(mSolveStart, mDataClampOn, mSolveTimestep);
            mpModel->ResetSolver();
            boost::static_pointer_cast<AbstractCvodeCellWithDataClamp>(mpModel)->TurnOnDataClamp(200);
            mpModel->Compute(mDataClampOn, mDataClampOff, mSolveTimestep);
            mpModel->ResetSolver();
            boost::static_pointer_cast<AbstractCvodeCellWithDataClamp>(mpModel)->TurnOffDataClamp();
            mpModel->Compute(mDataClampOff, mSolveEnd, mSolveTimestep);
        }
    }

    OdeSolution sol1 = mpModel->Compute(mSolveStart, mDataClampOn, mSolveTimestep);

    mpModel->ResetSolver();
    boost::static_pointer_cast<AbstractCvodeCellWithDataClamp>(mpModel)->TurnOnDataClamp(200);
    OdeSolution sol2 = mpModel->Compute(mDataClampOn, mDataClampOff, mSolveTimestep);
    mpModel->ResetSolver();
    boost::static_pointer_cast<AbstractCvodeCellWithDataClamp>(mpModel)->TurnOffDataClamp();
    OdeSolution sol3 = mpModel->Compute(mDataClampOff, mSolveEnd, mSolveTimestep);

    voltage_trace = sol1.GetAnyVariable("membrane_voltage");
    std::vector<double> voltage_trace_part_2 = sol2.GetAnyVariable("membrane_voltage");
    std::vector<double> voltage_trace_part_3 = sol3.GetAnyVariable("membrane_voltage");

    voltage_trace.erase(voltage_trace.end()-1);
    voltage_trace.insert( voltage_trace.end(), voltage_trace_part_2.begin(), voltage_trace_part_2.end() );
    voltage_trace.erase(voltage_trace.end()-1);
    voltage_trace.insert( voltage_trace.end(), voltage_trace_part_3.begin(), voltage_trace_part_3.end() );
    return voltage_trace;
}

std::vector<double> APSimulator::SolveForVoltageTraceWithParams(const std::vector<double>& rParams)
{
    std::vector<double> voltage_trace;
    if (mUseDataClamp == false)
    {
        voltage_trace = SolveForVoltageTraceWithParamsNoDataClamp(rParams);
    }
    else
    {
        voltage_trace = SolveForVoltageTraceWithParamsWithDataClamp(rParams);
    }
    return voltage_trace;
}

void APSimulator::SetTolerances(double rel_tol, double abs_tol)
{
    mpModel->SetTolerances(rel_tol, abs_tol);
}

double APSimulator::ExampleLogLikelihoodFunction(const std::vector<double>& test_trace)
{
    double total = 0;
    double temp;
    for (unsigned i=0; i<test_trace.size(); i++)
    {
        temp = test_trace[i];
        //std::cerr << temp << " ";
        total += temp*temp;
    }
    //std::cerr << "APSimulator::ExampleLogLikelihoodFunction: " << total << std::endl << std::flush;
    return total;
}

std::vector<double> APSimulator::GenerateSyntheticExptTrace(const std::vector<double>& rParams, double noise_sd, double c_seed)
{
    std::vector<double> expt_trace = SolveForVoltageTraceWithParams(rParams);
    RandomNumberGenerator::Instance()->Reseed(c_seed);
    double random_number;
    for (unsigned i=0; i<expt_trace.size(); i++)
    {
        random_number = RandomNumberGenerator::Instance()->StandardNormalRandomDeviate();
        expt_trace[i] +=  noise_sd*random_number;
    }
    mExptTrace = expt_trace;
    return expt_trace;
}

void APSimulator::UseDataClamp(double data_clamp_on, double data_clamp_off)
{
    mUseDataClamp = true;
    mDataClampOn = data_clamp_on;
    mDataClampOff = data_clamp_off;
}

void APSimulator::SetExperimentalTraceAndTimesForDataClamp(const std::vector<double>& expt_times, const std::vector<double>& expt_trace)
{
    boost::static_pointer_cast<AbstractCvodeCellWithDataClamp>(mpModel)->SetExperimentalData(expt_times,expt_trace);
    mExptTrace = expt_trace;
}

void APSimulator::SetExtracellularPotassiumConc( double extra_K_conc )
{
    try
    {
        mpModel->SetParameter("extracellular_potassium_concentration", extra_K_conc);
    }
    catch (Exception &e)
    {
        std::cerr << "No extracellular_potassium_concentration parameter in model" << std::endl << std::flush;
        std::cerr << "error was " << e.GetShortMessage() << std::endl << std::flush;
    }
}

void APSimulator::SetNumberOfSolves( unsigned num_solves )
{
    mHowManySolves = num_solves;
}

bool APSimulator::RunToSteadyState()
{
    SteadyStateRunner steady_runner(mpModel);
    unsigned max_paces = 50000u;
    steady_runner.SetMaxNumPaces(max_paces);
    bool result = steady_runner.RunToSteadyState();
    mHaveRunToSteadyState = result;
    return result;
}

void APSimulator::ArchiveStateVariables()
{
    boost::filesystem::path arch_dir;
    if (mHaveRunToSteadyState)
    {
        arch_dir = "projects/RossJ/archived_variables/steady_state/";
    }
    else
    {
        arch_dir = "projects/RossJ/archived_variables/non_steady_state/";
    }
    boost::filesystem::create_directories(arch_dir);
    //OutputFileHandler handler(arch_dir.string(), false);
    //handler.SetArchiveDirectory();
    //std::string arch_name = handler.GetOutputDirectoryFullPath() + "m_"+boost::lexical_cast<std::string>(mModelNumber)+".arch";
    std::ofstream ofs((arch_dir.string() + "m_"+boost::lexical_cast<std::string>(mModelNumber)+".arch").c_str());
    boost::archive::text_oarchive output_arch(ofs);
    output_arch <<  *mpModel;
}

void APSimulator::LoadStateVariables()
{
    std::string arch_file = "~/workspace/RossJ/archived_variables/steady_state/m_"+boost::lexical_cast<std::string>(mModelNumber)+".arch";
    std::cout << arch_file << std::endl << std::flush;
    std::ifstream ifs(arch_file.c_str(), std::ios::binary);
    boost::archive::text_iarchive input_arch(ifs);
    input_arch >> *mpModel;
}


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

#include "EQPlugin.h"
#include "FilterPlugin.h"

// Just for testing
#define private public

BOOST_AUTO_TEST_SUITE(EQPluginTests)

static nlohmann::json eq_json = {
    {"id", "eq-1"},
    {"kind", nfgrapher::contract::Eq3bandNodeInfo ::kind()},
    {"config", {}},
    {"params",
     {{"highGain",
       {{{"name", "setValueAtTime"},
         {"args", {{"value", 24}, {"startTime", 0}}}},
        {{"name", "linearRampToValueAtTime"},
         {"args", {{"value", 0}, {"endTime", 15E9}}}}}}}}};

/*
BOOST_AUTO_TEST_CASE(testLocalRenderTimeIsZero) {
  nfgrapher::contract::Eq3bandNodeInfo  eq_node((nfgrapher::Node)eq_json);
  nativeformat::plugin::eq::EQPlugin plugin(eq_node, 2, 44100.0, nullptr);
  BOOST_CHECK_EQUAL(plugin.localRenderSampleIndex(0), 0);
}
*/

BOOST_AUTO_TEST_CASE(testFilterSpecs) {
  nfgrapher::Node n = eq_json;
  nfgrapher::contract::Eq3bandNodeInfo eq_node((nfgrapher::Node)eq_json);
  nativeformat::plugin::eq::EQPlugin plugin(eq_node, 2, 44100.0, nullptr);
  size_t expected_filters = 2;
  std::vector<float> expected_freqs = {264.0f, 1000.0f, 3300.0f};
  std::vector<float> expected_gains = {0.0f, 0.0f, 24.0f};
  std::vector<float> expected_coefs = {
      8.60111f, -15.9084f, 7.34337f,  -0.986869f, 0.0229137f,
      1.0f,     -1.93182f, 0.951599f, -1.93182f,  0.951599f};

  size_t filters = plugin.get_filter_count();
  std::vector<float> freqs = plugin.get_freqs(0);
  std::vector<float> gains = plugin.get_gains(0);

  std::vector<float> coefs;
  for (int i = 0; i < filters; ++i) {
    std::vector<float> fc = plugin.get_coefs(i);
    coefs.insert(coefs.end(), fc.begin(), fc.end());
  }

  BOOST_TEST(filters == expected_filters);
  BOOST_TEST(freqs == expected_freqs);
  BOOST_TEST(gains == expected_gains);
  for (int i = 0; i < expected_coefs.size(); ++i) {
    BOOST_CHECK_CLOSE(coefs[i], expected_coefs[i], 0.01);
  }
}

BOOST_AUTO_TEST_CASE(testBandpassFilter) {
  nlohmann::json filter_json = {
      {"id", "filter-1"},
      {"kind", nfgrapher::contract::FilterNodeInfo::kind()},
      {"config", {}},
      {"params",
       {{"highCutoff",
         {{{"name", "setValueAtTime"},
           {"args", {{"value", 0.2}, {"startTime", 0}}}},
          {{"name", "linearRampToValueAtTime"},
           {"args", {{"value", 0.4}, {"endTime", 15E9}}}}}}}}};
  nfgrapher::Node n = filter_json;
  nfgrapher::contract::FilterNodeInfo fni((nfgrapher::Node)filter_json);
  nativeformat::plugin::eq::FilterPlugin plugin(fni, 2, 44100.0, nullptr);
}

BOOST_AUTO_TEST_SUITE_END()