#include <fstream>
#include <boost/program_options.hpp>
#include "picpac-cv.h"

/**
 * This program extract positive regions from the input db
 * and dump the vec file for opencv cascade training, assuming
 * - Each training image has exactly one positive region.
 * The program first computes the average aspect ratio, and
 * the set the width and height for positive region using the
 * aspect ratio and the given "min" side width.
 * The resulting width and height printed should be used for
 * invoking opencv_traincascade.
 */

using namespace std;
using namespace picpac;

cv::Rect bound (cv::Mat image) {
    int minx = image.cols, maxx = 0;
    int miny = image.rows, maxy = 0;
    for (int y = 0; y < image.rows; ++y) {
        uint8_t const *line = image.ptr<uint8_t const>(y);
        for (int x = 0; x < image.cols; ++x) {
            if (line[x] > 0) {
                if (x < minx) minx = x;
                if (x > maxx) maxx = x;
                if (y < miny) miny = y;
                if (y > maxy) maxy = y;
            }
        }
    }
    CHECK(minx < maxx);
    CHECK(miny < maxy);
    return cv::Rect(minx, miny, maxx-minx+1, maxy-miny+1);
}

int main(int argc, char const* argv[]) {
    ImageStream::Config config;
    config.loop = false;
    config.stratify = false;
    config.anno_type = CV_8U;
    config.anno_color1 = 1;
    config.anno_color2 = 0;
    config.anno_color3 = 0;
    config.anno_copy = false;
    config.anno_thickness = -1;

    fs::path db_path;
    fs::path vec_path;
    fs::path dir_path;
    int min;

    namespace po = boost::program_options;
    po::options_description desc("Allowed options");
    desc.add_options()
        ("help,h", "produce help message.")
        ("annotate,a", po::value(&config.annotate)->default_value("json"), "")
        ("db", po::value(&db_path), "")
        ("vec", po::value(&vec_path), "")
        ("dir", po::value(&dir_path), "")
        ("min", po::value(&min)->default_value(32), "")
        ;

    po::positional_options_description p;
    p.add("db", 1);
    p.add("vec", 1);

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

    if (vm.count("help") || db_path.empty() || vec_path.empty()) {
        cout << "Usage:" << endl;
        cout << "\tpicpac-stat ... <db>" << endl;
        cout << desc;
        cout << endl;
        return 0;
    }
    ImageStream db(db_path, config);
    // detect aspect ratio
    float ratio_sum = 0;
    int ratio_total = 0;
    for (;;) {
        try {
            ImageStream::Value v(db.next());
            cv::Rect bb = bound(v.annotation);
            ratio_sum += bb.width / bb.height;
            ratio_total += 1;

        }
        catch (EoS const &) {
            break;
        }
    }
    float ratio = ratio_sum / ratio_total;
    int width, height;
    if (ratio < 1) {
        width = min;
        height = min / ratio;
    }
    else {
        height = min;
        width = min * ratio;
    }
    cout << "width: " << width << endl;
    cout << "height: " << height << endl;

    db.reset();
    int c = 0;
    ofstream os(vec_path.c_str(), ios::binary);

    int vecsize = width * height;
    short tmp = 0;
    os.write(reinterpret_cast<char const *>(&ratio_total), sizeof(ratio_total));
    os.write(reinterpret_cast<char const *>(&vecsize), sizeof(vecsize));
    os.write(reinterpret_cast<char const *>(&tmp), sizeof(tmp));
    os.write(reinterpret_cast<char const *>(&tmp), sizeof(tmp));

    if (!dir_path.empty()) {
        fs::create_directories(dir_path);
    }
    for (;;) {
        try {
            ImageStream::Value v(db.next());
            if (v.image.channels() == 3) {
                cv::Mat tmp;
                cv::cvtColor(v.image, tmp, CV_BGR2GRAY);
                v.image = tmp;
            }
            cv::Rect bb = bound(v.annotation);
            CHECK(v.image.type() == CV_8U);
            cv::Mat roi = v.image(bb);
            cv::Mat sample;
            cv::resize(roi, sample, cv::Size(width, height));
            CHECK(sample.total() == width * height);
            short tmp = 0;
            os.write(reinterpret_cast<char const *>(&tmp), sizeof(tmp));
            for (int y = 0; y < sample.rows; ++y) {
                uint8_t const *line = sample.ptr<uint8_t const>(y);
                for (int x = 0; x < sample.cols; ++x) {
                    tmp = line[x];
                    os.write(reinterpret_cast<char const *>(&tmp), sizeof(tmp));

                }
            }
            if (!dir_path.empty()) {
                fs::path o = dir_path / (lexical_cast<string>(c) + ".jpg");
                cv::imwrite(o.native(), sample);
            }
            ++c;
        }
        catch (EoS const &) {
            break;
        }
    }
    CHECK(c == ratio_total);
    os.seekp(0);
    os.write(reinterpret_cast<char const *>(&c), sizeof(c));
    cout << c << " samples loaded." << endl;

    return 0;
}


#define BOOST_TEST_MODULE reasoner_pgm_test

#include <vector>
#include <iostream>

#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/bind.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/scoped_ptr.hpp>
#include <boost/make_shared.hpp>

#include <lemon/color.h>
#include <lemon/graph_to_eps.h>

#include <lemon/core.h>
#include <lemon/concepts/digraph.h>
#include <lemon/list_graph.h>
#include <lemon/maps.h>

#include "pgmlink/graph.h"
#include "pgmlink/hypotheses.h"
#include "pgmlink/feature.h"
#include "pgmlink/reasoner_pgm.h"
#include "pgmlink/traxels.h"

using namespace pgmlink;
using namespace std;
using namespace boost;

BOOST_AUTO_TEST_CASE( HypothesesGraph_build_hyp ) {
    HypothesesGraph graph;
    graph.add_node(13);
}

BOOST_AUTO_TEST_CASE( HypothesesGraph_build_hyp2 ) {

  std::cout << "Constructing HypothesesGraph" << std::endl;
  std::cout <<  std::endl;

  typedef HypothesesGraph::ArcIt ArcIt2;
  typedef HypothesesGraph::Arc Arc;
  typedef HypothesesGraph::NodeIt NodeIt;
  typedef HypothesesGraph::Node Node;
  using lemon::INVALID;

  HypothesesGraph g;
  HypothesesGraph* graph=&g;
  graph->add(node_traxel());

  std::cout << "Adding nodes and arcs" << std::endl;
  std::cout <<  std::endl;

  Node n1=g.add_node(0);
  Node n2=g.add_node(0);
  Node n3=g.add_node(0);
  Node n4=g.add_node(0);
  Node m1=g.add_node(1);
  Node m2=g.add_node(1);
  Node m3=g.add_node(1);
  Node m4=g.add_node(1);
  Node o1=g.add_node(2);
  Node o2=g.add_node(2);
  Node o3=g.add_node(2);
  Node o4=g.add_node(2);

  // we have to shuffle the order of adding arcs
  // that way we can test if the index sorting in formulate()
  // is working

  g.addArc(m1,o1);
  g.addArc(n2,m1);
  g.addArc(m4,o4);
  g.addArc(m4,o3);
  g.addArc(n3,m4);
  g.addArc(n1,m2);
  g.addArc(n3,m3);
  g.addArc(m2,o2);
  g.addArc(n2,m2);

  std::cout << "Nodes:";
  for (NodeIt i(g); i!=INVALID; ++i)
    std::cout << " " << g.id(i);
  std::cout << std::endl;

  std::cout << "Arcs:";
  for (ArcIt2 i(g); i!=INVALID; ++i)
  std::cout << " (" << g.id(g.source(i)) << "," << g.id(g.target(i)) << ")";
  std::cout << std::endl;
  std::cout <<  std::endl;

  std::cout << "Constructing Reasoner" << std::endl;
  std::cout <<  std::endl;

   Traxels empty;
   ConstantFeature e1(1);
   ConstantFeature e2(2);
   ConstantFeature e3(3);
   ConstantFeature e4(4);
   ConstantFeature e5(5);
   ConstantFeature e6(6);

   pgm::chaingraph::ECCV12ModelBuilder b;
   b.with_detection_vars(e1, e2)
     .appearance(e3)
     .disappearance(e4)
     .move(e5)
     .with_divisions(e6)
     .opportunity_cost(7)
     .forbidden_cost(8)
     ;

   Chaingraph mrf(b, true, 0.01, false);

  std::cout << "Formulating Factors" << std::endl;
  std::cout <<  std::endl;

  cout << "-> workflow: formulating model" << endl;
  mrf.formulate( *graph );

  ////
  //// check topology of graphical model
  ////
  const pgm::OpengmModel* model = mrf.get_graphical_model();
  BOOST_CHECK( model != NULL );
  BOOST_CHECK_EQUAL( model->numberOfVariables(), 21);
  BOOST_CHECK_EQUAL( model->numberOfFactors(), 36);

  // incoming factor
  size_t det_var; // detection variable
  det_var = mrf.get_node_map().find(m2)->second;
  BOOST_CHECK_EQUAL(model->numberOfFactors(det_var), 3);
  const pgm::OpengmModel::FactorType f = (*model)[model->factorOfVariable(det_var, 2)];
  BOOST_CHECK_EQUAL(f.numberOfVariables(), 3);
  BOOST_CHECK_EQUAL(f.size(), 8);
  BOOST_CHECK_EQUAL(f.shape(0), 2);
  BOOST_CHECK_EQUAL(f.shape(1), 2);
  BOOST_CHECK_EQUAL(f.shape(2), 2);
  BOOST_CHECK_EQUAL(f.function<0>()(0,0,0), 0);
  BOOST_CHECK_EQUAL(f.function<0>()(0,0,1), 8);
  BOOST_CHECK_EQUAL(f.function<0>()(0,1,0), 8);
  BOOST_CHECK_EQUAL(f.function<0>()(0,1,1), 8);
  BOOST_CHECK_EQUAL(f.function<0>()(1,0,0), 3);
  BOOST_CHECK_EQUAL(f.function<0>()(1,0,1), 0);
  BOOST_CHECK_EQUAL(f.function<0>()(1,1,0), 0);
  BOOST_CHECK_EQUAL(f.function<0>()(1,1,1), 8);

  // outgoing factor
  det_var = mrf.get_node_map().find(m4)->second;
  BOOST_CHECK_EQUAL(model->numberOfFactors(det_var), 3);
  const pgm::OpengmModel::FactorType f2 = (*model)[model->factorOfVariable(det_var, 1)];
  BOOST_CHECK_EQUAL(f2.numberOfVariables(), 3);
  BOOST_CHECK_EQUAL(f2.size(), 8);
  BOOST_CHECK_EQUAL(f2.shape(0), 2);
  BOOST_CHECK_EQUAL(f2.shape(1), 2);
  BOOST_CHECK_EQUAL(f2.shape(2), 2);
  BOOST_CHECK_EQUAL(f2.function<0>()(0,0,0), 7);
  BOOST_CHECK_EQUAL(f2.function<0>()(0,0,1), 8);
  BOOST_CHECK_EQUAL(f2.function<0>()(0,1,0), 8);
  BOOST_CHECK_EQUAL(f2.function<0>()(0,1,1), 8);
  BOOST_CHECK_EQUAL(f2.function<0>()(1,0,0), 4);
  BOOST_CHECK_EQUAL(f2.function<0>()(1,0,1), 5);
  BOOST_CHECK_EQUAL(f2.function<0>()(1,1,0), 5);
  BOOST_CHECK_EQUAL(f2.function<0>()(1,1,1), 6);

  // detection factor
  det_var = mrf.get_node_map().find(n4)->second;
  BOOST_CHECK_EQUAL(model->numberOfFactors(det_var), 3);
  const pgm::OpengmModel::FactorType f3 = (*model)[model->factorOfVariable(det_var, 0)];
  BOOST_CHECK_EQUAL(f3.numberOfVariables(), 1);
  BOOST_CHECK_EQUAL(f3.size(), 2);
  BOOST_CHECK_EQUAL(f3.shape(0), 2);
  BOOST_CHECK_EQUAL(f3.function<0>()(0), 2);
  BOOST_CHECK_EQUAL(f3.function<0>()(1), 1);

  cout << "-> workflow: infer" << endl;
  mrf.infer();
  cout << "-> workflow: conclude" << endl;
  mrf.conclude(*graph);
  prune_inactive(*graph);
}

