// This file automatically generated by create_export_module.py
#define NO_IMPORT_ARRAY

#include <NumpyEigenConverter.hpp>

#include <boost/cstdint.hpp>

void import_D_D_int()
{
	NumpyEigenConverter<Eigen::Matrix< int, Eigen::Dynamic, Eigen::Dynamic > >::register_converter();
}

void import_D_1_int()
{
	NumpyEigenConverter<Eigen::Matrix< int, Eigen::Dynamic, 1 > >::register_converter();
}

void import_D_2_int()
{
	NumpyEigenConverter<Eigen::Matrix< int, Eigen::Dynamic, 2 > >::register_converter();
}

void import_D_3_int()
{
	NumpyEigenConverter<Eigen::Matrix< int, Eigen::Dynamic, 3 > >::register_converter();
}

void import_D_4_int()
{
	NumpyEigenConverter<Eigen::Matrix< int, Eigen::Dynamic, 4 > >::register_converter();
}

void import_D_5_int()
{
	NumpyEigenConverter<Eigen::Matrix< int, Eigen::Dynamic, 5 > >::register_converter();
}

void import_D_6_int()
{
	NumpyEigenConverter<Eigen::Matrix< int, Eigen::Dynamic, 6 > >::register_converter();
}

void import_1_D_int()
{
	NumpyEigenConverter<Eigen::Matrix< int, 1, Eigen::Dynamic > >::register_converter();
}

void import_1_1_int()
{
	NumpyEigenConverter<Eigen::Matrix< int, 1, 1 > >::register_converter();
}

void import_1_2_int()
{
	NumpyEigenConverter<Eigen::Matrix< int, 1, 2 > >::register_converter();
}

void import_1_3_int()
{
	NumpyEigenConverter<Eigen::Matrix< int, 1, 3 > >::register_converter();
}

void import_1_4_int()
{
	NumpyEigenConverter<Eigen::Matrix< int, 1, 4 > >::register_converter();
}

void import_1_5_int()
{
	NumpyEigenConverter<Eigen::Matrix< int, 1, 5 > >::register_converter();
}

void import_1_6_int()
{
	NumpyEigenConverter<Eigen::Matrix< int, 1, 6 > >::register_converter();
}

void import_2_D_int()
{
	NumpyEigenConverter<Eigen::Matrix< int, 2, Eigen::Dynamic > >::register_converter();
}

void import_2_1_int()
{
	NumpyEigenConverter<Eigen::Matrix< int, 2, 1 > >::register_converter();
}

void import_2_2_int()
{
	NumpyEigenConverter<Eigen::Matrix< int, 2, 2 > >::register_converter();
}

void import_2_3_int()
{
	NumpyEigenConverter<Eigen::Matrix< int, 2, 3 > >::register_converter();
}

void import_2_4_int()
{
	NumpyEigenConverter<Eigen::Matrix< int, 2, 4 > >::register_converter();
}

void import_2_5_int()
{
	NumpyEigenConverter<Eigen::Matrix< int, 2, 5 > >::register_converter();
}

void import_2_6_int()
{
	NumpyEigenConverter<Eigen::Matrix< int, 2, 6 > >::register_converter();
}

void import_3_D_int()
{
	NumpyEigenConverter<Eigen::Matrix< int, 3, Eigen::Dynamic > >::register_converter();
}

void import_3_1_int()
{
	NumpyEigenConverter<Eigen::Matrix< int, 3, 1 > >::register_converter();
}

void import_3_2_int()
{
	NumpyEigenConverter<Eigen::Matrix< int, 3, 2 > >::register_converter();
}

void import_3_3_int()
{
	NumpyEigenConverter<Eigen::Matrix< int, 3, 3 > >::register_converter();
}

void import_3_4_int()
{
	NumpyEigenConverter<Eigen::Matrix< int, 3, 4 > >::register_converter();
}

void import_3_5_int()
{
	NumpyEigenConverter<Eigen::Matrix< int, 3, 5 > >::register_converter();
}

void import_3_6_int()
{
	NumpyEigenConverter<Eigen::Matrix< int, 3, 6 > >::register_converter();
}

void import_4_D_int()
{
	NumpyEigenConverter<Eigen::Matrix< int, 4, Eigen::Dynamic > >::register_converter();
}

void import_4_1_int()
{
	NumpyEigenConverter<Eigen::Matrix< int, 4, 1 > >::register_converter();
}

void import_4_2_int()
{
	NumpyEigenConverter<Eigen::Matrix< int, 4, 2 > >::register_converter();
}

void import_4_3_int()
{
	NumpyEigenConverter<Eigen::Matrix< int, 4, 3 > >::register_converter();
}

void import_4_4_int()
{
	NumpyEigenConverter<Eigen::Matrix< int, 4, 4 > >::register_converter();
}

void import_4_5_int()
{
	NumpyEigenConverter<Eigen::Matrix< int, 4, 5 > >::register_converter();
}

void import_4_6_int()
{
	NumpyEigenConverter<Eigen::Matrix< int, 4, 6 > >::register_converter();
}

void import_5_D_int()
{
	NumpyEigenConverter<Eigen::Matrix< int, 5, Eigen::Dynamic > >::register_converter();
}

void import_5_1_int()
{
	NumpyEigenConverter<Eigen::Matrix< int, 5, 1 > >::register_converter();
}

void import_5_2_int()
{
	NumpyEigenConverter<Eigen::Matrix< int, 5, 2 > >::register_converter();
}

void import_5_3_int()
{
	NumpyEigenConverter<Eigen::Matrix< int, 5, 3 > >::register_converter();
}

void import_5_4_int()
{
	NumpyEigenConverter<Eigen::Matrix< int, 5, 4 > >::register_converter();
}

void import_5_5_int()
{
	NumpyEigenConverter<Eigen::Matrix< int, 5, 5 > >::register_converter();
}

void import_5_6_int()
{
	NumpyEigenConverter<Eigen::Matrix< int, 5, 6 > >::register_converter();
}

void import_6_D_int()
{
	NumpyEigenConverter<Eigen::Matrix< int, 6, Eigen::Dynamic > >::register_converter();
}

void import_6_1_int()
{
	NumpyEigenConverter<Eigen::Matrix< int, 6, 1 > >::register_converter();
}

void import_6_2_int()
{
	NumpyEigenConverter<Eigen::Matrix< int, 6, 2 > >::register_converter();
}

void import_6_3_int()
{
	NumpyEigenConverter<Eigen::Matrix< int, 6, 3 > >::register_converter();
}

void import_6_4_int()
{
	NumpyEigenConverter<Eigen::Matrix< int, 6, 4 > >::register_converter();
}

void import_6_5_int()
{
	NumpyEigenConverter<Eigen::Matrix< int, 6, 5 > >::register_converter();
}

void import_6_6_int()
{
	NumpyEigenConverter<Eigen::Matrix< int, 6, 6 > >::register_converter();
}


#include "VideoFaceDetector.h"
#include <dlib/dnn.h>
#include <dlib/image_io.h>
#include <dlib/image_processing/frontal_face_detector.h>
#include <dlib/opencv.h>
#include <dlib/string.h>
#include <iostream>
//#include <nlohmann/json.hpp>
#include <opencv2/imgproc.hpp>

// using json = nlohmann::json;
using namespace dlib;

// ----------------------------------------------------------------------------------------

// The next bit of code defines a ResNet network.  It's basically copied
// and pasted from the dnn_imagenet_ex.cpp example, except we replaced the loss
// layer with loss_metric and made the network somewhat smaller.  Go read the
// introductory dlib DNN examples to learn what all this stuff means.
//
// Also, the dnn_metric_learning_on_images_ex.cpp example shows how to train
// this network. The dlib_face_recognition_resnet_model_v1 model used by this
// example was trained using essentially the code shown in
// dnn_metric_learning_on_images_ex.cpp except the mini-batches were made larger
// (35x15 instead of 5x5), the iterations without progress was set to 10000, and
// the training dataset consisted of about 3 million images instead of
// 55.  Also, the input layer was locked to images of size 150.
template <template <int, template <typename> class, int, typename> class block,
          int N, template <typename> class BN, typename SUBNET>
using residual = add_prev1<block<N, BN, 1, tag1<SUBNET>>>;

template <template <int, template <typename> class, int, typename> class block,
          int N, template <typename> class BN, typename SUBNET>
using residual_down =
    add_prev2<avg_pool<2, 2, 2, 2, skip1<tag2<block<N, BN, 2, tag1<SUBNET>>>>>>;

template <int N, template <typename> class BN, int stride, typename SUBNET>
using block =
    BN<con<N, 3, 3, 1, 1, relu<BN<con<N, 3, 3, stride, stride, SUBNET>>>>>;

template <int N, typename SUBNET>
using ares = relu<residual<block, N, affine, SUBNET>>;
template <int N, typename SUBNET>
using ares_down = relu<residual_down<block, N, affine, SUBNET>>;

template <typename SUBNET> using alevel0 = ares_down<256, SUBNET>;
template <typename SUBNET>
using alevel1 = ares<256, ares<256, ares_down<256, SUBNET>>>;
template <typename SUBNET>
using alevel2 = ares<128, ares<128, ares_down<128, SUBNET>>>;
template <typename SUBNET>
using alevel3 = ares<64, ares<64, ares<64, ares_down<64, SUBNET>>>>;
template <typename SUBNET> using alevel4 = ares<32, ares<32, ares<32, SUBNET>>>;

using anet_type = loss_metric<fc_no_bias<
    128, avg_pool_everything<alevel0<alevel1<alevel2<alevel3<alevel4<max_pool<
             3, 3, 2, 2,
             relu<affine<con<32, 7, 7, 2, 2, input_rgb_image_sized<150>
                             //                              input_rgb_image
                             >>>>>>>>>>>>;

static anet_type net;
static bool initialized = false;

const double VideoFaceDetector::TICK_FREQUENCY = cv::getTickFrequency();

void VideoFaceDetector::add_faces(const std::string &folder) {

  for (auto file_name : directory(folder).get_files()) {
    auto pos = file_name.name().find(".jpg");
    if (pos == std::string::npos)
      continue;
    matrix<rgb_pixel> face_chip;
    dlib::load_jpeg(face_chip, file_name);
    auto &face_descriptor = net(face_chip);
    if (face_descriptor.size() == face_dim) {
      nb++;
      copy(face_descriptor.begin(), face_descriptor.end(), back_inserter(xb));
      std::cout << "data" << nb << std::endl;
    }
  }

  // reindex
  reindex();
}