BOOST_AUTO_TEST_CASE( HypothesesGraph_build_hyp_mrf ) {

    Traxel n1, n2, n3, n4, m1, m2, m3, m4, o1, o2, o3, o4, o5;
    feature_array comn1(feature_array::difference_type(3)); //float vector
    feature_array comn2(feature_array::difference_type(3));
    feature_array comn3(feature_array::difference_type(3));
    feature_array comn4(feature_array::difference_type(3));
    feature_array comm1(feature_array::difference_type(3));
    feature_array comm2(feature_array::difference_type(3));
    feature_array comm3(feature_array::difference_type(3));
    feature_array comm4(feature_array::difference_type(3));
    feature_array como1(feature_array::difference_type(3));
    feature_array como2(feature_array::difference_type(3));
    feature_array como3(feature_array::difference_type(3));
    feature_array como4(feature_array::difference_type(3));
    feature_array como5(feature_array::difference_type(3));

    comn1[0] = 0;
    comn1[1] = 0;
    comn1[2] = 0;
    n1.features["com"] = comn1;
    n1.Id = 11;
    n1.Timestep = 0;

    comn2[0] = 1;
    comn2[1] = 1;
    comn2[2] = 1;
    n2.features["com"] = comn2;
    n2.Id = 12;
    n2.Timestep = 0;

    comn3[0] = 0;
    comn3[1] = 0;
    comn3[2] = 2;
    n3.features["com"] = comn3;
    n3.Id = 13;
    n3.Timestep = 0;

    comn4[0] = 2;
    comn4[1] = 0;
    comn4[2] = 0;
    n4.features["com"] = comn4;
    n4.Id = 14;
    n4.Timestep = 0;

    comm1[0] = 2;
    comm1[1] = 2;
    comm1[2] = 2;
    m1.features["com"] = comm1;
    m1.Id = 21;
    m1.Timestep = 1;

    comm2[0] = 1;
    comm2[1] = 0;
    comm2[2] = 1;
    m2.features["com"] = comm2;
    m2.Id = 22;
    m2.Timestep = 1;

    comm3[0] = 0;
    comm3[1] = 1;
    comm3[2] = 1;
    m3.features["com"] = comm3;
    m3.Id = 23;
    m3.Timestep = 1;

    comm4[0] = 3;
    comm4[1] = 3;
    comm4[2] = 3;
    m4.features["com"] = comm4;
    m4.Id = 24;
    m4.Timestep = 1;

    como1[0] = 4;
    como1[1] = 4;
    como1[2] = 4;
    o1.features["com"] = como1;
    o1.Id = 31;
    o1.Timestep = 2;

    como2[0] = 7;
    como2[1] = 19;
    como2[2] = 8;
    o2.features["com"] = como2;
    o2.Id = 32;
    o2.Timestep = 2;

    como3[0] = 0;
    como3[1] = 7;
    como3[2] = 1;
    o3.features["com"] = como3;
    o3.Id = 33;
    o3.Timestep = 2;

    como4[0] = 5;
    como4[1] = 0;
    como4[2] = 9;
    o4.features["com"] = como4;
    o4.Id = 34;
    o4.Timestep = 2;

    como5[0] = 0;
    como5[1] = 4;
    como5[2] = 0;
    o5.features["com"] = como5;
    o5.Id = 34;
    o5.Timestep = 2;

    TraxelStore ts;
    add(ts,n1);
    add(ts,n2);
    add(ts,n3);
    add(ts,n4);
    add(ts,m1);
    add(ts,m2);
    add(ts,m3);
    add(ts,m4);
    add(ts,o1);
    add(ts,o2);
    add(ts,o3);
    add(ts,o4);
    add(ts,o5);

    SingleTimestepTraxel_HypothesesBuilder builder(&ts);
    boost::shared_ptr<HypothesesGraph> graph = boost::shared_ptr<HypothesesGraph>(builder.build());

    Traxels empty;
    ConstantFeature e1(10);
    ConstantFeature e2(90);
    ConstantFeature e3(70);
    ConstantFeature e4(50);
    ConstantFeature e5(20);
    ConstantFeature e6(5);

    pgm::chaingraph::ECCV12ModelBuilder b1;
    b1.with_detection_vars(e1, e2)
      .appearance(e3)
      .disappearance(e4)
      .move(e5)
      .with_divisions(e6)
      ;

    Chaingraph mrf(b1, true, 0.01, false);

    pgm::chaingraph::TrainableModelBuilder b(e3,
					   e4,
					   e5,
					   0,
					   0);

    b.with_divisions(e6)
      .with_detection_vars(e1, e2);

    Chaingraph new_builder(b, true, 0.01, false);

    cout << "-> workflow: formulating model" << endl;
    mrf.formulate( *graph );
    cout << "-> workflow: infer" << endl;
    mrf.infer();

    cout << "-> new builder" << endl;
    new_builder.formulate( *graph );
    new_builder.infer();

    cout << "-> workflow: conclude" << endl;
    mrf.conclude(*graph);
    prune_inactive(*graph);

}


// Copyright 2020 Tier IV, Inc.
//
// 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.
// Copyright 2015-2020 Autoware Foundation. All rights reserved.
//
// 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.

#ifndef LANE_DEPARTURE_CHECKER__UTIL__CREATE_VEHICLE_FOOTPRINT_HPP_
#define LANE_DEPARTURE_CHECKER__UTIL__CREATE_VEHICLE_FOOTPRINT_HPP_

#include <Eigen/Dense>

#include "autoware_utils/geometry/geometry.hpp"
#include "tf2/utils.h"
#include "vehicle_info_util/vehicle_info_util.hpp"

struct FootprintMargin
{
  double lon;
  double lat;
};

inline autoware_utils::LinearRing2d createVehicleFootprint(
  const vehicle_info_util::VehicleInfo & vehicle_info,
  const FootprintMargin & margin = {0.0, 0.0})
{
  using autoware_utils::LinearRing2d;
  using autoware_utils::Point2d;

  const auto & i = vehicle_info;

  const double x_front = i.front_overhang_m + i.wheel_base_m + margin.lon;
  const double x_center = i.wheel_base_m / 2.0;
  const double x_rear = -(i.rear_overhang_m + margin.lon);
  const double y_left = i.wheel_tread_m / 2.0 + i.left_overhang_m + margin.lat;
  const double y_right = -(i.wheel_tread_m / 2.0 + i.right_overhang_m + margin.lat);

  LinearRing2d footprint;
  footprint.push_back(Point2d{x_front, y_left});
  footprint.push_back(Point2d{x_front, y_right});
  footprint.push_back(Point2d{x_center, y_right});
  footprint.push_back(Point2d{x_rear, y_right});
  footprint.push_back(Point2d{x_rear, y_left});
  footprint.push_back(Point2d{x_center, y_left});
  footprint.push_back(Point2d{x_front, y_left});

  return footprint;
}

inline FootprintMargin calcFootprintMargin(
  const geometry_msgs::msg::PoseWithCovariance & covariance, const double scale)
{
  const auto Cov_in_map = covariance.covariance;
  Eigen::Matrix2d Cov_xy_map;
  Cov_xy_map <<
    Cov_in_map[0 * 6 + 0], Cov_in_map[0 * 6 + 1],
    Cov_in_map[1 * 6 + 0], Cov_in_map[1 * 6 + 1];

  const double yaw_vehicle = tf2::getYaw(covariance.pose.orientation);

  // To get a position in a transformed coordinate, rotate the inverse direction
  Eigen::Matrix2d R_map2vehicle;
  R_map2vehicle <<
    std::cos(-yaw_vehicle), -std::sin(-yaw_vehicle),
    std::sin(-yaw_vehicle), std::cos(-yaw_vehicle);
  // Rotate covariance E((X, Y)^t*(X, Y)) = E(R*(x,y)*(x,y)^t*R^t)
  // when Rotate point (X, Y)^t= R*(x, y)^t.
  const Eigen::Matrix2d Cov_xy_vehicle =
    R_map2vehicle * Cov_xy_map * R_map2vehicle.transpose();

  // The longitudinal/lateral length is represented
  // in Cov_xy_vehicle(0,0), Cov_xy_vehicle(1,1) respectively.
  return FootprintMargin{Cov_xy_vehicle(0, 0) * scale, Cov_xy_vehicle(1, 1) * scale};
}

#endif  // LANE_DEPARTURE_CHECKER__UTIL__CREATE_VEHICLE_FOOTPRINT_HPP_