VideoFaceDetector::VideoFaceDetector(const std::string cascadeFilePath,
                                     cv::VideoCapture &videoCapture,
                                     const std::string name, bool learn) {

  if (!initialized) {
    deserialize("dlib_face_recognition_resnet_model_v1.dat") >> net;
    initialized = true;
  }

  setFaceCascade(cascadeFilePath);
  setVideoCapture(videoCapture);

  m_name = name;

  //  m_cli = new httplib::SSLClient("localhost", 5000);

  if (learn)
    add_faces(name);
  m_learn = learn;
}

void VideoFaceDetector::setVideoCapture(cv::VideoCapture &videoCapture) {
  m_videoCapture = &videoCapture;
}

cv::VideoCapture *VideoFaceDetector::videoCapture() const {
  return m_videoCapture;
}

void VideoFaceDetector::setFaceCascade(const std::string cascadeFilePath) {
  if (m_faceCascade == nullptr) {
    m_faceCascade = new cv::CascadeClassifier(cascadeFilePath);
  } else {
    m_faceCascade->load(cascadeFilePath);
  }

  if (m_faceCascade->empty()) {
    std::cerr << "Error creating cascade classifier. Make sure the file"
              << std::endl
              << cascadeFilePath << " exists." << std::endl;
  }
}

cv::CascadeClassifier *VideoFaceDetector::faceCascade() const {
  return m_faceCascade;
}

void VideoFaceDetector::setResizedWidth(const int width) {
  m_resizedWidth = std::max(width, 1);
}

int VideoFaceDetector::resizedWidth() const { return m_resizedWidth; }

bool VideoFaceDetector::isFaceFound() const { return m_foundFace; }

cv::Rect VideoFaceDetector::face() const {
  cv::Rect faceRect = m_trackedFace;
  faceRect.x = static_cast<int>(faceRect.x / m_scale);
  faceRect.y = static_cast<int>(faceRect.y / m_scale);
  faceRect.width = static_cast<int>(faceRect.width / m_scale);
  faceRect.height = static_cast<int>(faceRect.height / m_scale);
  return faceRect;
}

std::string VideoFaceDetector::name() const { return m_name; }

cv::Point VideoFaceDetector::facePosition() const {
  cv::Point facePos;
  facePos.x = static_cast<int>(m_facePosition.x / m_scale);
  facePos.y = static_cast<int>(m_facePosition.y / m_scale);
  return facePos;
}

void VideoFaceDetector::setTemplateMatchingMaxDuration(const double s) {
  m_templateMatchingMaxDuration = s;
}

double VideoFaceDetector::templateMatchingMaxDuration() const {
  return m_templateMatchingMaxDuration;
}

VideoFaceDetector::~VideoFaceDetector() {
  if (m_faceCascade != nullptr)
    delete m_faceCascade;

  //  if (m_cli != nullptr)
  //    delete m_cli;

  delete faiss_index;
  delete quantizer;
}

cv::Rect VideoFaceDetector::doubleRectSize(const cv::Rect &inputRect,
                                           const cv::Rect &frameSize) const {
  cv::Rect outputRect;
  // Double rect size
  outputRect.width = inputRect.width * 2;
  outputRect.height = inputRect.height * 2;

  // Center rect around original center
  outputRect.x = inputRect.x - inputRect.width / 2;
  outputRect.y = inputRect.y - inputRect.height / 2;

  // Handle edge cases
  if (outputRect.x < frameSize.x) {
    outputRect.width += outputRect.x;
    outputRect.x = frameSize.x;
  }
  if (outputRect.y < frameSize.y) {
    outputRect.height += outputRect.y;
    outputRect.y = frameSize.y;
  }

  if (outputRect.x + outputRect.width > frameSize.width) {
    outputRect.width = frameSize.width - outputRect.x;
  }
  if (outputRect.y + outputRect.height > frameSize.height) {
    outputRect.height = frameSize.height - outputRect.y;
  }

  return outputRect;
}

cv::Point VideoFaceDetector::centerOfRect(const cv::Rect &rect) const {
  return cv::Point(rect.x + rect.width / 2, rect.y + rect.height / 2);
}

cv::Rect VideoFaceDetector::biggestFace(std::vector<cv::Rect> &faces) const {
  assert(!faces.empty());

  cv::Rect *biggest = &faces[0];
  for (auto &face : faces) {
    if (face.area() < biggest->area())
      biggest = &face;
  }
  return *biggest;
}

/*
 * Face template is small patch in the middle of detected face.
 */
cv::Mat VideoFaceDetector::getFaceTemplate(const cv::Mat &frame,
                                           cv::Rect face) {
  face.x += face.width / 4;
  face.y += face.height / 4;
  face.width /= 2;
  face.height /= 2;

  cv::Mat faceTemplate = frame(face).clone();
  return faceTemplate;
}

void VideoFaceDetector::limitRect(const cv::Mat &frame, cv::Rect &rect) {
  if (rect.x < 0)
    rect.x = 0;
  if (rect.y < 0)
    rect.y = 0;
  if (rect.x + rect.width > frame.cols)
    rect.width = frame.cols - rect.x;
  if (rect.y + rect.height > frame.rows)
    rect.height = frame.rows - rect.y;
}

float VideoFaceDetector::search_face_local(const std::vector<float> &xq) {

  auto *I = new long[1];
  auto *D = new float[1];

  //  std::cout << "before search" << std::endl;

  faiss_index->search(1, xq.data(), 1, D, I);

  return D[0];
}

void VideoFaceDetector::reindex() {

  if (nb < 1)
    return;
  if (faiss_index != nullptr) {
    delete faiss_index;
    delete quantizer;
  }

  quantizer = new faiss::IndexFlatL2(face_dim);
  faiss_index = new faiss::IndexIVFFlat(quantizer, face_dim, 1);
  faiss_index->nprobe = 1;

  auto data = xb.data();

  // here we specify METRIC_L2, by default it performs inner-product search
  faiss_index->train(nb, data);
  faiss_index->add(nb, data);
}

void VideoFaceDetector::searchFaceChip(const cv::Mat &chip_frame) {
  static std::vector<float> xq(face_dim);

  if (chip_frame.empty())
    return;

  //  std::cout << "chip_frame" << chip_frame.cols << std::endl;

  //  cv::imshow("test", chip_frame);

  // later will be multiple face_chips as well
  matrix<rgb_pixel> face_chip;
  assign_image(face_chip, cv_image<bgr_pixel>(chip_frame));
  auto &face_descriptor = net(face_chip);

  if (xb.empty()) {
    copy(face_descriptor.begin(), face_descriptor.end(), back_inserter(xb));
    nb = 1;
    cv::imwrite(m_name + "/1.jpg", chip_frame);
    reindex();
  } else {
    //  json data = net(face_chip);

    xq.assign(face_descriptor.begin(), face_descriptor.end());

    auto conf = search_face_local(xq);
    if (conf > 0.2f) {
      nb++;
      cv::imshow("test", chip_frame);
      std::cout << "num points: " << nb << " confidence " << conf << std::endl;
      copy(face_descriptor.begin(), face_descriptor.end(), back_inserter(xb));
      cv::imwrite(m_name + "/" + std::to_string(nb) + ".jpg", chip_frame);
      reindex();
    }
  }

  //  auto res = m_cli->Post("/search_face", data.dump(), "application/json");
  //  if (res && res->status == 200) {
  //    data = json::parse(res->body);
  //    auto first = data[0];
  //    if (first["confidence"] > 0.8f) {
  //      return first["name"];
  //    }
  //  }

  //  return "unknow";
}

void VideoFaceDetector::detectFaceAllSizes(const cv::Mat &frame) {
  // Minimum face size is 1/5th of screen height
  // Maximum face size is 2/3rds of screen height
  m_faceCascade->detectMultiScale(
      frame, m_allFaces, 1.1, 3, 0, cv::Size(frame.rows / 5, frame.rows / 5),
      cv::Size(frame.rows * 2 / 3, frame.rows * 2 / 3));

  if (m_allFaces.empty())
    return;

  m_foundFace = true;

  // Locate biggest face
  m_trackedFace = biggestFace(m_allFaces);

  // Copy face template
  m_faceTemplate = getFaceTemplate(frame, m_trackedFace);

  // Calculate roi
  m_faceRoi =
      doubleRectSize(m_trackedFace, cv::Rect(0, 0, frame.cols, frame.rows));

  // Update face position
  m_facePosition = centerOfRect(m_trackedFace);
}

void VideoFaceDetector::detectFaceAroundRoi(const cv::Mat &frame) {
  // Detect faces sized +/-20% off biggest face in previous search
  m_faceCascade->detectMultiScale(
      frame(m_faceRoi), m_allFaces, 1.1, 3, 0,
      cv::Size(m_trackedFace.width * 8 / 10, m_trackedFace.height * 8 / 10),
      cv::Size(m_trackedFace.width * 12 / 10, m_trackedFace.width * 12 / 10));

  if (m_allFaces.empty()) {
    // Activate template matching if not already started and start timer
    m_templateMatchingRunning = true;
    if (m_templateMatchingStartTime == 0)
      m_templateMatchingStartTime = cv::getTickCount();
    return;
  }

  // Turn off template matching if running and reset timer
  m_templateMatchingRunning = false;
  m_templateMatchingCurrentTime = m_templateMatchingStartTime = 0;

  // Get detected face
  m_trackedFace = biggestFace(m_allFaces);

  // Add roi offset to face
  m_trackedFace.x += m_faceRoi.x;
  m_trackedFace.y += m_faceRoi.y;

  // Get face template
  m_faceTemplate = getFaceTemplate(frame, m_trackedFace);

  // Calculate roi
  m_faceRoi =
      doubleRectSize(m_trackedFace, cv::Rect(0, 0, frame.cols, frame.rows));

  // Update face position
  m_facePosition = centerOfRect(m_trackedFace);
}

void VideoFaceDetector::detectFacesTemplateMatching(const cv::Mat &frame) {
  // Calculate duration of template matching
  m_templateMatchingCurrentTime = cv::getTickCount();
  double duration = static_cast<double>(m_templateMatchingCurrentTime -
                                        m_templateMatchingStartTime) /
                    TICK_FREQUENCY;

  // If template matching lasts for more than 2 seconds face is possibly lost
  // so disable it and redetect using cascades
  if (duration > m_templateMatchingMaxDuration) {
    m_foundFace = false;
    m_templateMatchingRunning = false;
    m_templateMatchingStartTime = m_templateMatchingCurrentTime = 0;
    m_facePosition.x = m_facePosition.y = 0;
    m_trackedFace.x = m_trackedFace.y = m_trackedFace.width =
        m_trackedFace.height = 0;
    return;
  }

  // Edge case when face exits frame while
  if (m_faceTemplate.rows * m_faceTemplate.cols == 0 ||
      m_faceTemplate.rows <= 1 || m_faceTemplate.cols <= 1) {
    m_foundFace = false;
    m_templateMatchingRunning = false;
    m_templateMatchingStartTime = m_templateMatchingCurrentTime = 0;
    m_facePosition.x = m_facePosition.y = 0;
    m_trackedFace.x = m_trackedFace.y = m_trackedFace.width =
        m_trackedFace.height = 0;
    return;
  }

  // Template matching with last known face
  // cv::matchTemplate(frame(m_faceRoi), m_faceTemplate, m_matchingResult,
  // CV_TM_CCOEFF);
  cv::matchTemplate(frame(m_faceRoi), m_faceTemplate, m_matchingResult,
                    cv::TM_SQDIFF_NORMED);
  cv::normalize(m_matchingResult, m_matchingResult, 0, 1, cv::NORM_MINMAX, -1,
                cv::Mat());
  double min, max;
  cv::Point minLoc, maxLoc;
  cv::minMaxLoc(m_matchingResult, &min, &max, &minLoc, &maxLoc);

  // Add roi offset to face position
  minLoc.x += m_faceRoi.x;
  minLoc.y += m_faceRoi.y;

  // Get detected face
  // m_trackedFace = cv::Rect(maxLoc.x, maxLoc.y, m_trackedFace.width,
  // m_trackedFace.height);
  m_trackedFace =
      cv::Rect(minLoc.x, minLoc.y, m_faceTemplate.cols, m_faceTemplate.rows);
  m_trackedFace =
      doubleRectSize(m_trackedFace, cv::Rect(0, 0, frame.cols, frame.rows));

  // Get new face template
  m_faceTemplate = getFaceTemplate(frame, m_trackedFace);

  // Calculate face roi
  m_faceRoi =
      doubleRectSize(m_trackedFace, cv::Rect(0, 0, frame.cols, frame.rows));

  // Update face position
  m_facePosition = centerOfRect(m_trackedFace);
}

cv::Point VideoFaceDetector::getFrameAndDetect(cv::Mat &frame) {
  *m_videoCapture >> frame;

  // Downscale frame to m_resizedWidth width - keep aspect ratio
  m_scale =
      static_cast<double>(std::min(m_resizedWidth, frame.cols)) / frame.cols;
  cv::Size resizedFrameSize = cv::Size(static_cast<int>(m_scale * frame.cols),
                                       static_cast<int>(m_scale * frame.rows));

  if (frame.empty() || resizedFrameSize.empty())
    return cv::Point(0, 0);

  cv::Mat resizedFrame;
  cv::resize(frame, resizedFrame, resizedFrameSize);

  if (!m_foundFace) {
    detectFaceAllSizes(resizedFrame); // Detect using cascades over whole image
  } else {
    detectFaceAroundRoi(resizedFrame); // Detect using cascades only in ROI
    if (m_templateMatchingRunning) {
      detectFacesTemplateMatching(
          resizedFrame); // Detect using template matching
    }
  }

  if (m_learn) {

    // search face chip with this face to update label
    auto trackedFace =
        cv::Rect(static_cast<int>(m_trackedFace.x / m_scale),
                 static_cast<int>(m_trackedFace.y / m_scale),
                 static_cast<int>(m_trackedFace.width / m_scale),
                 static_cast<int>(m_trackedFace.height / m_scale));
    limitRect(frame, trackedFace);
    if (trackedFace.empty())
      return cv::Point(0, 0);

    cv::Mat chip_frame;
    cv::resize(frame(trackedFace), chip_frame, cv::Size(150, 150));
    searchFaceChip(chip_frame);
  }

  return m_facePosition;
}

cv::Point VideoFaceDetector::operator>>(cv::Mat &frame) {
  return this->getFrameAndDetect(frame);
}


//  Copyright (c) 2021 DNV AS
//
//  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

#include <jsScript/jsQuantity.h>
#include <jsScript/jsUnitValue.h>
#include <jsScript/jsUnit.h>
#include <jsScript/jsClass.h>

#include <sstream>
#include <limits>
#include "Units/Runtime/DynamicQuantity.h"
#include "Units/Runtime/UnitParser.h"
#include <boost/spirit/include/qi.hpp>
#include <boost/spirit/include/phoenix_core.hpp>
#include <boost/spirit/include/phoenix_operator.hpp>
#include "Reflection/Classes/Class.h"
#include "Units/Formatting/IUnitFormatterService.h"
#include "Units/Formatting/DimensionFormatter.h"
#include "Services/ServiceProvider.h"

jsQuantity::formatFlags jsQuantity::m_string_operator_format = includeUnitString;
using namespace DNVS::MoFa::Units::Runtime;
using namespace DNVS::MoFa::Units;

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

jsBoundDouble jsBoundDouble::operator-()
{
    return jsBoundDouble(-m_value, m_inclusive);
}

jsBoundDouble& jsBoundDouble::operator+()
{
    return *this;
}

jsBoundDouble jsBoundDouble::infinity()
{
    return jsBoundDouble(std::numeric_limits<double>::infinity());
}

std::mutex jsQuantity::m_mutex;

std::mutex& jsQuantity::GetMutex()
{
    return m_mutex;
}

jsQuantity::jsQuantity(const jsQuantity& quantity)
    : m_quantity(quantity.m_quantity)
    , m_phenomenon(quantity.m_phenomenon)
{
}

jsQuantity::jsQuantity(const DNVS::MoFa::Units::Runtime::DynamicPhenomenon& phenomenon, double value, const std::string& unitName)
    : m_phenomenon(phenomenon)
    , m_quantity(value, unitName)
{
    ThrowIfQuantityAndPhenomenonAreIncompatible();
}

jsQuantity::jsQuantity(const DNVS::MoFa::Units::Runtime::DynamicPhenomenon& phenomenon)
    : m_phenomenon(phenomenon)
{
}

jsQuantity::jsQuantity(const DNVS::MoFa::Units::Runtime::DynamicPhenomenon& phenomenon, const std::string& quantity)
    : m_phenomenon(phenomenon)
{
    std::lock_guard<std::mutex> guard(GetMutex());
    ///We are using boost::spirit for parsing the quantity here for performance reasons. It is faster than std::stringstream.
    ///It also handles NaN and Infinity correctly.
    using boost::spirit::qi::double_;
    using boost::spirit::qi::char_;
    using boost::spirit::qi::phrase_parse;
    using boost::spirit::qi::_1;
    using boost::spirit::qi::space;

    double value;
    std::vector<char> unitVect;
    if (phrase_parse(quantity.begin(), quantity.end(), double_[boost::phoenix::ref(value) = _1] >> (*char_)[boost::phoenix::ref(unitVect) = _1], space))
    {
        if (unitVect.empty())
            m_quantity = DynamicQuantity(value, GetInputUnitSystem().GetUnit(m_phenomenon));
        else
        {
            std::string unit(&unitVect.front(), unitVect.size());
            m_quantity = DynamicQuantity(value, unit);
            ThrowIfQuantityAndPhenomenonAreIncompatible();
        }
    }
    else
        throw bad_type_conversion(quantity, phenomenon.GetName());
}

jsQuantity::jsQuantity(const DynamicPhenomenon& phenomenon, const DynamicQuantity& quantity)
    : m_phenomenon(phenomenon)
    , m_quantity(quantity)
{
    m_quantity.TryChangeUnit(GetDatabaseUnitSystem().GetUnit(phenomenon));
}

jsQuantity& jsQuantity::operator=(const jsQuantity& quantity)
{
    m_phenomenon = quantity.m_phenomenon;
    m_quantity = quantity.m_quantity;
    return *this;
}

jsQuantity::jsQuantity(double value)
    : m_quantity(value)
{
}

jsQuantity::jsQuantity(const DNVS::MoFa::Units::Runtime::DynamicPhenomenon& phenomenon, double value, bool databaseUnits)
    : m_phenomenon(phenomenon)
{
    if (databaseUnits)
    {
        m_quantity = DynamicQuantity(value, SimplifiedUnit(1.0, phenomenon.GetDimension()));
    }
    else
    {
        UnitSystem unitSystem = GetInputUnitSystem();
        Unit unit = unitSystem.GetUnit(m_phenomenon);
        m_quantity = DynamicQuantity(value, unit);
    }
}

jsQuantity::jsQuantity(double value, const std::string& unit)
    : m_quantity(value, unit)
{
    auto phenomenons = DynamicPhenomenon::GetCompatiblePhenomenons(m_quantity.GetSimplifiedUnit().GetDimension());
    if (phenomenons.size() == 1)
        m_phenomenon = phenomenons.front();
    else
        throw std::runtime_error("Unknown phenomenon for " + unit);
}

jsQuantity::jsQuantity(const DNVS::MoFa::Units::Runtime::DynamicPhenomenon& phenomenon, const jsUnitValue& value)
    : m_phenomenon(phenomenon)
    , m_quantity(value)
{
    ThrowIfQuantityAndPhenomenonAreIncompatible();
}

jsQuantity::~jsQuantity()
{

}

void jsQuantity::init(jsTypeLibrary& typeLibrary)
{
    jsTClass<jsQuantity> cls(typeLibrary, "Quantity");
    if (cls.reinit()) return;
    cls.Function("toString", &jsQuantity::operator std::string);
    cls.Function("signature", &jsQuantity::signature);
    cls.Function("toDouble", &jsQuantity::toDouble);
    cls.ImplicitConversion(&jsQuantity::operator std::string);
    cls.ImplicitConversion(&jsQuantity::operator const DynamicQuantity&);

    using namespace DNVS::MoFa::Reflection::Classes;
    Class<jsQuantity> cls2(jsStack::stack()->GetTypeLibrary(), "");
    cls2.Function("toString", &jsQuantity::operator std::string);
}

jsQuantity::operator const DynamicQuantity&() const
{
    if (!m_quantity.GetSimplifiedUnit().HasUnitName())
    {
        Unit unit = GetDatabaseUnit();
        const_cast<jsQuantity*>(this)->m_quantity = DynamicQuantity(m_quantity.GetNeutralValue(), unit);
    }
    return m_quantity;
}

std::string jsQuantity::signature() const
{
    return Formatting::DimensionFormatter().FormatAsHtml(m_phenomenon.GetDimension());
}

jsQuantity::operator double() const
{
    return m_quantity.GetNeutralValue();
}