#include <boost/graph/adjacency_list.hpp>
#include <utility>
#include <algorithm>
#include <iterator>
#include <iostream>

int main()
{
  boost::adjacency_list<> g;

  boost::add_vertex(g);
  boost::add_vertex(g);
  boost::add_vertex(g);
  boost::add_vertex(g);

  std::pair<boost::adjacency_list<>::vertex_iterator,
    boost::adjacency_list<>::vertex_iterator> vs = boost::vertices(g);

  std::copy(vs.first, vs.second,
    std::ostream_iterator<boost::adjacency_list<>::vertex_descriptor>{
      std::cout, "\n"});
}


#include "Global.h"
#include "Loggers/None.h"
#include <boost/date_time/gregorian/gregorian_types.hpp>
#include <boost/date_time/date_duration.hpp>
#include <boost/date_time/posix_time/ptime.hpp>

//std::ofstream ofs("output.nl");
//Logger logger(std::ofstream("output.nl"));
float Global::INF = std::numeric_limits<float>::infinity();
float Global::MV  = -999;
float Global::NC  = -998;
float Global::TOL = 0.001;
float Global::pi  = 3.14159265;

boost::variate_generator<boost::mt19937, boost::uniform_01<> > Global::mRand(boost::mt19937(0), boost::uniform_01<>());

Logger* Global::logger = new LoggerNone(Logger::message);

void Global::setLogger(Logger* iLogger) {
   if(logger)
      delete logger;
   logger = iLogger;
}

bool Global::isValid(float iValue) {
   return !std::isnan(iValue) && !std::isinf(iValue) && !Global::isMissing(iValue);
}
bool Global::isMissing(float iValue) {
   return iValue == Global::MV;
}

float Global::getMoment(const std::vector<float>& iValues, int iMoment) {
   float moment = 0;
   // Find mean
   float mean = 0;
   int meanC = 0;
   for(int i = 0; i < (int) iValues.size(); i++) {
      assert(!std::isnan(iValues[i]));
      assert(!std::isinf(iValues[i]));
      if(!isMissing(iValues[i])) {
         mean += iValues[i];
         meanC++;
      }
   }
   if(meanC == 0) {
      return Global::MV;
   }

   mean = mean / meanC;

   if(iMoment == 1) {
      return mean;
   }
   else {
      // Compute central moment
      // TODO
      for(int i = 0; i < (int) iValues.size(); i++) {
         if(iValues[i] != Global::MV) {
            if(iMoment == 2)
               moment +=  (iValues[i] - mean)*(iValues[i] - mean);
            else
               moment += pow((double) iValues[i] - mean, (double) iMoment);
         }
      }
      if(meanC == 0)
         return Global::MV;
      else
         return moment / meanC;
   }
}
float Global::getTime(int iDate, int iInit, float iOffset) {
   float newOffset = iOffset + iInit;
   float time = getOffset(iDate, newOffset);
   assert(time >= 0 && time <= 24);
   return time;
}
int Global::getDate(int iDate, float iOffset) {
   return getDate(iDate, 0, iOffset);
}
int Global::getDayOfWeek(int iDate) {
   int year  = floor(iDate / 10000);
   int month = floor(iDate % 10000)/100;
   int day   = floor(iDate % 100);

   boost::gregorian::date currDate(year, month, day);
   return currDate.day_of_week();
}
int Global::getYear(int iDate) {
   return floor(iDate / 10000);
}
int Global::getMonth(int iDate) {
   return floor(iDate % 10000)/100;
}
int Global::getDay(int iDate) {
   return floor(iDate % 100);
}
int Global::getDate(int iDate, int iInit, float iOffset) {
   int year  = floor(iDate / 10000);
   int month = floor(iDate % 10000)/100;
   int day   = floor(iDate % 100);

   int offDay = floor((iOffset+iInit)/24);
   //std::cout << floor(2.5) << " " << ceil(2.5) << std::endl;
   //std::cout << floor(-2.5) << " " << ceil(-2.5) << std::endl;
   // Negative offsets round up, so subtract a day
   //if(iOffset < 0)
   //   offDay = iOffset - 1;

   boost::gregorian::date currDate(year, month, day);
   boost::gregorian::date_duration diff(offDay);
   boost::gregorian::date newDate = currDate + diff;
   int newYear  = newDate.year();
   int newMonth = newDate.month();
   int newDay   = newDate.day();
   int returnDate = newYear * 10000 + newMonth * 100 + newDay;
   return returnDate;
}

float Global::getOffset(int iDate, float iOffset) {
   float offset;
   if(iOffset < 0) {
      int addDay = -floor(iOffset/24);
      offset = iOffset + 24*addDay;
   }
   else {
      offset = fmod(iOffset, 24);
   }
   assert(offset < 24);
   assert(offset >= 0);
   return offset;
}

float Global::getTimeDiff(int iDate1, int iInit1, float iOffset1, int iDate2, int iInit2, float iOffset2) {
   int year1 = floor(iDate1 / 10000);
   int month1= floor(iDate1 % 10000)/100;
   int day1  = floor(iDate1 % 100);

   int year2 = floor(iDate2 / 10000);
   int month2= floor(iDate2 % 10000)/100;
   int day2  = floor(iDate2 % 100);

   boost::gregorian::date date1(year1, month1, day1);
   boost::gregorian::date date2(year2, month2, day2);
   boost::gregorian::date_period diff(date2, date1);
   long int numDays = diff.length().days();

   return numDays*24 + (iInit1 - iInit2) + (iOffset1 - iOffset2);
}

float Global::getJulianDay(int iDate, int iInit, float iOffset) {
   int year  = floor(iDate / 10000);
   int month = floor(iDate % 10000)/100;
   int day   = floor(iDate % 100);

   boost::gregorian::date currDate(year, month, day);
   return currDate.day_of_year();

   /*
   struct tm tm1;

   tm1.tm_year  = year-1900;
   tm1.tm_mon   = month-1;
   tm1.tm_mday  = day;
   tm1.tm_hour  = 0;
   tm1.tm_min   = 0;
   tm1.tm_sec   = 0;
   tm1.tm_isdst = 0;

   time_t time = mktime(&tm1) + iOffset/60;

   struct tm *tm2 = gmtime(&time);

   return tm2->tm_yday;
   */

}
double Global::clock() {
   timeval t;
   gettimeofday(&t, NULL);
   double sec = (t.tv_sec);
   double msec= (t.tv_usec);
   return sec + msec/1e6;
}

int Global::getLowerIndex(float iX, const std::vector<float>& iValues) {
   int index = Global::MV;
   for(int i = 0; i < (int) iValues.size(); i++) {
      float currValue = iValues[i];
      if(Global::isValid(currValue)) {
         if(currValue < iX) {
            index = i;
         }
         else if(currValue == iX) {
            index = i;
            break;
         }
         else if(currValue > iX) {
            break;
         }
      }
   }
   return index;
}
int Global::getUpperIndex(float iX, const std::vector<float>& iValues) {
   int index = Global::MV;
   for(int i = iValues.size()-1; i >= 0; i--) {
      float currValue = iValues[i];
      if(Global::isValid(currValue)) {
         if(currValue > iX) {
            index = i;
         }
         else if(currValue == iX) {
            index = i;
            break;
         }
         else if(currValue < iX) {
            break;
         }
      }
   }
   return index;
}
int Global::getNumValid(std::vector<float> iValues) {
   int num = 0;
   for(int i = 0; i < iValues.size(); i++) {
      if(Global::isValid(iValues[i])) {
         num++;
      }
   }
   return num;
}

float Global::interp(float x, float x0, float x1, float y0, float y1) {
   if(isValid(x) && isValid(x0) && isValid(x1) && isValid(y0) && isValid(y1)) {
      if(x0 == x1) {
         return (y1 + y0)/2;
      }
      else {
         assert(x >=x0);
         assert(x <=x1);
         assert(x1 != x0);
         return y0 + (y1 - y0)*(x - x0)/(x1 - x0);
      }
   }
   else {
      return Global::MV;
   }
}
float Global::getRand() {
   return mRand();
}

int Global::getCurrentDate() {
    boost::gregorian::date today = boost::gregorian::day_clock::local_day();
    return today.year()*10000 + today.month()*100 + today.day();
}

int Global::getInt(const std::string& iString) {
   std::stringstream ss(iString);
   int value;
   ss >> value;
   return(value);
}
float Global::getFloat(const std::string& iString) {
   std::stringstream ss(iString);
   float value;
   ss >> value;
   return(value);
}

bool Global::createDirectory(std::string iPath) {
   if(!boost::filesystem::exists(iPath)) {
      if(!boost::filesystem::create_directory(iPath)) {
         return false;
      }
   }
   return true;
}

std::string Global::getDirectory(std::string iFilename) {
   int last = iFilename.find_last_of("/");
   if(last != std::string::npos) {
      std::string dir = iFilename.substr(0, last);
      return dir;
   }
   else {
      return "./";
   }
}

int Global::getDate(time_t iUnixTime) {
   boost::gregorian::date epoch(1970,1,1);
   boost::gregorian::date_duration diff(iUnixTime/86400);
   boost::gregorian::date newDate = epoch + diff;

   return newDate.year() * 10000 + newDate.month() * 100 + newDate.day();
}
float Global::getTime(time_t iUnixTime) {
   int date = getDate(iUnixTime);
   return (iUnixTime - getUnixTime(date, 0))/3600;
}
time_t Global::getUnixTime(int iDate, int iOffset) {
   int   date   = getDate(iDate, iOffset);
   float offset = getOffset(iDate, iOffset);
   int year = getYear(date);
   int month = getMonth(date);
   int day = getDay(date);
   // boost::posix_time::ptime unixTime(boost::gregorian::date(year, month, day));
   boost::gregorian::date time(year, month, day);
   boost::gregorian::date epoch(1970,1,1);
   boost::gregorian::date_duration diff = time - epoch;
   time_t days = diff.days();
   time_t unixTime = days*86400 + ((time_t) offset)*3600;
   return unixTime;
}
float Global::logit(float p) {
   if(!Global::isValid(p) || p <= 0 || p >= 1)
      return Global::MV;
   return log(p/(1-p));
}
float Global::invLogit(float x) {
   if(!Global::isValid(x))
      return Global::MV;
   return exp(x)/(exp(x)+1);
}


/*
* Copyright 2020 Virtru Corporation
*
* SPDX - License Identifier: BSD-3-Clause-Clear
*
*/
//
//  TDF SDK
//
//  Created by Sujan Reddy on 2020/04/20.
//