Unit jsQuantity::GetInputUnit() const
{
    return GetInputUnitSystem().GetUnit(m_phenomenon);
}

Unit jsQuantity::GetDatabaseUnit() const
{
    return GetDatabaseUnitSystem().GetUnit(m_phenomenon);
}

jsQuantity::operator std::string() const
{
    return format(10, std::ios_base::floatfield, m_string_operator_format);
}

const DynamicQuantity& jsQuantity::GetQuantity() const
{
    return m_quantity;
}

const DynamicPhenomenon& jsQuantity::GetPhenomenon() const
{
    return m_phenomenon;
}

jsValue* jsQuantity::operator-()
{
    return new jsUnitValue(-m_quantity);
}

jsValue* jsQuantity::operator==(jsValue* op2)
{
    double compatibleNumber = 0;
    if (!getCompatibleNumber(op2, compatibleNumber)) return jsScriptable::operator==(op2);
    if (_isnan(*this) || _isnan(compatibleNumber)) return toJScript(false);
    bool result = jsQuantity::compareTolerant(*this, compatibleNumber) == 0;
    return toJScript(result);
}

jsValue* jsQuantity::operator!=(jsValue* op2)
{
    double compatibleNumber = 0;
    if (!getCompatibleNumber(op2, compatibleNumber)) return jsScriptable::operator==(op2);
    if (_isnan(*this) || _isnan(compatibleNumber)) return toJScript(true);
    bool result = jsQuantity::compareTolerant(*this, compatibleNumber) != 0;
    return toJScript(result);
}

jsValue* jsQuantity::operator<=(jsValue* op2)
{
    double compatibleNumber = 0;
    if (!getCompatibleNumber(op2, compatibleNumber)) return jsScriptable::operator==(op2);
    bool result = jsQuantity::compareTolerant(*this, compatibleNumber) <= 0;
    return toJScript(result);
}

jsValue* jsQuantity::operator>=(jsValue* op2)
{
    double compatibleNumber = 0;
    if (!getCompatibleNumber(op2, compatibleNumber)) return jsScriptable::operator==(op2);
    bool result = jsQuantity::compareTolerant(*this, compatibleNumber) >= 0;
    return toJScript(result);
}

jsValue* jsQuantity::operator<(jsValue* op2) {
    double compatibleNumber = 0;
    if (!getCompatibleNumber(op2, compatibleNumber)) return jsScriptable::operator==(op2);
    bool result = jsQuantity::compareTolerant(*this, compatibleNumber) < 0;
    return toJScript(result);
}

jsValue* jsQuantity::operator>(jsValue* op2)
{
    double compatibleNumber = 0;
    if (!getCompatibleNumber(op2, compatibleNumber)) return jsScriptable::operator==(op2);
    bool result = jsQuantity::compareTolerant(*this, compatibleNumber) > 0;
    return toJScript(result);
}

bool jsQuantity::getCompatibleNumber(jsValue* op2, double& number)
{
    try {
        DynamicQuantity otherQuantity = fromJScript(op2, jsType<DynamicQuantity>());
        DynamicQuantity thisQuantity = *this;
        otherQuantity.ChangeUnit(thisQuantity.GetSimplifiedUnit());
        number = otherQuantity.GetValue();
        return true;
    }
    catch(...)
    {
        return false;
    }
}

void jsQuantity::ThrowIfQuantityAndPhenomenonAreIncompatible()
{
    if (!m_quantity.IsCompatible(m_phenomenon))
        throw std::runtime_error("Invalid unit: '" + m_quantity.GetSimplifiedUnit().GetUnitName() + "' is not a valid " + m_phenomenon.GetName() + " unit");
}

jsValue* jsQuantity::operator+(jsValue* op2)
{
    try {
        DynamicQuantity otherQuantity = fromJScript(op2, jsType<DynamicQuantity>());
        DynamicQuantity thisQuantity = *this;
        return new jsUnitValue(thisQuantity + otherQuantity);
    }
    catch(...) {}
    return jsValue::operator+(op2);
}

jsValue* jsQuantity::operator-(jsValue* op2)
{
    try {
        DynamicQuantity otherQuantity = fromJScript(op2, jsType<DynamicQuantity>());
        DynamicQuantity thisQuantity = *this;
        return new jsUnitValue(thisQuantity - otherQuantity);
    }
    catch (...) {}
    return jsValue::operator-(op2);
}

jsValue* jsQuantity::operator*(jsValue* op2)
{
    try {
        DynamicQuantity otherQuantity = fromJScript(op2, jsType<DynamicQuantity>());
        DynamicQuantity thisQuantity = *this;
        DynamicQuantity result = thisQuantity * otherQuantity;
        if (result.RemoveUnitIfDimensionLess())
            return toJScript(result.GetValue());
        else
            return new jsUnitValue(result);
    }
    catch (...) {}
    try {
        double value = fromJScript(op2, jsType<double>());
        DynamicQuantity thisQuantity = *this;
        return new jsUnitValue(thisQuantity * value);
    }
    catch (...) {}
    return jsValue::operator*(op2);
}

jsValue* jsQuantity::InverseDivide(jsValue* op2)
{
    try {
        double value = fromJScript(op2, jsType<double>());
        DynamicQuantity thisQuantity = *this;
        return new jsUnitValue(value / thisQuantity);
    }
    catch (...) {}
    return jsValue::InverseDivide(op2);
}

jsValue* jsQuantity::operator/(jsValue* op2)
{
    try {
        DynamicQuantity otherQuantity = fromJScript(op2, jsType<DynamicQuantity>());
        DynamicQuantity thisQuantity = *this;
        DynamicQuantity result = thisQuantity / otherQuantity;
        if (result.RemoveUnitIfDimensionLess())
            return toJScript(result.GetValue());
        else
            return new jsUnitValue(result);
    }
    catch (...) {}
    try {
        double value = fromJScript(op2, jsType<double>());
        DynamicQuantity thisQuantity = *this;
        return new jsUnitValue(thisQuantity / value);
    }
    catch (...) {}
    return jsValue::operator/(op2);
}

std::string jsQuantity::format(formatFlags format, unitType unitTypeFlag) const
{
    int precision = 10;
    std::ios::fmtflags floatFlag = std::ios_base::floatfield;
    auto service = DNVS::MoFa::Services::ServiceProvider::Instance().TryGetService<Formatting::IUnitFormatterService>();
    if (service)
    {
        precision = service->GetPrecision(m_phenomenon);
        floatFlag = service->GetFloatFlags(m_phenomenon);
    }
    return this->format(precision, floatFlag, format, unitTypeFlag);
}

std::string FormatUsingStream(std::ios_base::fmtflags flags, size_t precision, double number)
{
    std::stringstream stream;
    stream.precision(precision);
    if ((flags&std::ios_base::floatfield) == std::ios_base::floatfield)
        flags = (flags)&~std::ios_base::floatfield;
    stream.setf(flags);
    stream << number;
    return stream.str();
}

std::string FormatUsingSprintF(std::ios_base::fmtflags flags, size_t precision, double number)
{
    std::string format = "%.*";
    switch (flags&std::ios_base::floatfield)
    {
    case std::ios_base::floatfield:
        format += "g";
        break;
    case std::ios_base::fixed:
        format += "f";
        break;
    case std::ios_base::scientific:
        format += "e";
        break;
    }
    char numberAsString[200];
    sprintf_s(numberAsString, 200, format.c_str(), int(precision), number);
    return numberAsString;
}

std::string jsQuantity::format(int precision, std::ios_base::fmtflags flags, formatFlags format, unitType unitTypeFlag) const
{
    double number = value(unitTypeFlag);
    if (_isnan(number)) return "NaN";
    if (!_finite(number)) {
        if (number > 0) return "+Infinity";
        else return "-Infinity";
    }
    if (fabs(m_quantity.GetNeutralValue()) < 1e-15)
        number = 0;
    std::string formattedNumber = FormatUsingSprintF(flags, precision, number);
    if (format == includeUnitString) {
        formattedNumber += " " + unitString(unitTypeFlag);
    }
    return formattedNumber;
}

double jsQuantity::value(unitType unitTypeFlag) const
{
    if (unitTypeFlag == databaseUnit) return m_quantity.GetNeutralValue();
    else return GetInputUnitSystem().Convert(m_phenomenon, m_quantity);
}

std::string jsQuantity::unitString(unitType unitTypeFlag) const
{
    if (unitTypeFlag == databaseUnit) {
        return GetDatabaseUnitSystem().GetUnit(m_phenomenon).GetUnitName();
    }
    else {
        return GetInputUnitSystem().GetUnit(m_phenomenon).GetUnitName();
    }
}

DynamicDimension jsQuantity::GetDimension() const
{
    return m_phenomenon.GetDimension();
}

double jsQuantity::toDouble() const
{
    return value();
}

void jsQuantity::checkRange(const jsBoundDouble& lower_bound, const jsBoundDouble& upper_bound)
{
    if ((lower_bound.inclusive() && double(*this) < double(lower_bound)) ||
        (double(*this) <= double(lower_bound)) ||
        (upper_bound.inclusive() && double(*this) > double(upper_bound)) ||
        (double(*this) >= double(upper_bound)))
    {
        std::string error = m_phenomenon.GetName() + " value: " + format() + " out of range ";
        if (lower_bound.inclusive()) error += "[";
        else error += "<";
        error += mofa::lexical_cast<std::string>(double(lower_bound));
        error += ",";
        error += mofa::lexical_cast<std::string>(double(upper_bound));
        if (upper_bound.inclusive()) error += "]";
        else error += ">";
        throw std::range_error(error.c_str());
    }
}

double jsQuantity::convert(const std::string& unitName) const
{
    DynamicQuantity q(m_quantity);
    q.ChangeUnit(unitName);
    return q.GetValue();
}

jsQuantity::formatFlags jsQuantity::stringOperatorFormat(formatFlags operator_format)
{
    // Return value is previous operator format
    formatFlags result = m_string_operator_format;
    m_string_operator_format = operator_format;
    return result;
}

bool jsQuantity::compatible(const jsQuantity& quantity)
{
    if (!m_phenomenon.IsValid()) return false;
    if (!quantity.m_phenomenon.IsValid()) return false;
    return m_quantity.IsCompatible(quantity.m_quantity);
}