#include "ec_key_pair.h"
#include "crypto_utils.h"
#include "tdf_exception.h"

#include <boost/algorithm/string.hpp>
#include <type_traits>
#include <openssl/rand.h>
#include <openssl/bio.h>
#include <openssl/pem.h>
#include <openssl/ec.h>
#include <openssl/kdf.h>

namespace virtru::crypto {

    /// Constants
    constexpr auto kX509CertTag = "BEGIN CERTIFICATE";

    /// Constructor
    ECKeyPair::ECKeyPair(EVP_PKEY_free_ptr pkey)
        : m_pkey(std::move(pkey)) {}

    /// Creates an instance of EC(Elliptic Curve) Key and will generate a new key pair every time it is called.
    std::unique_ptr<ECKeyPair> ECKeyPair::Generate(const std::string& curveName) {

        auto eccgrp = OBJ_txt2nid(curveName.data());
        if (eccgrp == NID_undef) {
            ThrowOpensslException("Unknown curve name.");
        }

        EC_free_ptr ec{EC_KEY_new_by_curve_name(eccgrp)};

        // Create the public/private EC key pair here
        if (1 != EC_KEY_generate_key(ec.get())) {
            ThrowOpensslException("Failed ECKeyPair generation.");
        }

        if (1 != EC_KEY_check_key(ec.get())) {
            ThrowOpensslException("Failed EC sanity check.");
        }

        EVP_PKEY_free_ptr evppkeyPtr { EVP_PKEY_new()};
        if (1 != EVP_PKEY_assign_EC_KEY(evppkeyPtr.get(), ec.release())) {
            ThrowOpensslException("Error assigning EC key to EVP_PKEY structure.");
        }

        return std::unique_ptr<ECKeyPair>(new ECKeyPair(std::move(evppkeyPtr)));
    }

    /// Return public key in PEM format.
    std::string ECKeyPair::PublicKeyInPEMFormat() const {
        BIO_free_ptr bio{BIO_new(BIO_s_mem())};

        if (1 != PEM_write_bio_PUBKEY(bio.get(), m_pkey.get())) {
            ThrowOpensslException("Error writing EC public key data in PEM format.");
        }

        // Read the public key from the buffer and put it in the string
        std::string publicKeyPem(BIO_pending(bio.get()), '\0');
        auto readResult = BIO_read(bio.get(), publicKeyPem.data(), static_cast<int>(publicKeyPem.size()));
        if (readResult <= 0) {
            ThrowOpensslException("Failed to read public key data.");
        }

        return publicKeyPem;
    }

    /// Return private key in PEM format.
    std::string ECKeyPair::PrivateKeyInPEMFormat() const {
        BIO_free_ptr bio{BIO_new(BIO_s_mem())};

        if (1 != PEM_write_bio_PrivateKey(bio.get(), m_pkey.get(), nullptr,
                nullptr, 0, 0, nullptr)) {
            ThrowOpensslException("Error writing EC private key data in PEM format.");
        }

        // Read the private key from the buffer and put it in the string
        std::string privateKeyPem(BIO_pending(bio.get()), '\0');
        auto readResult = BIO_read(bio.get(), privateKeyPem.data(), static_cast<int>(privateKeyPem.size()));
        if (readResult <= 0) {
            ThrowOpensslException("Failed to read private key data.");
        }

        return privateKeyPem;
    }

    /// Return the key size in bits.
    unsigned int ECKeyPair::KeySize() const {
        return EVP_PKEY_bits(m_pkey.get());
    }

    /// Return the curve name.
    std::string ECKeyPair::CurveName() const {
        auto ec = EVP_PKEY_get0_EC_KEY(m_pkey.get());
        auto ecGroup =  EC_KEY_get0_group(ec);

        auto curveName = OBJ_nid2sn(EC_GROUP_get_curve_name(ecGroup));
        if (curveName == nullptr) {
            ThrowOpensslException("Failed to get the curve name from ec key.");
        }

        return curveName;
    }

    /// Generate a public key given the private key and it's curve.
    std::string ECKeyPair::GetPEMPublicKeyFromPrivateKey(const std::string& privateKeyInPEM,
                                                     const std::string& curveName) {

        // Create a group give the curve name.
        auto eccgrp = OBJ_txt2nid(curveName.data());
        if (eccgrp == NID_undef) {
            ThrowOpensslException("Unknown curve name.");
        }

        EC_free_ptr ec{EC_KEY_new_by_curve_name(eccgrp)};
        auto group = EC_KEY_get0_group(ec.get());

        /// Extract private key as big number from the pem formatted private key
        BIO_free_ptr preBio{BIO_new(BIO_s_mem())};
        if ((size_t)BIO_write(preBio.get(), privateKeyInPEM.data(), privateKeyInPEM.size()) != privateKeyInPEM.size()) {
            ThrowOpensslException("Failed to load private key.");
        }

        EC_free_ptr ecPre {PEM_read_bio_ECPrivateKey(preBio.get(), nullptr, nullptr, nullptr)};
        if (!ecPre) {
            ThrowOpensslException("Failed to read ec private key from pem format");
        }

        if (1 != EC_KEY_check_key(ecPre.get())) {
            ThrowOpensslException("Failed the sanity check for ec private key");
        }

        auto bigNum = EC_KEY_get0_private_key(ecPre.get());
        if (!bigNum) {
            ThrowOpensslException("Failed get a BIGNUM from ec private key.");
        }

        // Set the private key to ec key        ;
        if (1 != EC_KEY_set_private_key(ec.get(), bigNum)) {
            ThrowOpensslException("Failed to set the private key to ec key");
        }

        // Create a ECPoint and generate a public key.
        ECPoint_free_ptr pubKey{EC_POINT_new(group)};
        if (!EC_POINT_mul(group, pubKey.get(), bigNum, nullptr, nullptr, nullptr)) {
            ThrowOpensslException("Failed to generate ec public key from EC_POINT_mul");
        }

        // Set the public key to ec key        ;
        if (1 !=  EC_KEY_set_public_key(ec.get(), pubKey.get())) {
            ThrowOpensslException("Failed to set the public key to ec key");
        }

        EVP_PKEY_free_ptr evppkeyPtr { EVP_PKEY_new()};
        if (1 != EVP_PKEY_assign_EC_KEY(evppkeyPtr.get(), ec.release())) {
            ThrowOpensslException("Error assigning EC key to EVP_PKEY structure.");
        }

        BIO_free_ptr bio{BIO_new(BIO_s_mem())};
        if (1 != PEM_write_bio_PUBKEY(bio.get(), evppkeyPtr.get())) {
            ThrowOpensslException("Error writing EC public key data in PEM format.");
        }

        // Read the public key from the buffer and put it in the string
        std::string publicKeyPem(BIO_pending(bio.get()), '\0');
        auto readResult = BIO_read(bio.get(), publicKeyPem.data(), static_cast<int>(publicKeyPem.size()));
        if (readResult <= 0) {
            ThrowOpensslException("Failed to read public key data.");
        }

        return publicKeyPem;
    }

    std::string ECKeyPair::GetPEMPublicKeyFromX509Cert(const std::string& pemKeyInX509) {

        if (boost::contains(pemKeyInX509, kX509CertTag)) {
            EC_free_ptr ecPub;

            BIO_free_ptr pubBio{BIO_new(BIO_s_mem())};
            if ((size_t)BIO_write(pubBio.get(), pemKeyInX509.data(), pemKeyInX509.size()) != pemKeyInX509.size()) {
                ThrowOpensslException("Failed to load public key.");
            }

            X509_free_ptr x509Ptr{ PEM_read_bio_X509(pubBio.get(), NULL, NULL, NULL) };
            if (!x509Ptr) {
                ThrowOpensslException("Failed to create X509 cert struct.");
            }

            EVP_PKEY_free_ptr evppkeyPtr { X509_get_pubkey(x509Ptr.get()) };
            if (!evppkeyPtr) {
                ThrowOpensslException("Failed to create EVP_PKEY.");
            }

            BIO_free_ptr bio{BIO_new(BIO_s_mem())};

            if (1 != PEM_write_bio_PUBKEY(bio.get(), evppkeyPtr.get())) {
                ThrowOpensslException("Error writing EC public key data in PEM format.");
            }

            // Read the public key from the buffer and put it in the string
            std::string publicKeyPem(BIO_pending(bio.get()), '\0');
            auto readResult = BIO_read(bio.get(), publicKeyPem.data(), static_cast<int>(publicKeyPem.size()));
            if (readResult <= 0) {
                ThrowOpensslException("Failed to read public key data.");
            }

            return publicKeyPem;
        }

        return pemKeyInX509;
    }

    /// Calculate shared secret from public key from one party and the private key from another party.
    std::vector<gsl::byte> ECKeyPair::ComputeECDHKey(const std::string& publicKeyInPEM,
            const std::string& privateKeyInPEM) {

        if (publicKeyInPEM.empty() || privateKeyInPEM.empty()) {
            ThrowException("Invalid data to calculate the share secret.");
        }

        ///
        /// Extract public key
        ///
        EC_free_ptr ecPub = getECPublicKey(publicKeyInPEM);

        ///
        /// Extract private key
        ///
        BIO_free_ptr preBio{BIO_new(BIO_s_mem())};
        if ((size_t)BIO_write(preBio.get(), privateKeyInPEM.data(), privateKeyInPEM.size()) != privateKeyInPEM.size()) {
            ThrowOpensslException("Failed to load private key.");
        }

        EC_free_ptr ecPre {PEM_read_bio_ECPrivateKey(preBio.get(), nullptr, nullptr, nullptr)};
        if (!ecPre) {
            ThrowOpensslException("Failed to ec key from private key");
        }

        if (1 != EC_KEY_check_key(ecPre.get())) {
            ThrowOpensslException("Failed ec key(private) sanity check.");
        }

        /// Calculate shared secret length.
        std::vector<gsl::byte> symmetricKey;
        auto secretLen = EC_GROUP_get_degree(EC_KEY_get0_group(ecPre.get()));
        secretLen = (secretLen + 7) / 8;
        symmetricKey.resize(secretLen);

        auto pubkey  = EC_KEY_get0_public_key(ecPub.get());
        if (ECDH_compute_key(symmetricKey.data(), symmetricKey.size(), pubkey, ecPre.get(), nullptr) == -1) {
            ThrowOpensslException("Failed to compute ECDH key.");
        }

        return symmetricKey;
    }

    /// Return the compressed EC point for the public key.
    std::vector<gsl::byte> ECKeyPair::CompressedECPublicKey(const std::string& publicKeyInPEM) {

        ///
        /// Extract public key
        ///
        BIO_free_ptr pubBio{BIO_new(BIO_s_mem())};
        if ((size_t)BIO_write(pubBio.get(), publicKeyInPEM.data(), publicKeyInPEM.size()) != publicKeyInPEM.size()) {
            ThrowOpensslException("Failed to load public key.");
        }

        EC_free_ptr ecPub {PEM_read_bio_EC_PUBKEY(pubBio.get(), nullptr, nullptr, nullptr)};
        if (!ecPub) {
            ThrowOpensslException("Failed to ec key from public key");
        }

        if (1 != EC_KEY_check_key(ecPub.get())) {
            ThrowOpensslException("Failed ec key(public) sanity check.");
        }

        auto pubkey  = EC_KEY_get0_public_key(ecPub.get());
        auto group = EC_KEY_get0_group(ecPub.get());
        if (!pubkey || !group) {
            ThrowOpensslException("Failed to get ec publickey/group.");
        }

        /// Get the EC_POINT in compressed form.
        std::vector<gsl::byte> point;
        auto len = EC_POINT_point2oct(group, pubkey,POINT_CONVERSION_COMPRESSED, nullptr, 0, nullptr);
        if (len == 0) {
            ThrowOpensslException("Failed to get ec point.");
        }

        point.resize(len);
        if(EC_POINT_point2oct(group, pubkey, POINT_CONVERSION_COMPRESSED,
                              reinterpret_cast<std::uint8_t*>(point.data()), len, nullptr) != len) {
            ThrowOpensslException("Failed to get ec point.");
        }

        return point;
    }

    /// Return Public key in PEM format from compressed EC Point.
    std::string ECKeyPair::GetPEMPublicKeyFromECPoint(Bytes compressedECPoint, const std::string& curveName) {

        auto eccgrp = OBJ_txt2nid(curveName.data());
        if (eccgrp == NID_undef) {
            ThrowOpensslException("Unknown curve name.");
        }

        EC_free_ptr ec{EC_KEY_new_by_curve_name(eccgrp)};
        auto group = EC_KEY_get0_group(ec.get());

        ECPoint_free_ptr ecPointFreePtr{EC_POINT_new(group)};

        auto retValue = EC_POINT_oct2point(group, ecPointFreePtr.get(),
                                      reinterpret_cast<const uint8_t *>(compressedECPoint.data()),
                                      compressedECPoint.size(), nullptr);
        if (retValue != 1) {
            ThrowOpensslException("Failed to get ec point from compressed point.");
        }

        retValue = EC_KEY_set_public_key(ec.get(), ecPointFreePtr.get());
        if (retValue != 1) {
            ThrowOpensslException("Failed to set public key.");
        }

        EVP_PKEY_free_ptr evppkeyPtr { EVP_PKEY_new()};
        if (1 != EVP_PKEY_assign_EC_KEY(evppkeyPtr.get(), ec.release())) {
            ThrowOpensslException("Error assigning EC key to EVP_PKEY structure.");
        }

        BIO_free_ptr bio{BIO_new(BIO_s_mem())};
        if (1 != PEM_write_bio_PUBKEY(bio.get(), evppkeyPtr.get())) {
            ThrowOpensslException("Error writing EC public key data in PEM format.");
        }

        // Read the public key from the buffer and put it in the string
        std::string publicKeyPem(BIO_pending(bio.get()), '\0');
        auto readResult = BIO_read(bio.get(), publicKeyPem.data(), static_cast<int>(publicKeyPem.size()));
        if (readResult <= 0) {
            ThrowOpensslException("Failed to read public key data.");
        }

        return publicKeyPem;
    }

    /// Generate a key using key derivation function.
    std::vector<gsl::byte> ECKeyPair::calculateHKDF(Bytes salt, Bytes secret) {

        std::vector<gsl::byte> key(secret.size());

        EVP_PKEY_CTX_free_ptr evpPkeyCtxPtr { EVP_PKEY_CTX_new_id(EVP_PKEY_HKDF, nullptr)};
        if (!evpPkeyCtxPtr) {
            ThrowOpensslException("Failed to create EVP_PKEY_CTX.");
        }

        if (1 != EVP_PKEY_derive_init(evpPkeyCtxPtr.get())) {
            ThrowOpensslException("EVP_PKEY_derive_init failed");
        }

        if (1 != EVP_PKEY_CTX_set_hkdf_md(evpPkeyCtxPtr.get(), EVP_sha256())) {
            ThrowOpensslException("EVP_PKEY_CTX_set_hkdf_md failed");
        }

        if (1 != EVP_PKEY_CTX_set1_hkdf_salt(evpPkeyCtxPtr.get(),
                                             reinterpret_cast<const std::uint8_t*>(salt.data()),
                                             salt.size())) {
            ThrowOpensslException("EVP_PKEY_CTX_set1_hkdf_salt failed");
        }

        if (1 != EVP_PKEY_CTX_set1_hkdf_key(evpPkeyCtxPtr.get(),
                                            reinterpret_cast<const std::uint8_t*>(secret.data()),
                                            secret.size())) {
            ThrowOpensslException("EVP_PKEY_CTX_set1_hkdf_key failed");
        }

        if (1 != EVP_PKEY_CTX_add1_hkdf_info(evpPkeyCtxPtr.get(), nullptr, 0)) {
            ThrowOpensslException("EVP_PKEY_CTX_add1_hkdf_info failed");
        }

        size_t outlen = key.size();
        if (1 != EVP_PKEY_derive(evpPkeyCtxPtr.get(), reinterpret_cast<std::uint8_t*>(key.data()), &outlen)) {
            ThrowOpensslException("EVP_PKEY_derive failed");
        }

        key.resize(outlen);

        return key;
    }

    /// Compute ECSDSA signature for the digest for the key-pair of the curve.
    std::vector<gsl::byte> ECKeyPair::ComputeECDSASig(Bytes digest, const std::string& privateKeyInPEM) {

        if (privateKeyInPEM.empty()) {
            ThrowException("Invalid data to compute the signature.");
        }

        BIO_free_ptr preBio{BIO_new(BIO_s_mem())};
        if ((size_t)BIO_write(preBio.get(), privateKeyInPEM.data(), privateKeyInPEM.size()) != privateKeyInPEM.size()) {
            ThrowOpensslException("Failed to load private key.");
        }

        EC_free_ptr ecKey {PEM_read_bio_ECPrivateKey(preBio.get(), nullptr, nullptr, nullptr)};
        if (!ecKey) {
            ThrowOpensslException("Failed to read ec private key from pem format");
        }

        if (1 != EC_KEY_check_key(ecKey.get())) {
            ThrowOpensslException("Failed the sanity check for ec private key");
        }

        ECDSASig_free_ptr ecdsaSigFreePtr(ECDSA_do_sign(reinterpret_cast<const uint8_t*>(digest.data()),
                                                        digest.size(), ecKey.get()));

        if (!ecdsaSigFreePtr) {
            ThrowOpensslException("Error generating the signature ECDSA_do_sign()");
        }

        // Calculate the size of the signature.
        auto group = EC_KEY_get0_group(ecKey.get());
        auto order = EC_GROUP_get0_order(group);
        auto bigNumLen = BN_num_bytes(order);
        auto sigLength = bigNumLen * 2;
        std::vector<gsl::byte> signature(sigLength);

        // Add 'r' to signature
        auto result =  BN_bn2binpad(ECDSA_SIG_get0_r(ecdsaSigFreePtr.get()),
                                    reinterpret_cast<uint8_t*>(signature.data()),  bigNumLen);
        if (!result) {
            ThrowOpensslException("Error converting BIGNUM to big endian - BN_bn2bin_padded()");
        }

        // Add 's' to signature
        result =  BN_bn2binpad(ECDSA_SIG_get0_s(ecdsaSigFreePtr.get()),
                               reinterpret_cast<uint8_t*>(signature.data() + bigNumLen), bigNumLen);
        if (!result) {
            ThrowOpensslException("Error converting BIGNUM to big endian - BN_bn2bin_padded()");
        }

        return signature;
    }

    /// Verify the signature for the digest for the key-pair of the curve.
    bool ECKeyPair::VerifyECDSASignature(Bytes digest, Bytes signature, const std::string& publicKeyInPEM) {

        if (publicKeyInPEM.empty()) {
            ThrowException("Invalid data to compute the signature.");
        }

        ///
        /// Extract public key
        ///
        EC_free_ptr ecPub = getECPublicKey(publicKeyInPEM);

        ECDSASig_free_ptr ecdsaSigFreePtr(ECDSA_SIG_new());
        if (!ecdsaSigFreePtr) {
            ThrowOpensslException("Error creating ECDSA_SIG");
        }

        BIGNUM_free_ptr rBigNum(BN_new());
        BIGNUM_free_ptr sBigNum(BN_new());

        auto sizeOfRAndS = (signature.size() / 2);

        if (!BN_bin2bn(reinterpret_cast<const uint8_t*>(signature.data()), sizeOfRAndS, rBigNum.get()) ||
            !BN_bin2bn(reinterpret_cast<const uint8_t*>(signature.data()+sizeOfRAndS), sizeOfRAndS, sBigNum.get())) {
            ThrowOpensslException("Error converting from big endian - BN_bin2bn()");
        }

        auto result = ECDSA_SIG_set0(ecdsaSigFreePtr.get(), rBigNum.release(), sBigNum.release());
        if (!result) {
            ThrowOpensslException("Error constructing ECDSA_SIG");
        }

        result = ECDSA_do_verify(reinterpret_cast<const uint8_t*>(digest.data()), digest.size(),
                                 ecdsaSigFreePtr.get(), ecPub.get());

        return (result == 1);
    }