int jsQuantity::compareTolerant(const double& a1, const double& a2)
{
    if (a1 == 0.0 && a2 == 0.0) return 0;

    if (fabs(a1) == std::numeric_limits<double>::infinity() || fabs(a2) == std::numeric_limits<double>::infinity()) {
        if (a1 == a2) {
            return 0;
        }
        else if (a1 == -std::numeric_limits<double>::infinity() || a2 == std::numeric_limits<double>::infinity()) {
            return -1;
        }
        else {
            return 1;
        }
    }

    double maximum = (std::max)(fabs(a1), fabs(a2));
    double normalised_a1 = a1 / maximum;
    double normalised_a2 = a2 / maximum;

    if (fabs(normalised_a1 - normalised_a2) < 1e-10) return 0;
    else if (normalised_a1 < normalised_a2) return -1;
    else return 1;
}


// ====================================================================
// This file is part of FlexibleSUSY.
//
// FlexibleSUSY is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published
// by the Free Software Foundation, either version 3 of the License,
// or (at your option) any later version.
//
// FlexibleSUSY is distributed in the hope that it will be useful, but
// WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with FlexibleSUSY.  If not, see
// <http://www.gnu.org/licenses/>.
// ====================================================================

/**
 * @file rkf_integrator.cpp
 * @brief Implementation of the RKF_integrator class
 */

#include "rkf_integrator.hpp"
#include "logger.hpp"
#include "wrappers.hpp"

#include "config.h"

#ifdef ENABLE_ODEINT

#include <boost/numeric/odeint/algebra/algebra_dispatcher.hpp>
#include <boost/numeric/odeint/algebra/vector_space_algebra.hpp>
#include <boost/numeric/odeint/external/eigen/eigen_algebra_dispatcher.hpp>
#include <boost/numeric/odeint/external/eigen/eigen_resize.hpp>
#include <boost/numeric/odeint/integrate/integrate_adaptive.hpp>
#include <boost/numeric/odeint/stepper/generation.hpp>
#include <boost/numeric/odeint/stepper/runge_kutta_fehlberg78.hpp>

namespace boost {
namespace numeric {
namespace odeint {

template <typename Scalar, int R, int C, int O, int MR, int MC>
struct vector_space_norm_inf<Eigen::Array<Scalar,R,C,O,MR,MC> > {
   using result_type = typename Eigen::NumTraits<Scalar>::Real;
   result_type operator()(const Eigen::Array<Scalar,R,C,O,MR,MC>& x) const
      {
         return x.cwiseAbs().maxCoeff();
      }
};

} // namespace odeint
} // namespace numeric
} // namespace boost

namespace flexiblesusy {

namespace runge_kutta {

/**
 * The vector of the initial values of the parameters is
 * updated so that after calling this function, this vector contains
 * the updated values of the parameters at the end-point of the
 * integration.
 *
 * @param[in] start initial value of the independent variable
 * @param[in] end final value of the independent variable
 * @param[inout] pars initial values of the parameters
 * @param[in] derivs function calculating the derivatives
 * @param[in] tol desired accuracy to use in integration step
 */
void RKF_integrator::operator()(double start, double end,
                                Eigen::ArrayXd& pars, const Derivs& derivs,
                                double tol) const
{
   using state_type = Eigen::ArrayXd;
   using stepper_type = boost::numeric::odeint::runge_kutta_fehlberg78<
      state_type, double, state_type, double,
      boost::numeric::odeint::vector_space_algebra
      >;

   const double guess = (end - start) * 0.1; // first step size
   const auto derivatives = [derivs] (const state_type& y, state_type& dydt, double t) -> void {
      dydt = derivs(t, y);
   };

   const auto stepper = boost::numeric::odeint::make_controlled(tol, tol, stepper_type());
   boost::numeric::odeint::integrate_adaptive(
      stepper, derivatives, pars, start, end, guess, RKF_observer());
}

void RKF_integrator::RKF_observer::operator()(const Eigen::ArrayXd& state, double t) const
{
   if (!IsFinite(state)) {
      int max_step_dir = 0;
      for (int i = 0; i < state.size(); ++i) {
         if (!IsFinite(state(i))) {
            max_step_dir = i;
            break;
         }
      }
#ifdef ENABLE_VERBOSE
      ERROR("RKF_integrator: non-perturbative running at Q = "
            << std::exp(t) << " GeV of parameter y(" << max_step_dir
            << ") = " << state(max_step_dir));
#endif
      throw NonPerturbativeRunningError(std::exp(t), max_step_dir, state(max_step_dir));
   }
}

} // namespace runge_kutta

} // namespace flexiblesusy

#else

namespace flexiblesusy {

namespace runge_kutta {

/**
 * The vector of the initial values of the parameters is
 * updated so that after calling this function, this vector contains
 * the updated values of the parameters at the end-point of the
 * integration.
 *
 * @param[in] start initial value of the independent variable
 * @param[in] end final value of the independent variable
 * @param[inout] pars initial values of the parameters
 * @param[in] derivs function calculating the derivatives
 * @param[in] tol desired accuracy to use in integration step
 */
void RKF_integrator::operator()(double, double, Eigen::ArrayXd&, const Derivs&,
                                double) const
{
   throw DisabledOdeintError("Cannot call operator(), because odeint support is disabled.");
}

} // namespace runge_kutta

} // namespace flexiblesusy

#endif


#include <booster/booster.h>
#include <booster/helper.h>

#include "utils.h"
// #define TEST_SGECONV
int test_general_conv_kernels(int output_channels, int input_channels, int input_h, int input_w, int kernel_h, int kernel_w, int stride, int pad)
{
    booster::ConvParam conv_param;
    // conv_param
    conv_param.output_channels = output_channels;
    conv_param.input_channels = input_channels;
    conv_param.input_h = input_h;
    conv_param.input_w = input_w;
    conv_param.kernel_h = kernel_h;
    conv_param.kernel_w = kernel_w;
    conv_param.stride_h = stride;
    conv_param.stride_w = stride;
    conv_param.pad_left = pad;
    conv_param.pad_bottom = pad;
    conv_param.pad_right = pad;
    conv_param.pad_top = pad;
    conv_param.group = 1;
    conv_param.bias_term = true;
    conv_param.AssignOutputDim();
    conv_param.activation = booster::None;
    conv_param.LogParams("TEST");

    float* kernel_data = (float*) malloc(sizeof(float) * conv_param.kernel_h * conv_param.kernel_w * conv_param.input_channels * conv_param.output_channels);
    rand_fill<float>(kernel_data, conv_param.kernel_h * conv_param.kernel_w * conv_param.input_channels * conv_param.output_channels);
    // float* naive_processed_kernel = (float*) malloc(sizeof(float) * conv_param.kernel_h * conv_param.kernel_w * conv_param.input_channels * conv_param.output_channels);
    // float* im2col_processed_kernel = (float*) malloc(sizeof(float) * conv_param.kernel_h * conv_param.kernel_w * conv_param.input_channels * conv_param.output_channels);

    float* input_data = (float*) malloc(sizeof(float) * conv_param.input_channels * conv_param.input_h * conv_param.input_w);
    rand_fill<float>(input_data, conv_param.input_channels * conv_param.input_h * conv_param.input_w);
    float* output_data_1 = (float*) malloc(sizeof(float) * conv_param.output_channels * conv_param.output_h * conv_param.output_w);
    float* output_data_2 = (float*) malloc(sizeof(float) * conv_param.output_channels * conv_param.output_h * conv_param.output_w);
    float* output_data_3 = (float*) malloc(sizeof(float) * conv_param.output_channels * conv_param.output_h * conv_param.output_w);

    float* bias_data = (float*) malloc(sizeof(float) * conv_param.output_channels);

    rand_fill<float>(bias_data, conv_param.output_channels);
    //Initialize
    printf("Initializing naive...\n");
    booster::ConvBooster naive_booster;
    naive_booster.ForceSelectAlgo(booster::NAIVE);
    int buffer_size = 0;
    int processed_kernel_size = 0;
    naive_booster.GetBufferSize(&conv_param, &buffer_size, &processed_kernel_size);
    float* naive_processed_kernel = (float*) malloc(sizeof(float) * processed_kernel_size);
    float* naive_buffer = (float*) malloc(sizeof(float) * buffer_size);
    naive_booster.Init(&conv_param, naive_processed_kernel, kernel_data);

    printf("Initializing im2col...\n");
    booster::ConvBooster im2col_booster;
    im2col_booster.ForceSelectAlgo(booster::IM2COL);
    im2col_booster.GetBufferSize(&conv_param, &buffer_size, &processed_kernel_size);
    float* im2col_processed_kernel = (float*) malloc(sizeof(float) * processed_kernel_size);
    float* im2col_buffer = (float*) malloc(sizeof(float) * buffer_size);
    im2col_booster.Init(&conv_param, im2col_processed_kernel, kernel_data);

#ifdef TEST_SGECONV
    printf("Initializing sgeconv...\n");
    booster::ConvBooster sgeconv_booster;
    sgeconv_booster.ForceSelectAlgo(booster::SGECONV);
    sgeconv_booster.GetBufferSize(&conv_param, &buffer_size, &processed_kernel_size);
    float* sgeconv_processed_kernel = (float*) malloc(sizeof(float) * processed_kernel_size);
    float* sgeconv_buffer = (float*) malloc(sizeof(float) * buffer_size);
    sgeconv_booster.Init(&conv_param, sgeconv_processed_kernel, kernel_data);
#endif

    //Forward
    printf("Forward naive...\n");
    naive_booster.Forward(&conv_param, output_data_1, input_data, naive_processed_kernel, naive_buffer, bias_data);
    printf("Forward im2col...\n");
    im2col_booster.Forward(&conv_param, output_data_2, input_data, im2col_processed_kernel, im2col_buffer, bias_data);

#ifdef TEST_SGECONV
    printf("Forward sgeconv...\n");
    sgeconv_booster.Forward(&conv_param, output_data_3, input_data, sgeconv_processed_kernel, sgeconv_buffer, bias_data);
#endif

    //Check results
    diff(output_data_1, output_data_2, conv_param.output_channels * conv_param.output_w * conv_param.output_h);
#ifdef TEST_SGECONV
    diff(output_data_1, output_data_3, conv_param.output_channels * conv_param.output_w * conv_param.output_h);
#endif
    //Cleanup
    free(naive_processed_kernel);
    free(naive_buffer);
    free(im2col_processed_kernel);
    free(im2col_buffer);
#ifdef TEST_SGECONV
    free(sgeconv_processed_kernel);
    free(sgeconv_buffer);
#endif
    free(kernel_data);
    free(input_data);
    free(output_data_1);
    free(output_data_2);
    free(output_data_3);
    free(bias_data);

    return 0;
}

int main()
{
    test_general_conv_kernels(64, 32, 224, 224, 5, 5, 4, 1);
    return 0;
}