    /// Retrieve EC_KEY from pem formatted public key.
    EC_free_ptr ECKeyPair::getECPublicKey(const std::string& publicKey){

        BIO_free_ptr pubBio{BIO_new(BIO_s_mem())};
        if ((size_t)BIO_write(pubBio.get(), publicKey.data(), publicKey.size()) != publicKey.size()) {
            ThrowOpensslException("Failed to load public key.");
        }

        EC_free_ptr ecPub;
        if (boost::contains(publicKey, kX509CertTag)) {

            X509_free_ptr x509Ptr{ PEM_read_bio_X509(pubBio.get(), NULL, NULL, NULL) };
            if (!x509Ptr) {
                ThrowOpensslException("Failed to create X509 cert struct.");
            }

            EVP_PKEY_free_ptr evppkeyPtr { X509_get_pubkey(x509Ptr.get()) };
            if (!evppkeyPtr) {
                ThrowOpensslException("Failed to create EVP_PKEY.");
            }

            ecPub.reset(EVP_PKEY_get1_EC_KEY(evppkeyPtr.get()));
            if (!ecPub) {
                ThrowOpensslException("Failed to ec key from public key");
            }
        } else {
            ecPub.reset(PEM_read_bio_EC_PUBKEY(pubBio.get(), nullptr, nullptr, nullptr));
            if (!ecPub) {
                ThrowOpensslException("Failed to ec key from public key");
            }
        }

        if (1 != EC_KEY_check_key(ecPub.get())) {
            ThrowOpensslException("Failed ec key(public) sanity check.");
        }

        return ecPub;
    }

}  // namespace virtru::crypto


/*
 * main.cpp
 *
 *  Created on: Sep 10, 2019
 *      Author: kyberszittya
 */
#include <limits>
#include <memory>

#include <ros/ros.h>
#include <Eigen/Dense>
#include <autoware_msgs/CloudClusterArray.h>
#include <autoware_msgs/Lane.h>
#include <autoware_msgs/LaneArray.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/PointStamped.h>
#include <monitoring_msgs/MonitorFeedback.h>
#include <monitoring_msgs/MonitorFeedbackArray.h>
#include <visualization_msgs/Marker.h>
#include <std_msgs/Int32.h>

#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <geometry_msgs/TransformStamped.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2/transform_datatypes.h>
#include <planner_msgs/LocalPlannerObjectDetection.h>

class RobotFirstOrderState
{
public:
	Eigen::Vector3d pos;
	Eigen::Quaterniond ori;
};

const double MAX_LATERAL_DISTANCE = 1.4;
const double MAX_LATERAL_DISTANCE_SQR = MAX_LATERAL_DISTANCE * MAX_LATERAL_DISTANCE;
const double MAX_LOOKAHEAD_DISTANCE = 16.8;
const double MAX_LOOKAHEAD_DISTANCE_SQR = MAX_LOOKAHEAD_DISTANCE * MAX_LOOKAHEAD_DISTANCE;
// Avoidance constants
const double AVOIDANCE_TRAJECTORY_POINT_LENGTH = 37;

class RobotFirstOrderStateSubscriber
{
private:
	std::unique_ptr<RobotFirstOrderState> state;
	std::shared_ptr<ros::NodeHandle> nh;
	// State subscribers
	std::string     topic_name_current_pose;
	ros::Subscriber sub_current_pose;
	std::string     topic_name_current_velocity;
	ros::Subscriber sub_current_velocity;
	geometry_msgs::TransformStamped _tr_base_link;
	geometry_msgs::TransformStamped _tr_velodyne;
	tf2_ros::Buffer _buffer;
	tf2_ros::TransformListener _listener;
	geometry_msgs::PoseStamped pose;
	geometry_msgs::PointStamped last_detected_object;
	// Pose
	ros::Publisher  pub_obstacle_lane_id;
public:
	RobotFirstOrderStateSubscriber(std::shared_ptr<ros::NodeHandle> nh): state(new RobotFirstOrderState()), nh(nh), _listener(_buffer) {}

	void cbCurrentPose(const geometry_msgs::PoseStamped::ConstPtr& msg)
	{
		pose = *msg;
		state->pos << msg->pose.position.x, msg->pose.position.y, msg->pose.position.z;
	}

	void cbCurrentVelocity(const geometry_msgs::TwistStamped::ConstPtr& msg)
	{

	}

	void init(std::shared_ptr<ros::NodeHandle> nh)
	{
		// TODO: with parameters
		// State subscribers
		sub_current_velocity = nh->subscribe("current_velocity", 100, &RobotFirstOrderStateSubscriber::cbCurrentVelocity, this);
		sub_current_pose     = nh->subscribe("current_pose",     100, &RobotFirstOrderStateSubscriber::cbCurrentPose,     this);
	}

	void distanceBetweenPointAndWaypoint(const geometry_msgs::PointStamped& point, const autoware_msgs::Lane& lane,
			const int& closest_waypoint_id,
			double& longitudinal_distance,
			double& lateral_distance,
			unsigned int& lane_id)
	{
		lateral_distance = std::numeric_limits<double>::infinity();
		longitudinal_distance = std::numeric_limits<double>::infinity();
		double lookahead_distance = 0.0;
		try
		{
			int target_waypoints;
			if (closest_waypoint_id + AVOIDANCE_TRAJECTORY_POINT_LENGTH > lane.waypoints.size())
			{
				target_waypoints = lane.waypoints.size();
			}
			else
			{
				target_waypoints = closest_waypoint_id + AVOIDANCE_TRAJECTORY_POINT_LENGTH;
			}
			for (unsigned int i = closest_waypoint_id; i < target_waypoints; i++)
			{
				geometry_msgs::PoseStamped waypoint_transformed;
				geometry_msgs::PointStamped point_transformed;
				_tr_base_link = _buffer.lookupTransform("base_link", "map", pose.header.stamp, ros::Duration(10.0));
				_tr_velodyne  = _buffer.lookupTransform("velodyne", "base_link", pose.header.stamp, ros::Duration(10.0));
				tf2::doTransform(lane.waypoints[i].pose, waypoint_transformed, _tr_base_link);
				tf2::doTransform(point, point_transformed, _tr_velodyne);
				double _d_obsx = point_transformed.point.x;
				double _d_obsy = point_transformed.point.y;
				double dx = (waypoint_transformed.pose.position.x - point_transformed.point.x);
				double dy = (waypoint_transformed.pose.position.y - point_transformed.point.y);
				double d_dist = sqrt(dx*dx + dy*dy);
				//lookahead_distance += d_dist;
				if (d_dist < 2.0)
				{
					lateral_distance = waypoint_transformed.pose.position.y - point_transformed.point.y;
					longitudinal_distance = _d_obsx;
					ROS_INFO_STREAM("Obstacle detected near to the waypoint" << lateral_distance << '\t' <<
							longitudinal_distance << '\t' << _d_obsy);
					last_detected_object = point_transformed;
					lane_id = i;
					break;
				}

			}
		}
		catch (tf2::LookupException& e)
		{
			ROS_ERROR_STREAM("Unable to find transform: " << e.what());
		}
	}

	friend class ObstacleMonitor;
};

class ObstacleMonitor
{
private:

	std::shared_ptr<ros::NodeHandle> nh;
	std::unique_ptr<RobotFirstOrderStateSubscriber> ego_state_subscriber;
	ros::Subscriber sub_detected_object_topic;
	ros::Subscriber sub_closest_waypoint_id;
	ros::Subscriber sub_base_waypoints;
	std::atomic<unsigned int> closest_waypoint_id;
	autoware_msgs::Lane msg_current_base_lane;
	// Publish event stream
	monitoring_msgs::MonitorFeedback event_detected_object;
	ros::Publisher pub_event_detected_object;
	ros::Publisher pub_local_planner_object_detection; 			///< Publish planner object detection
	ros::Publisher  pub_detected_object_marker;
	// Autoware lane
public:
	ObstacleMonitor(std::shared_ptr<ros::NodeHandle> nh): nh(nh), ego_state_subscriber(new RobotFirstOrderStateSubscriber(nh)) {}

	void cbClosestLaneId(const std_msgs::Int32::ConstPtr& msg)
	{
		closest_waypoint_id = msg->data;
		ROS_DEBUG_STREAM("Closest waypoint: " << closest_waypoint_id);
	}

	void cbBaseWaypoints(const autoware_msgs::Lane::ConstPtr& msg)
	{
		msg_current_base_lane = *msg;

	}

	void cbDetectedObject(const autoware_msgs::CloudClusterArray::ConstPtr& msg)
	{
		for (const auto& cluster: msg->clusters)
		{
			double lateral_distance;
			double longitudinal_distance;
			unsigned int lane_id;
			ego_state_subscriber->distanceBetweenPointAndWaypoint(cluster.centroid_point,
					msg_current_base_lane, closest_waypoint_id,
					longitudinal_distance,
					lateral_distance,
					lane_id);
			if (longitudinal_distance < MAX_LOOKAHEAD_DISTANCE && lateral_distance < MAX_LATERAL_DISTANCE)
			{
				auto event_time = ros::Time::now();
				monitoring_msgs::MonitorFeedback event;
				// Publish local planner object detection
				planner_msgs::LocalPlannerObjectDetection localplanner_msg;
				localplanner_msg.avoidance_start_index = closest_waypoint_id;
				localplanner_msg.avoidance_end_index   = closest_waypoint_id + AVOIDANCE_TRAJECTORY_POINT_LENGTH;
				localplanner_msg.header.stamp = event_time;
				localplanner_msg.event.signal_id = "LANE_OBJECT_DETECTED";
				localplanner_msg.event.header.stamp = event_time;
				localplanner_msg.event.val = abs(2.0);
				localplanner_msg.event.severity = monitoring_msgs::MonitorFeedback::SEVERITY_WARNING;
				localplanner_msg.event.eval = 1;
				pub_local_planner_object_detection.publish(localplanner_msg);
				visualization_msgs::Marker object_marker;
				object_marker.header.frame_id = "base_link";
				object_marker.header.stamp = ros::Time::now();

				object_marker.color.a = 1.0;
				object_marker.color.r = 1.0;
				object_marker.color.g = 0.0;
				object_marker.color.b = 0.0;
				object_marker.type = visualization_msgs::Marker::SPHERE;
				object_marker.scale.x = 1.0;
				object_marker.scale.y = 1.0;
				object_marker.scale.z = 1.0;
				object_marker.pose.position.x = ego_state_subscriber->last_detected_object.point.x;
				object_marker.pose.position.y = ego_state_subscriber->last_detected_object.point.y;
				object_marker.pose.position.z = ego_state_subscriber->last_detected_object.point.z;
				pub_detected_object_marker.publish(object_marker);
				ROS_INFO_STREAM("Obstacle detected at: " << lateral_distance << '\t' << longitudinal_distance << '\n');

			}
		}

	}