#ifndef RAI_QUADRUPEDCONTROLLER_HPP
#define RAI_QUADRUPEDCONTROLLER_HPP

#include <Eigen/Core>
#include "string"
#include "SimpleMLPLayer.hpp"
#include <cerberus_anymal_control/utils/IK.hpp>
#include <cerberus_anymal_control/utils/GraphLoader.hpp>
# define M_PI       3.14159265358979323846    /* pi */
# define M_PI_2        1.57079632679489661923    /* pi/2 */

namespace RAI {

class QuadrupedController {
  static constexpr int EstimationDim = 4;
  static constexpr int ObservationDim = 60;
  static constexpr int ObservationStride = 4;
  static constexpr int HistoryLength = 128;
  static constexpr int jointHistoryLength = 20;
  static constexpr unsigned int actionDimension = 16;
  static constexpr int stateDimension = 133;
  static constexpr unsigned int nStack = 40;

  typedef typename Eigen::Matrix<double, 3, 1> AngularVelocity;
  typedef typename Eigen::Matrix<double, 3, 1> LinearVelocity;
  typedef typename Eigen::Matrix<double, 4, 1> Quaternion;
  typedef typename Eigen::Matrix<double, 3, 3> RotationMatrix;
  typedef typename Eigen::Matrix<double, 4, 1> Vector4d;
  typedef typename Eigen::Matrix<double, stateDimension, 1> State;
  typedef typename Eigen::Matrix<double, actionDimension, 1> Action;
  typedef typename Eigen::Matrix<double, ObservationDim, 1> Observation;
  using Dtype = double;
  using JointSpaceVector = Eigen::Matrix<double, 12, 1>;

  typedef typename Eigen::VectorXd VectorXd;

 public:
  QuadrupedController();

  ~QuadrupedController();

  void reset(GraphLoader<float> *graph);

  void updateAction(Action action_in);

  void getDesJointPos(Eigen::Matrix<double, 12, 1> &output,
                      const Eigen::Matrix<double, 19, 1> &generalizedCoordinates,
                      const Eigen::Matrix<double, 18, 1> &generalizedVelocities,
                      double headingVel, double lateralVel, double yawrate);

  void updateHistory(const VectorXd &q,
                     const VectorXd &u);

  static void QuattoEuler(const Quaternion &q, double &roll, double &pitch, double &yaw);

 private:
  double anglediff(double target, double source);

  double anglemod(double a);

  double wrapAngle(double a);

  int fastfloor(double a);

  void conversion_GeneralizedState2LearningState(State &state,
                                                        const VectorXd &q,
                                                        const VectorXd &u);

  RotationMatrix quatToRotMat(Quaternion &q);

  State state_;
  Action action_;
  JointSpaceVector jointNominalConfig;
  State state_unscaled_; /// just memory slot for computation
  State stateOffset_;
  State stateScale_;
  Action actionOffset_;
  Action actionScale_;
  Action scaledAction_;
  Eigen::Matrix<Dtype, 3, 1> e_g;
  Eigen::Matrix<Dtype, 3, 1> x_, y_;
  Eigen::Matrix<Dtype, 3, 1> command_;
  Eigen::Matrix<Dtype, 12 * jointHistoryLength, 1> jointVelHist_, jointPosHist_, tempHist_;
  Eigen::Matrix<float, ObservationDim, nStack> historyBuffer_;
  Observation observationScale_;
  Observation observationOffset_;
  std::vector<Eigen::Matrix<double, 3, 1>> footPos_Target;
  std::vector<Eigen::Matrix<double, 3, 1>> prevfootPos_Target;
  std::vector<Eigen::Matrix<double, 3, 1>> prevfootPos_Target2;
  Eigen::Matrix<Dtype, 12, 1> footPositionOffset_;

  double h0_ = -0.5;
  double clearance_[4];

  double freqScale_ = 0.0025 * 2.0 * M_PI;
  double baseFreq_ = 0.0;

  unsigned long int controlCounter = 0;
  unsigned long int stopCounter = 0;
  unsigned long int historyUpdateCounter = 0;
  unsigned long int decimation = 8;

  InverseKinematics IK_;
  tensorflow::TensorShape state_dims_inv;
  tensorflow::Tensor state_tf_tensor;

  tensorflow::TensorShape history_dims_inv;
  tensorflow::Tensor history_tf_tensor;
  GraphLoader<float> *graph_;

 public:
  double pi_[4];
  double piD_[4];
  JointSpaceVector jointPositionTarget_;

};

}

#endif //RAI_QUADRUPEDCONTROLLER_HPP


#include <utility>

#include <sstream>

#include <gurobi_c++.h>

#include <boost/exception/diagnostic_information.hpp>

#include <gflags/gflags.h>
#include <glog/logging.h>

#include <boost/config.hpp>
#include <boost/format.hpp>
#include <boost/optional.hpp>

#include <ortools/constraint_solver/routing.h>
#include <ortools/constraint_solver/routing_parameters.pb.h>
#include <ortools/constraint_solver/routing_parameters.h>

#include "util/input.h"
#include "util/logging.h"
#include "util/validation.h"

#include "location_container.h"
#include "solver_wrapper.h"
#include "estimate_solver.h"
#include "break.h"
#include "second_step_solver.h"
#include "gexf_writer.h"

DEFINE_string(problem,
              "../problem.json", "a file path to the problem instance");
DEFINE_validator(problem, &util::file::Exists
);

DEFINE_string(human_planners_solution,
              "", "a file path to the solution file for warm start");
DEFINE_validator(human_planners_solution, &util::file::Exists
);

DEFINE_string(solution,
              "", "a file path to the solution file for warm start");
DEFINE_validator(solution, &util::file::IsNullOrExists
);

DEFINE_string(maps,
              "../data/scotland-latest.osrm", "a file path to the map");
DEFINE_validator(maps, &util::file::Exists
);

DEFINE_string(output,
              "output.gexf", "an output file");

static const auto DEFAULT_TIME_LIMIT_TEXT = "00:03:00";
static const auto DEFAULT_TIME_LIMIT = boost::posix_time::duration_from_string(DEFAULT_TIME_LIMIT_TEXT);
DEFINE_string(time_limit, DEFAULT_TIME_LIMIT_TEXT,
              "time limit for proving the optimality");

DEFINE_double(gap_limit,
              0.001, "gap limit for proving the optimality");

DEFINE_string(break_time_window,
              "00:120:00",
              "Time window for breaks");
DEFINE_validator(break_time_window, &util::time_duration::IsNullOrPositive
);

DEFINE_string(visit_time_window,
              "00:120:00", "Time window for visits");
DEFINE_validator(visit_time_window, &util::time_duration::IsNullOrPositive
);

DEFINE_string(begin_end_shift_time_extension,
              "00:15:00",
              "Extra time added to the shift before and after working day");
DEFINE_validator(begin_end_shift_time_extension, &util::time_duration::IsNullOrPositive
);

void ParseArgs(int argc, char *argv[]) {
    gflags::SetVersionString("0.0.1");
    gflags::SetUsageMessage("Robust Optimization for Workforce Scheduling\n"
                            "Example: rows-mip"
                            " --problem=problem.json"
                            " --maps=./data/scotland-latest.osrm");

    static const auto REMOVE_FLAGS = false;
    gflags::ParseCommandLineFlags(&argc, &argv, REMOVE_FLAGS);

    VLOG(1) << boost::format("Launched with the arguments:\n"
                             "problem: %1%\n"
                             "maps: %2%\n") % FLAGS_problem % FLAGS_maps;
}

int main(int argc, char *argv[]) {
    util::SetupLogging(argv[0]);
    ParseArgs(argc, argv);

    const auto human_planner_schedule = util::LoadHumanPlannerSchedule(FLAGS_human_planners_solution);

    auto cancel_token = std::make_shared<std::atomic<bool> >(false);
    std::shared_ptr<rows::Printer> printer = util::CreatePrinter("log");
    const auto all_problem = util::LoadProblem(FLAGS_problem, printer);
    const auto problem = all_problem.Trim(boost::posix_time::ptime{human_planner_schedule.date()}, boost::posix_time::hours(24));

    const auto engine_config = util::CreateEngineConfig(FLAGS_maps);
    const auto visit_time_window = util::GetTimeDurationOrDefault(FLAGS_visit_time_window, boost::posix_time::not_a_date_time);
    const auto break_time_window = util::GetTimeDurationOrDefault(FLAGS_break_time_window, boost::posix_time::not_a_date_time);
    const auto begin_end_shift_time_extension
            = util::GetTimeDurationOrDefault(FLAGS_begin_end_shift_time_extension, boost::posix_time::not_a_date_time);

    operations_research::RoutingSearchParameters search_params = operations_research::DefaultRoutingSearchParameters();
//    search_params.set_first_solution_strategy(operations_research::FirstSolutionStrategy::ALL_UNPERFORMED);
//    search_params.mutable_local_search_operators()->set_use_exchange_subtrip(operations_research::OptionalBoolean::BOOL_TRUE);
//    search_params.mutable_local_search_operators()->set_use_relocate_expensive_chain(operations_research::OptionalBoolean::BOOL_TRUE);
//    search_params.mutable_local_search_operators()->set_use_light_relocate_pair(operations_research::OptionalBoolean::BOOL_TRUE);
//    search_params.mutable_local_search_operators()->set_use_relocate(operations_research::OptionalBoolean::BOOL_TRUE);
//    search_params.mutable_local_search_operators()->set_use_exchange(operations_research::OptionalBoolean::BOOL_TRUE);
//    search_params.mutable_local_search_operators()->set_use_exchange_pair(operations_research::OptionalBoolean::BOOL_TRUE);
//    search_params.mutable_local_search_operators()->set_use_extended_swap_active(operations_research::OptionalBoolean::BOOL_TRUE);
//    search_params.mutable_local_search_operators()->set_use_swap_active(operations_research::OptionalBoolean::BOOL_TRUE);
//    search_params.mutable_local_search_operators()->set_use_node_pair_swap_active(operations_research::OptionalBoolean::BOOL_TRUE);
//    search_params.mutable_local_search_operators()->set_use_cross_exchange(operations_research::OptionalBoolean::BOOL_TRUE);
//    search_params.mutable_local_search_operators()->set_use_relocate_neighbors(operations_research::OptionalBoolean::BOOL_TRUE);

//    search_params.set_local_search_metaheuristic(operations_research::LocalSearchMetaheuristic_Value_GUIDED_LOCAL_SEARCH);
//    search_params.set_guided_local_search_lambda_coefficient(1.0);

    auto problem_data_factory_ptr = std::make_shared<rows::RealProblemDataFactory>(engine_config);
    const auto problem_data = problem_data_factory_ptr->makeProblem(problem);
    rows::EstimateSolver solver{*problem_data,
                                human_planner_schedule,
                                search_params,
                                util::GetTimeDurationOrDefault(FLAGS_visit_time_window, boost::posix_time::not_a_date_time),
                                util::GetTimeDurationOrDefault(FLAGS_break_time_window, boost::posix_time::not_a_date_time),
                                util::GetTimeDurationOrDefault(FLAGS_begin_end_shift_time_extension, boost::posix_time::not_a_date_time),
                                boost::posix_time::seconds(30)};

    operations_research::RoutingModel model{solver.index_manager()};
//    second_stage_model->solver()->set_fail_intercept(&FailureInterceptor);
    solver.ConfigureModel(model, printer, cancel_token, 1.0);

    printer->operator<<(rows::TracingEvent(rows::TracingEventType::Started, "Stage1"));
    const auto solution_assignment = model.SolveWithParameters(search_params);
    printer->operator<<(rows::TracingEvent(rows::TracingEventType::Finished, "Stage1"));

    if (solution_assignment == nullptr) {
        throw util::ApplicationError("No second stage solution found.", util::ErrorCode::ERROR);
    }

    // TODO: save solution

    return EXIT_SUCCESS;
}