	void init()
	{
		// Event stream
		pub_local_planner_object_detection =
				nh->advertise<planner_msgs::LocalPlannerObjectDetection>("/monitor/events/local_planner/object_detection", 1000);
		pub_detected_object_marker =
				nh->advertise<visualization_msgs::Marker>("monitor/events/local_planner/detected_object/marker", 10);

		sub_detected_object_topic = nh->subscribe("/detection/lidar_detector/cloud_clusters", 10, &ObstacleMonitor::cbDetectedObject, this);
		// Autoware specific
		sub_closest_waypoint_id = nh->subscribe("/closest_waypoint", 10, &ObstacleMonitor::cbClosestLaneId, this);
		sub_base_waypoints = nh->subscribe("/base_waypoints", 10, &ObstacleMonitor::cbBaseWaypoints, this);
	}
};

int main(int argc, char ** argv)
{
	ros::init(argc, argv, "ros_object_detection_monitor");
	std::shared_ptr<ros::NodeHandle> nh(new ros::NodeHandle);
	ObstacleMonitor monitor(nh);
	monitor.init();
	ROS_INFO("Node initialized: object detection monitor");
	ros::spin();
	return 0;
}


// Author(s): Wieger Wesselink
// Copyright: see the accompanying file COPYING or copy at
// https://github.com/mCRL2org/mCRL2/blob/master/COPYING
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
/// \file pbes_expression_test.cpp
/// \brief Add your file description here.

#define BOOST_TEST_MODULE pbes_expression_test
#include <boost/test/included/unit_test_framework.hpp>
#include "mcrl2/data/data_expression.h"
#include "mcrl2/pbes/find.h"
#include "mcrl2/pbes/join.h"
#include "mcrl2/pbes/parse.h"

using namespace mcrl2;
using namespace mcrl2::pbes_system;
using namespace mcrl2::pbes_system::detail;

BOOST_AUTO_TEST_CASE(test_accessors)
{
  using namespace mcrl2::pbes_system::accessors;

  std::string EXPRESSIONS =
    "datavar                                  \n"
    "  n: Nat;                                \n"
    "                                         \n"
    "predvar                                  \n"
    "                                         \n"
    "expressions                              \n"
    "  val(n > 2);                            \n"
    "  val(n > 3)                             \n"
  ;

  std::vector<pbes_expression> expressions = parse_pbes_expressions(EXPRESSIONS).first;
  pbes_expression x = expressions[0];
  pbes_expression y = expressions[1];
  data::variable d(core::identifier_string("d"), data::sort_nat::nat());
  data::variable_list v = { d };
  pbes_expression z = d;
  propositional_variable_instantiation X(core::identifier_string("X"), { d });

  std::set<pbes_expression> q;
  q.insert(x);
  q.insert(y);
  q.insert(z);

  {
    using namespace mcrl2::pbes_system::accessors;

    pbes_expression a, b, c;
    data::variable_list w;
    core::identifier_string s;
    data::data_expression e;
    std::set<pbes_expression> q1;

    a = not_(x);
    b = arg(a);
    BOOST_CHECK(x == b);

    a = and_(x, y);
    b = left(a);
    c = right(a);
    BOOST_CHECK(x == b);
    BOOST_CHECK(y == c);

    a = or_(x, y);
    b = left(a);
    c = right(a);
    BOOST_CHECK(x == b);
    BOOST_CHECK(y == c);

    a = imp(x, y);
    b = left(a);
    c = right(a);
    BOOST_CHECK(x == b);
    BOOST_CHECK(y == c);

    a = forall(v, x);
    w = var(a);
    b = arg(a);
    BOOST_CHECK(v == w);
    BOOST_CHECK(x == b);

    a = exists(v, x);
    w = var(a);
    b = arg(a);
    BOOST_CHECK(v == w);
    BOOST_CHECK(x == b);

    s = name(X);
    BOOST_CHECK(s == core::identifier_string("X"));

    const data::data_expression_list& f = param(X);
    data::data_expression_list g = { d };
    BOOST_CHECK(f == g);

    a = join_or(q.begin(), q.end());
    q1 = split_or(a);
    BOOST_CHECK(q == q1);

    a = join_and(q.begin(), q.end());
    q1 = split_and(a);
    BOOST_CHECK(q == q1);
  }

  {
    pbes_expression a;
    core::identifier_string s;
    data::data_expression e;
    std::set<pbes_expression> q1;

    a = not_(x);
    a = and_(x, y);
    a = or_(x, y);
    a = imp(x, y);
    a = forall(v, x);
    a = exists(v, x);
    s = name(X);
    param(X);
    a = join_or(q.begin(), q.end());
    a = join_and(q.begin(), q.end());
    q1 = split_or(a);
    q1 = split_and(a);
  }
}

BOOST_AUTO_TEST_CASE(test_term_traits)
{
  typedef core::term_traits<pbes_expression> tr;

  const std::string VARSPEC =
    "datavar         \n"
    "  m: Nat;       \n"
    "  n: Nat;       \n"
    "  b: Bool;      \n"
    "  c: Bool;      \n"
    "                \n"
    "predvar         \n"
    "  X: Bool, Pos; \n"
    "  Y: Nat;       \n"
    ;

  pbes_expression x, y, z;
  data::variable_list v;
  data::data_expression_list e;

  // and 1
  x = parse_pbes_expression("Y(1) && Y(2)", VARSPEC);
  z = pbes_system::accessors::left(x);
  z = pbes_system::accessors::right(x);

  // and 2
  x = parse_pbes_expression("val(b && c)", VARSPEC);
  if (tr::is_and(x))
  {
    BOOST_CHECK(false);
    z = pbes_system::accessors::left(x);
    z = pbes_system::accessors::right(x);
  }

  // or 1
  x = parse_pbes_expression("Y(1) || Y(2)", VARSPEC);
  z = pbes_system::accessors::left(x);
  z = pbes_system::accessors::right(x);

  // or 2
  x = parse_pbes_expression("val(b || c)", VARSPEC);
  if (tr::is_or(x))
  {
    BOOST_CHECK(false);
    z = pbes_system::accessors::left(x);
    z = pbes_system::accessors::right(x);
  }

  // imp 1
  x = parse_pbes_expression("Y(1) => !Y(2)", VARSPEC);
  z = pbes_system::accessors::left(x);
  z = pbes_system::accessors::right(x);

  // imp 2
  x = parse_pbes_expression("val(b => c)", VARSPEC);
  if (tr::is_imp(x))
  {
    BOOST_CHECK(false);
    z = pbes_system::accessors::left(x);
    z = pbes_system::accessors::right(x);
  }

  // not 1
  x = parse_pbes_expression("!(Y(1) || Y(2))", VARSPEC);
  z = pbes_system::accessors::arg(x);

  // not 2
  x = parse_pbes_expression("!val(n < 10)", VARSPEC);
  z = pbes_system::accessors::arg(x);

  // not 3
  x = parse_pbes_expression("val(!(n < 10))", VARSPEC);
  if (tr::is_not(x))
  {
    BOOST_CHECK(false);
    z = pbes_system::accessors::arg(x);
  }

  // prop var 1
  x = parse_pbes_expression("Y(1)", VARSPEC);
  e = atermpp::down_cast<propositional_variable_instantiation>(x).parameters();

  // forall 1
  x = parse_pbes_expression("forall k:Nat.Y(k)", VARSPEC);
  v = tr::var(x);
  z = pbes_system::accessors::arg(x);

  // exists 1
  x = parse_pbes_expression("exists k:Nat.Y(k)", VARSPEC);
  v = tr::var(x);
  z = pbes_system::accessors::arg(x);
}


// Copyright (C) 2016 Vicente J. Botet Escriba
//
//  Distributed under the Boost Software License, Version 1.0. (See accompanying
//  file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

// <experimental/monad.hpp>
// <experimental/optional.hpp>

#include <boost/detail/lightweight_test.hpp>
#include <experimental/monad.hpp>
#include <experimental/optional.hpp>

std::experimental::optional<int> twice(int i) {
  if (i==0) return std::experimental::nullopt;
  return std::experimental::make_optional(2*i);
}

int main()
{
  namespace stde = std::experimental;

  static_assert(stde::is_nullable<stde::optional<int>>::value, "ERROR");
  static_assert(stde::is_type_constructible<stde::optional<int>>::value, "ERROR");
  static_assert(stde::is_monad<stde::optional<stde::_t>>::value, "ERROR");

  {
    stde::optional<int> x = stde::none<stde::optional>();
    stde::optional<int> y = stde::monad::bind(x,twice);
    BOOST_TEST(! stde::has_value(y));
  }
  {
    int v=1;
    stde::optional<int> x = stde::make<stde::optional>(v);
    stde::optional<int> y = stde::monad::bind(x,twice);
    BOOST_TEST(stde::has_value(y));
    BOOST_TEST(*y == 2);
  }
  {
    int v=0;
    stde::optional<int> x = stde::make<stde::optional>(v);
    stde::optional<int> y = stde::monad::bind(x,twice);
    BOOST_TEST(! stde::has_value(y));
  }
  {
    int v=0;
    stde::optional<stde::optional<int>> x = stde::make<stde::optional>(v);
    stde::optional<int> y = stde::monad::unwrap(x);
    BOOST_TEST(stde::has_value(y));
    BOOST_TEST(*y == 0);
  }

  return ::boost::report_errors();
}


#ifndef BOOST_SAFE_FLOAT_LITERALS_HPP
#define BOOST_SAFE_FLOAT_LITERALS_HPP

#include <boost/safe_float.hpp>

namespace boost
{
namespace safe_float
{
namespace literals
{
namespace detail
{
#define HEX_PARSE_ERROR "Not a valid hex floating point number literal"
#define DEC_PARSE_ERROR "Not a valid dec floating point number literal"
#define VALUE_ERROR "Floating point number isn't a positive power of 0.5"

// A struct to be used in static_assert
template<typename T>
struct always_false
{
    static constexpr bool value = false;
};

// A constexpr version of isxdigit working on ASCII
constexpr bool isxdigit(char c)
{
    return ((c >= '0' and c <= '9') or (c >= 'a' and c <= 'f') or (c >= 'A' and c <= 'F'));
}

// A constexpr version of isdigit working on ASCII
constexpr bool isdigit(char c)
{
    return (c >= '0' and c <= '9');
}

// constexpr function to get the represented value of a hex digit
constexpr int hexvalue(char xdigit)
{
    return (xdigit >= '0' and xdigit <= '9') ?
               xdigit - '0' :
               (xdigit >= 'a' and xdigit <= 'f') ? xdigit - 'a' + 10 :
                                                   (xdigit >= 'A' and xdigit <= 'F') ? xdigit - 'A' + 10 : 0;
}

// constexpr function to get the represented value of a digit
constexpr int decvalue(char digit)
{
    return (digit >= '0' and digit <= '9') ? digit - '0' : 0;
}

// constexpr function that count bit equal to 1
constexpr int count_bit(unsigned long long a)
{
    int count = 0;
    while (a != 0)
    {
        if (a & 0x1) count++;
        a >>= 1;
    }

    return count;
}

// A constexpr function that count the number of bit with value 0 before a bit with 1
constexpr int count_shift(unsigned long long a)
{
    int count = 0;
    while (a != 0)
    {
        if (a & 0x1) return count;
        count++;
        a >>= 1;
    }

    return count;
}

template<typename T>
constexpr T trivial_pow(T x, int e)
{
    return e < 0 ? 1 / trivial_pow(x, -e) : e == 0 ? 1 : x * trivial_pow(x, e - 1);
}

// A function to know is a literal begins with an hex prefix
template<char FIRST>
constexpr bool has_hex_prefix()
{
    return false;
}

template<char FIRST, char SECOND, char... REST>
constexpr bool has_hex_prefix()
{
    return FIRST == '0' and (SECOND == 'x' or SECOND == 'X');
}

/*
 * Hex parsing related functions (better understanding by reading from bottom to top)
 */

// Function used to validate the number and returning its value
template<typename RT, unsigned long long MANTISSA, int POW, bool SIGN, int EXPONENT>
constexpr RT validate_hex()
{
    // Is it a power of two ?
    static_assert(count_bit(MANTISSA) == 1, VALUE_ERROR);
    // Is the exponent negative (positive power of 0.5 means negative power of 2)
    static_assert(count_shift(MANTISSA) + POW + (SIGN ? EXPONENT : -EXPONENT) < 0, VALUE_ERROR);
    return static_cast<RT>(MANTISSA) * trivial_pow(2.0l, POW + (SIGN ? EXPONENT : -EXPONENT));
}

// Function used to parse the digit of the exponent
template<typename RT, unsigned long long MANTISSA, int POW, bool SIGN, int EXPONENT>
constexpr RT parse_hex_exponent()
{
    return validate_hex<RT, MANTISSA, POW, SIGN, EXPONENT>();
}

template<typename RT, unsigned long long MANTISSA, int POW, bool SIGN, int EXPONENT, char FIRST, char... REST>
constexpr RT parse_hex_exponent()
{
    static_assert(isdigit(FIRST), HEX_PARSE_ERROR);
    return parse_hex_exponent<RT, MANTISSA, POW, SIGN, EXPONENT * 10 + decvalue(FIRST), REST...>();
}

// This function parses the first character after the 'p'
template<typename RT, unsigned long long MANTISSA, int POW>
constexpr RT parse_hex_exponent_sign()
{
    static_assert(always_false<RT>::value, HEX_PARSE_ERROR);
    return RT{};
}

template<typename RT, unsigned long long MANTISSA, int POW, char FIRST, char... REST>
constexpr RT parse_hex_exponent_sign()
{
    static_assert(isxdigit(FIRST) or FIRST == '+' or FIRST == '-', HEX_PARSE_ERROR);
    if constexpr (isxdigit(FIRST))
        return parse_hex_exponent<RT, MANTISSA, POW, true, hexvalue(FIRST), REST...>();
    else
    {
        // If we have a sign, we need to make there is at least one other character after
        static_assert(sizeof...(REST) > 0, HEX_PARSE_ERROR);
        if constexpr (FIRST == '+')
            return parse_hex_exponent<RT, MANTISSA, POW, true, 0, REST...>();
        else
            return parse_hex_exponent<RT, MANTISSA, POW, false, 0, REST...>();
    }
}

// Function used to parse the characters of a hex float literal that appear after the period
template<typename RT, unsigned long long MANTISSA, int POW>
constexpr RT parse_hex_post_period()
{
    static_assert(always_false<RT>::value, HEX_PARSE_ERROR);
    return RT{};
}

template<typename RT, unsigned long long MANTISSA, int POW, char FIRST, char... REST>
constexpr RT parse_hex_post_period()
{
    static_assert(isxdigit(FIRST) or FIRST == 'p' or FIRST == 'P', HEX_PARSE_ERROR);
    if constexpr (isxdigit(FIRST))
        return parse_hex_post_period<RT, MANTISSA * 16 + hexvalue(FIRST), POW - 4, REST...>();
    else
        return parse_hex_exponent_sign<RT, MANTISSA, POW, REST...>();
}

// Function used to parse the characters of a hex float literal that appear before the period
template<typename RT, unsigned long long MANTISSA>
constexpr RT parse_hex_pre_period()
{
    // If they are no more characters and we're here it means that is not a good hex float literal
    static_assert(always_false<RT>::value, HEX_PARSE_ERROR);
    return RT{};
}

template<typename RT, unsigned long long MANTISSA, char FIRST, char... REST>
constexpr RT parse_hex_pre_period()
{
    static_assert(isxdigit(FIRST) or FIRST == '.' or FIRST == 'p' or FIRST == 'P', HEX_PARSE_ERROR);
    if constexpr (isxdigit(FIRST))
        return parse_hex_pre_period<RT, MANTISSA * 16 + hexvalue(FIRST), REST...>();
    else if constexpr (FIRST == '.')
        return parse_hex_post_period<RT, MANTISSA, 0, REST...>();
    else
        return parse_hex_exponent_sign<RT, MANTISSA, 0, REST...>();
}

template<typename RT, char FIRST, char SECOND, char... REST>
constexpr RT test_hex()
{
    // Discard 2 first char as they are the hex marker
    return parse_hex_pre_period<RT, 0, REST...>();
}

// Validate value and returns it
template<typename RT, unsigned long long MANTISSA, int POW, bool SIGN, int EXPONENT>
constexpr RT validate_dec()
{
    constexpr RT value =
        (POW + (SIGN ? EXPONENT : -EXPONENT) < 0 ? MANTISSA / trivial_pow(10.0l, -POW - (SIGN ? EXPONENT : -EXPONENT)) :
                                                   MANTISSA * trivial_pow(10.0l, POW + (SIGN ? EXPONENT : -EXPONENT)));
    return value;
}

// This function parses the exponent past the first character
template<typename RT, unsigned long long MANTISSA, int POW, bool SIGN, int EXPONENT>
constexpr RT parse_dec_exponent()
{
    return validate_dec<RT, MANTISSA, POW, SIGN, EXPONENT>();
}

template<typename RT, unsigned long long MANTISSA, int POW, bool SIGN, int EXPONENT, char FIRST, char... REST>
constexpr RT parse_dec_exponent()
{
    static_assert(isdigit(FIRST), DEC_PARSE_ERROR);
    return parse_dec_exponent<RT, MANTISSA, POW, SIGN, EXPONENT * 10 + decvalue(FIRST), REST...>();
}

// This function parses the first character of the exponent
template<typename RT, unsigned long long MANTISSA, int POW>
constexpr RT parse_dec_exponent_sign()
{
    static_assert(always_false<RT>::value, DEC_PARSE_ERROR);
    return RT{};
}

template<typename RT, unsigned long long MANTISSA, int POW, char FIRST, char... REST>
constexpr RT parse_dec_exponent_sign()
{
    static_assert(isdigit(FIRST) or ((FIRST == '+' or FIRST == '-') and sizeof...(REST) > 0), DEC_PARSE_ERROR);
    if constexpr (isdigit(FIRST))
        return parse_dec_exponent<RT, MANTISSA, POW, true, decvalue(FIRST), REST...>();
    else if constexpr (FIRST == '+')
        return parse_dec_exponent<RT, MANTISSA, POW, true, 0, REST...>();
    else
        return parse_dec_exponent<RT, MANTISSA, POW, false, 0, REST...>();
}

// This function parses the characters that are after the period
template<typename RT, unsigned long long MANTISSA, int POW>
constexpr RT parse_dec_post_period()
{
    return validate_dec<RT, MANTISSA, POW, true, 0>();
}

template<typename RT, unsigned long long MANTISSA, int POW, char FIRST, char... REST>
constexpr RT parse_dec_post_period()
{
    static_assert(isdigit(FIRST) or FIRST == 'e' or FIRST == 'E', DEC_PARSE_ERROR);
    if constexpr (isdigit(FIRST))
        return parse_dec_post_period<RT, MANTISSA * 10 + decvalue(FIRST), POW - 1, REST...>();
    else
        return parse_dec_exponent_sign<RT, MANTISSA, POW, REST...>();
}

// This character parses the first characters before the period
template<typename RT, unsigned long long MANTISSA>
constexpr RT parse_dec_pre_period()
{
    return validate_dec<RT, MANTISSA, 0, true, 0>();
}

template<typename RT, unsigned long long MANTISSA, char FIRST, char... REST>
constexpr RT parse_dec_pre_period()
{
    static_assert(isdigit(FIRST) or FIRST == '.' or FIRST == 'e' or FIRST == 'E', DEC_PARSE_ERROR);
    if constexpr (isdigit(FIRST))
        return parse_dec_pre_period<RT, MANTISSA * 10 + decvalue(FIRST), REST...>();
    else if constexpr (FIRST == '.')
        return parse_dec_post_period<RT, MANTISSA, 0, REST...>();
    else
        return parse_dec_exponent_sign<RT, MANTISSA, 0, REST...>();
}

template<typename RT, char... STR>
constexpr RT test_dec()
{
    return parse_dec_pre_period<RT, 0, STR...>();
}

// Don't pollute global namespace
#undef HEX_PARSE_ERROR
#undef DEC_PARSE_ERROR
#undef VALUE_ERROR

template<typename RT, char... STR>
constexpr RT parse_float_literal()
{
    if constexpr (has_hex_prefix<STR...>())
        return test_hex<RT, STR...>();
    else
        return test_dec<RT, STR...>();
}
} // namespace detail

template<char... STR>
boost::safe_float::safe_float<double> operator""_sd()
{
    return boost::safe_float::safe_float<double>(detail::parse_float_literal<double, STR...>());
}

} // namespace literals
} // namespace safe_float
} // namespace boost

#endif // BOOST_SAFE_FLOAT_LITERALS_HPP