//==================================================================
// Author       : Pointner Sebastian
// Company      : Johannes Kepler University
// Name         : SMT Macro Placer
// Workfile     : terminal.hpp
//
// Date         : 22. December 2019
// Compiler     : gcc version 9.2.0 (GCC)
// Copyright    : Johannes Kepler University
// Description  : SoC Terminal Component
//==================================================================
#ifndef TERMINAL_HPP
#define TERMINAL_HPP

#include <iostream>
#include <string>
#include <algorithm>

#include <pin.hpp>
#include <object.hpp>
#include <logger.hpp>
#include <encoding_utils.hpp>

#include <boost/algorithm/string.hpp>

namespace Placer {

enum eTerminalType {
    ePowerTerminal,
    eSignalTerminal,
    eUnknownTerminal};

/**
 * @class Terminal
 * @brief Soc Terminal Pin
 */
class Terminal: public virtual Object {
public:

    Terminal(std::string const & name,
             e_pin_direction const direction);

    Terminal(std::string const & name,
             size_t const pos_x,
             size_t const pos_y,
             e_pin_direction const direction,
             eOrientation const orientation);

    virtual ~Terminal();

    bool operator== (Terminal const & t);

    bool is_free();

    void set_direction(e_pin_direction const direction);
    e_pin_direction get_direction() const;

    z3::expr& get_pos_x();
    z3::expr& get_pos_y();

    size_t get_pox_x_numerical();
    size_t get_pos_y_numerical();

    void add_solution_pos_x(size_t const val);
    void add_solution_pos_y(size_t const val);

    size_t get_solution_pos_x(size_t const sol);
    size_t get_solution_pos_y(size_t const sol);

    std::string get_name();
    std::string get_id();
    size_t get_key();
    bool has_solution(size_t const solution);

    bool is_input();
    bool is_output();
    bool is_bidirectional();

    bool is_power_terminal();
    bool is_signal_terminal();

    void set_bitwidth(size_t const width);
    size_t get_bitwidth () const;
    bool has_bitwidth();

    void set_frequency(size_t const frequency);
    size_t get_frequency() const;
    bool has_frequency();

    virtual void dump(std::ostream & stream = std::cout);

private:
    Utils::Logger* m_logger;
    EncodingUtils* m_encode;
    std::string m_name;
    bool m_free;
    size_t m_key;
    e_pin_direction m_direction;
    eTerminalType m_terminal_type;

    size_t m_bitwidth;
    size_t m_frequency;

    z3::expr m_pos_x;
    z3::expr m_pos_y;
    eOrientation m_orientation;

    std::vector<size_t> m_solutions_x;
    std::vector<size_t> m_solutions_y;

    void resolve_terminal_type();
    static std::vector<std::string> m_terminal_keywords;
};

} /* namespace Placer */

#endif /* TERMINAL_HPP */


#include "robotiq_epick_interface/epick_gripper_interface.h"

#include <ros/spinner.h>
#include <ros/callback_queue.h>
#include <algorithm>    // std::min std::max

#include <boost/bind.hpp>

namespace epick_interface
{
  inline float clip(float value, const float min, const float max){
    value = std::min(value, max);
    return std::max(value, min);
  }

  inline uint8_t toUint8(float value){
    return static_cast<uint8_t>(clip(round(value), 0., 255.));
  }

  inline uint8_t kpaToUint8(const float& relative_KPa){
    return toUint8(relative_KPa + 100.);
  }

  inline float uintToKpa(const uint8_t gP){
    return static_cast<float>(gP - 100); // KPa
  }

  inline uint8_t secToUint8(const float& rdel_sec){
    return toUint8(rdel_sec*10);
  }

  EpickGripper::EpickGripper(ros::NodeHandle& nh): nh_(nh)
  {
    loadParams();
    initPublishers();
    initSubscribers();
  }

  bool EpickGripper::waitForConnection(const ros::Duration& timeout)
  {
    ros::Duration(0.1).sleep();
    ros::Rate r(30);
    const ros::Time start_time = ros::Time::now();
    while(ros::ok()){
      if(ros::Time::now() - start_time > timeout){
        return false;
      }else{
        return true;
      }
      r.sleep();
    }
    return false;
  }

  bool EpickGripper::isReady()
  {
    return cur_status_.gSTA == 3 && cur_status_.gACT == 1;
  }

  bool EpickGripper::isReset()
  {
    return  cur_status_.gSTA == 0 || cur_status_.gACT == 0;
  }

  bool EpickGripper::isMoving()
  {
    return cur_status_.gGTO == 1 && cur_status_.gOBJ == 0;
  }

  bool EpickGripper::isStopped()
  {
    return cur_status_.gOBJ != 0;
  }

  bool EpickGripper::objectDetected()
  {
    return cur_status_.gOBJ == 1 || cur_status_.gOBJ == 2;
  }

  EpickGripper::FaultStatus EpickGripper::getFaultStatus()
  {
    return static_cast<EpickGripper::FaultStatus>(cur_status_.gFLT);
  }

  EpickGripper::ActuatorStatus EpickGripper::getActuatorStatus()
  {
    return static_cast<EpickGripper::ActuatorStatus>(cur_status_.gVAS);
  }

  float EpickGripper::getPressure()
  {
    return uintToKpa(cur_status_.gPO);
  }

  float EpickGripper::getRequestedPressure()
  {
    return uintToKpa(cur_status_.gPR);
  }

  bool EpickGripper::isClosed()
  {
    return cur_status_.gPO >= 230;
  }

  bool EpickGripper::isOpened()
  {
    return cur_status_.gPO <= 13;
  }

  bool EpickGripper::waitUntilStopped(const ros::Duration& timeout)
  {
    ros::Rate r(30);
    const ros::Time start_time = ros::Time::now();
    while(ros::ok()){
      if(ros::Time::now() - start_time > timeout){
        return false;
      }

      if(isStopped()){
        return true;
      }
    }
    return false;
  }

 bool EpickGripper::waitUntilMoving(const ros::Duration& timeout)
  {
    ros::Rate r(30);
    const ros::Time start_time = ros::Time::now();
    while(ros::ok()){
      if(ros::Time::now() - start_time > timeout){
        return false;
      }

      if(isStopped()){
        return true;
      }
    }
    return false;
  }

  void EpickGripper::reset()
  {
    GripperOutput cmd;
    cmd.rACT = 0;
    cmd.rGTO = 0;
    cmd_pub_.publish(cmd);
    return;
  }

  bool EpickGripper::activate(const ros::Duration& timeout)
  {
    GripperOutput cmd;
    cmd.rACT = 1;
    cmd.rMOD = 0;
    cmd.rGTO = 0;
    cmd.rPR = 0;
    cmd.rSP = 0;
    cmd.rFR = 0;
    cmd_pub_.publish(cmd);
    ros::Rate r(30);
    ros::Time start_time = ros::Time::now();
    while(ros::ok()){
      if(ros::Time::now() - start_time > timeout){
        return false;
      }

      if(isReady()){
        return true;
      }
      ros::spinOnce();
      r.sleep();
    }
    return false;
  }

  void EpickGripper::loadParams()
  {

    cmd_pub_topic_name_   = "output";
    state_sub_topic_name_ = "input";
    return;
  }

  void EpickGripper::initSubscribers()
  {
    state_sub_ = nh_.subscribe(state_sub_topic_name_, 1,
                               &EpickGripper::statusCb, this);
  }

  void EpickGripper::initPublishers()
  {
    cmd_pub_ = nh_.advertise<GripperOutput>(cmd_pub_topic_name_, 1);
  }

  void EpickGripper::statusCb(GripperInput msg)
  {
    cur_status_ = msg;
  }

  void EpickGripper::autoRelease()
  {
    GripperOutput cmd;
    cmd.rACT = 1;
    cmd.rATR = 1;
    cmd_pub_.publish(cmd);
  }

  bool EpickGripper::goTo(const float& max_press_kpa, const float& action_timeout_sec,
                  const float& min_press_kpa, const bool block, const ros::Duration& timeout
                   ){
    // REGULATE VACUUM PRESSURE REQUEST
    const float staurated_min = std::min(min_press_kpa, 0.f);
    GripperOutput cmd;
    cmd.rACT = 1;
    cmd.rMOD = 1;
    cmd.rGTO = 1;
    cmd.rPR = kpaToUint8(max_press_kpa);
    cmd.rSP = secToUint8(action_timeout_sec);
    cmd.rFR = kpaToUint8(staurated_min);
    //cmd.rATR = open_valve_;
    cmd_pub_.publish(cmd);

    ROS_WARN_STREAM("GRIP pressure uint  " << static_cast<int>(cmd.rPR));
    ros::Duration(0.1).sleep();

    if(block){
      if(!waitUntilMoving(timeout)){
          return false;
      }
      return waitUntilStopped(timeout);
    }
    return true;
  }

  bool EpickGripper::stop(bool block, const ros::Duration& timeout)
  {
    GripperOutput cmd;
    cmd.rACT = 1;
    cmd.rGTO = 0;
    cmd_pub_.publish(cmd);
    ros::Duration(0.1).sleep();
    if(block)
      return waitUntilStopped(timeout);
    return true;
  }

  bool EpickGripper::drop(const float& max_press_kpa, const float& action_timeout_sec,
                  const float& min_press_kpa, const bool block, const ros::Duration& timeout)
  {
    if(!isReady()){
      ROS_WARN("Impossible to actuate epick while it is not in ready state!");
      return false;
    }
    if(isOpened())
      return true;
    float gripping_press = std::max(0.f, max_press_kpa); // we need positive pressure to release
    return goTo(gripping_press, action_timeout_sec, min_press_kpa, block, timeout);
  }

  bool EpickGripper::grasp(const float& max_press_kpa, const float& action_timeout_sec,
                  const float& min_press_kpa, const bool block, const ros::Duration& timeout)
  {
    if(!isReady()){
     ROS_WARN("Impossible to actuate epick while it is not in ready state!");
      return false;
    }
    if(isClosed())
      return true;
    ROS_WARN_STREAM("GRIP pressure  " << max_press_kpa);
    float gripping_press = std::min(-10.f, max_press_kpa); // we need negative pressure to grasp
    return goTo(gripping_press, action_timeout_sec, min_press_kpa, block, timeout);
  }

}


#include <unistd.h>
#include <atomic>
#include <cmath>
#include <csignal>
#include <cstdlib>

// How to include Eigen
// https://stackoverflow.com/questions/56172620/how-to-build-a-simple-c-demo-using-eigen-with-bazel
#include <Eigen/Eigen>
#include <Eigen/Core>
#include <fstream>
#include <iostream>
#include <iterator>
#include <memory>
#include <sstream>
#include <string>
#include <thread>
#include <vector>

#include "os1.h"
#include "os1_packet.h"
#include "os1_util.h"
#include "lidar_scan.h"
#include "tensorflow/core/framework/tensor.h"
#include "tensorflow/core/framework/types.h"
#include "tensorflow/core/framework/tensor_types.h"
#include "tensorflow/core/framework/tensor_shape.h"
// #include "ouster/os1.h"
// #include "ouster/os1_packet.h"
// #include "ouster/os1_util.h"
// #include "ouster/lidar_scan.h"
// #include "ouster/viz.h"
namespace tf = ::tensorflow;
namespace OS1 = ouster::OS1;
// namespace viz = ouster::viz;
#include <chrono>

// https://stackoverflow.com/questions/50799510/how-to-run-custom-gpu-tensorflowop-from-c-code

/**
 * Print usage
 */
void print_help() {
    std::cout
        << "Usage: viz [options] [hostname] [udp_destination]\n"
        << "Options:\n"
        << "  -m <512x10 | 512x20 | 1024x10 | 1024x20 | 2048x10> : lidar mode, "
           "default 1024x10\n"
        << "  -f <path> : use provided metadata file; do not configure via TCP"
        << std::endl;
}

std::string read_metadata(const std::string& meta_file) {
    std::stringstream buf{};
    std::ifstream ifs{};
    ifs.open(meta_file);
    buf << ifs.rdbuf();
    ifs.close();

    if (!ifs) {
        std::cerr << "Failed to read " << meta_file
                  << "; check that the path is valid" << std::endl;
        std::exit(EXIT_FAILURE);
    }

    return buf.str();
}

int main(int argc, char** argv) {
    int W = 1024;
    int H = OS1::pixels_per_column;
    OS1::lidar_mode mode = OS1::MODE_1024x10;
    bool do_config = true;  // send tcp commands to configure sensor
    std::string metadata{};
    tf::Tensor tf_tensor(tf::DT_FLOAT, tf::TensorShape({1, 65536, 3}));
    tf::TTypes<float, 3>::Tensor map_tensor = tf_tensor.tensor<float, 3>();
    try {
        int c = 0;
        while ((c = getopt(argc, argv, "hm:f:")) != -1) {
            switch (c) {
                case 'h':
                    print_help();
                    return 1;
                    break;
                case 'm':
                    mode = OS1::lidar_mode_of_string(optarg);
                    if (mode) {
                        W = OS1::n_cols_of_lidar_mode(mode);
                    } else {
                        std::cout << "Lidar Mode must be 512x10, 512x20, "
                                     "1024x10, 1024x20, or 2048x10"
                                  << std::endl;
                        print_help();
                        std::exit(EXIT_FAILURE);
                    }
                    break;
                case 'f':
                    do_config = false;
                    metadata = read_metadata(optarg);
                    break;
                case '?':
                    std::cout << "Invalid Argument Format" << std::endl;
                    print_help();
                    std::exit(EXIT_FAILURE);
                    break;
            }
        }
    } catch (const std::exception& ex) {
        std::cout << "Invalid Argument Format: " << ex.what() << std::endl;
        print_help();
        std::exit(EXIT_FAILURE);
    }

    if (do_config && argc != optind + 2) {
        std::cerr << "Expected 2 arguments after options" << std::endl;
        print_help();
        std::exit(EXIT_FAILURE);
    }

    std::shared_ptr<OS1::client> cli;
    if (do_config) {
        std::cout << "Configuring sensor: " << argv[optind]
                  << " UDP Destination:" << argv[optind + 1] << std::endl;
        cli = OS1::init_client(argv[optind], argv[optind + 1], mode);
    } else {
        std::cout << "Listening for sensor data" << std::endl;
        cli = OS1::init_client();
    }

    if (!cli) {
        std::cerr << "Failed to initialize client" << std::endl;
        print_help();
        std::exit(EXIT_FAILURE);
    }

    uint8_t lidar_buf[OS1::lidar_packet_bytes + 1];
    uint8_t imu_buf[OS1::imu_packet_bytes + 1];

    auto ls = std::unique_ptr<ouster::LidarScan>(new ouster::LidarScan(W, H));

    // auto vh = viz::init_viz(W, H);

    if (do_config) metadata = OS1::get_metadata(*cli);

    auto info = OS1::parse_metadata(metadata);

    auto xyz_lut = OS1::make_xyz_lut(W, H, info.beam_azimuth_angles,
                                     info.beam_altitude_angles);

    // Use to signal termination
    std::atomic_bool end_program{false};

    // auto it = std::back_inserter(*ls);
    auto it = ls->begin();

    // callback that calls update with filled lidar scan
    auto batch_and_update = OS1::batch_to_iter<ouster::LidarScan::iterator>(
        xyz_lut, W, H, ouster::LidarScan::Point::Zero(),
        &ouster::LidarScan::make_val, [&](uint64_t) {
            // swap lidar scan and point it to new buffer
            // viz::update(*vh, ls);
            it = ls->begin();
            // std::cout << "Not Initialized with the first frame" << std::endl;
        });

    int count = 0;
    int batch = 0;
    bool complete = false;
    auto start = std::chrono::high_resolution_clock::now();
    // Start poll thread
    std::thread poll([&] {
        while (!end_program) {
            if (count > 1){
                // std::cout << "Next Batch count " << batch << std::endl;
                batch += 1;
                count = 0;
            }

            // Poll the client for data and add to our lidar scan
            OS1::client_state st = OS1::poll_client(*cli);
            if (st & OS1::client_state::ERROR) {
                std::cerr << "Client returned error state" << std::endl;
                std::exit(EXIT_FAILURE);
            }
            if (st & OS1::client_state::LIDAR_DATA) {
                if (OS1::read_lidar_packet(*cli, lidar_buf)){

                    // Get starting timepoint
                    // auto start = std::chrono::high_resolution_clock::now();

                    // Call the function, here sort()
                    batch_and_update(lidar_buf, it , map_tensor, complete);
                    // Get ending timepoint
                    // auto stop = std::chrono::high_resolution_clock::now();

                    // // Get duration. Substart timepoints to
                    // // get durarion. To cast it to proper unit
                    // // use duration cast method
                    // auto duration = std::chrono::duration_cast<std::chrono::microseconds>(stop - start);

                    // std::cout << "Time taken by function: "
                    //     << duration.count() << " microseconds" << std::endl;
                    // tf::TTypes<float, 3>::Tensor map_tensor_2 = tf_tensor.tensor<float, 3>();
                    // for(int k = 0; k < 65536; k ++){
                    //     std::cout << map_tensor_2(0,k,0) << " " <<  map_tensor_2(0,k,1) << " "   <<  map_tensor_2(0,k,2) << std::endl;

                    // }
                    // std::cout << tf_tensor.shape() << std::endl;
                    // std::cout << map_tensor_2.setZero() << std::endl;
                    if(complete){
                        auto stop = std::chrono::high_resolution_clock::now();
                        auto duration = std::chrono::duration_cast<std::chrono::microseconds>(stop - start);
                        std::cout << "Time taken by function: "
                            << duration.count() << " microseconds" << std::endl;
                        count += 1;
                        map_tensor.setZero();
                        start = std::chrono::high_resolution_clock::now();
                    }

                    // if (batch > 0){
                    //     end_program = true;
                    // }
                }
            }
            if (st & OS1::client_state::IMU_DATA) {
                OS1::read_imu_packet(*cli, imu_buf);
            }
        }
    });

    // print the content of the point cloud
    // while(batch < 1){}
    // if (batch == 1){
    //     auto it2 = ls->data_;
    //     int npoints = 0;
    //     for (;npoints < H * W ; npoints++)
    //     {
    //         std::cout << it2[npoints] << std::endl;
    //     }
    // }
    while(true){}
    // clean up
    poll.join();
    return EXIT_SUCCESS;
}


#include <iostream>

#include <boost/numeric/linear_algebra/operators.hpp>
#include <boost/numeric/linear_algebra/concepts.hpp>

#include <libs/numeric/linear_algebra/test/algebraic_functions.hpp>

int main(int, char* [])
{
    using namespace mtl;

    math::add<int>     int_add;

    std::cout << "equal_results(2,5,  3,4, int_add) "
         << equal_results(2,5,  3,4, int_add)  << std::endl;
    std::cout << "equal_results(2,4,  3,4, int_add) "
         << equal_results(2,4,  3,4, int_add)  << std::endl;

    std::cout << "identity_pair(2,4, int_add) "
         << identity_pair(2,4, int_add)  << std::endl;
    std::cout << "identity_pair(0,0, int_add) "
         << identity_pair(0,0, int_add)  << std::endl << std::endl;

    std::cout << "ordered_algebraic_division(3,2, int_add) "
         << ordered_algebraic_division(3,2, int_add)  << std::endl;
    std::cout << "ordered_algebraic_division(4,2, int_add) "
         << ordered_algebraic_division(4,2, int_add)  << std::endl;
    std::cout << "ordered_algebraic_division(-6,2, int_add) "
         << ordered_algebraic_division(-6,2, int_add)  << std::endl;

    return 0;
}