/* -------------------------------------------------------------------------
 *   A Modular Optimization framework for Localization and mApping  (MOLA)
 * Copyright (C) 2018-2021 Jose Luis Blanco, University of Almeria
 * See LICENSE for license information.
 * ------------------------------------------------------------------------- */

/**
 * @file   test-mp2p_icp-algos.cpp
 * @brief  Unit tests for mp2p_icp solvers
 * @author Jose Luis Blanco Claraco
 * @date   May 12, 2019
 */

#include <mp2p_icp/ICP_LibPointmatcher.h>
#include <mp2p_icp/Matcher_Point2Plane.h>
#include <mp2p_icp/Matcher_Points_DistanceThreshold.h>
#include <mp2p_icp/Solver_GaussNewton.h>
#include <mp2p_icp/Solver_Horn.h>
#include <mp2p_icp/Solver_OLAE.h>
#include <mp2p_icp/load_xyz_file.h>
#include <mrpt/core/exceptions.h>
#include <mrpt/core/get_env.h>
#include <mrpt/maps/CSimplePointsMap.h>
#include <mrpt/poses/CPose3D.h>
#include <mrpt/poses/CPose3DQuat.h>
#include <mrpt/poses/Lie/SE.h>
#include <mrpt/poses/Lie/SO.h>
#include <mrpt/random.h>
#include <mrpt/system/CTimeLogger.h>
#include <mrpt/system/filesystem.h>
#include <mrpt/version.h>

#include <Eigen/Dense>
#include <iostream>

// Used to validate OLAE. However, it may make the Gauss-Newton solver, or the
// robust kernel with outliers to fail.
static double XYZ_RANGE = mrpt::get_env<double>("XYZ_RANGE", 5.0);
static int    NUM_REPS  = mrpt::get_env<int>("NUM_REPS", 3);
static bool   DO_SAVE_STAT_FILES =
    mrpt::get_env<bool>("DO_SAVE_STAT_FILES", false);
static bool DO_PRINT_ALL = mrpt::get_env<bool>("DO_PRINT_ALL", false);

const std::string datasetDir = MP2P_DATASET_DIR;

static void test_icp(
    const std::string& inFile, const std::string& icpClassName,
    const std::string& solverName, const std::string& matcherName)
{
    using namespace mrpt::poses::Lie;

    const auto fileFullPath = datasetDir + inFile;

    const mrpt::maps::CSimplePointsMap::Ptr pts =
        mp2p_icp::load_xyz_file(fileFullPath);

    std::cout << "Running " << icpClassName << "|" << solverName << "|"
              << matcherName << " test on: " << inFile << " with "
              << pts->size() << " points\n";

    double outliers_ratio = 0;
    bool   use_robust     = true;

    const std::string tstName = mrpt::format(
        "test_icp_Model=%s_Algo=%s_%s_%s_outliers=%06.03f_robust=%i",
        inFile.c_str(), icpClassName.c_str(), solverName.c_str(),
        matcherName.c_str(), outliers_ratio, use_robust ? 1 : 0);

#if MRPT_VERSION >= 0x218
    const auto bbox      = pts->boundingBox();
    const auto bbox_size = bbox.max - bbox.min;
#else
    mrpt::math::TPoint3D bbox_min, bbox_max;
    pts->boundingBox(bbox_min, bbox_max);
    const auto bbox_size = bbox_max - bbox_min;
#endif
    const double max_dim = mrpt::max3(bbox_size.x, bbox_size.y, bbox_size.z);

    const double f = 0.25;

    mrpt::system::CTicTac timer;

    // Collect stats: columns are (see write TXT to file code at the bottom)
    mrpt::math::CMatrixDouble stats(NUM_REPS, 2 + 2 + 1);

    for (int rep = 0; rep < NUM_REPS; rep++)
    {
        // Create transformed (and corrupted?) point cloud:
        auto&        rnd   = mrpt::random::getRandomGenerator();
        const double Dx    = rnd.drawUniform(-f * bbox_size.x, f * bbox_size.x);
        const double Dy    = rnd.drawUniform(-f * bbox_size.y, f * bbox_size.y);
        const double Dz    = rnd.drawUniform(-f * bbox_size.z, f * bbox_size.z);
        const double yaw   = mrpt::DEG2RAD(rnd.drawUniform(-20.0, 20.0));
        const double pitch = mrpt::DEG2RAD(rnd.drawUniform(-20.0, 20.0));
        const double roll  = mrpt::DEG2RAD(rnd.drawUniform(-20.0, 20.0));

        const auto gt_pose = mrpt::poses::CPose3D(Dx, Dy, Dz, yaw, pitch, roll);

        stats(rep, 0) = SO<3>::log(gt_pose.getRotationMatrix()).norm();
        stats(rep, 1) = gt_pose.norm();

        auto pts_reg = mrpt::maps::CSimplePointsMap::Create();
        pts_reg->changeCoordinatesReference(*pts, gt_pose);

        // Point clouds: reference and modified:
        mp2p_icp::metric_map_t pc_ref, pc_mod;
        pc_ref.layers["raw"] = pts;
        pc_mod.layers["raw"] = pts_reg;

        const auto init_guess = mrpt::math::TPose3D::Identity();

        mp2p_icp::ICP::Ptr icp = std::dynamic_pointer_cast<mp2p_icp::ICP>(
            mrpt::rtti::classFactory(icpClassName));

        if (!icp)
            THROW_EXCEPTION_FMT(
                "Could not create object of type `%s`, is it registered?",
                icpClassName.c_str());

        // Initialize solvers:
        if (!solverName.empty())
        {
            mp2p_icp::Solver::Ptr solver =
                std::dynamic_pointer_cast<mp2p_icp::Solver>(
                    mrpt::rtti::classFactory(solverName));

            if (!solver)
                THROW_EXCEPTION_FMT(
                    "Could not create Solver of type `%s`, is it registered?",
                    solverName.c_str());

            icp->solvers().clear();
            icp->solvers().push_back(solver);
        }

        // Initialize matchers:
        if (!matcherName.empty())
        {
            mp2p_icp::Matcher::Ptr matcher =
                std::dynamic_pointer_cast<mp2p_icp::Matcher>(
                    mrpt::rtti::classFactory(matcherName));

            if (!matcher)
                THROW_EXCEPTION_FMT(
                    "Could not create Matcher of type `%s`, is it registered?",
                    matcherName.c_str());

            icp->matchers().push_back(matcher);
        }

        // Special parameters:
        if (!icp->matchers().empty())
        {
            if (auto m = std::dynamic_pointer_cast<
                    mp2p_icp::Matcher_Points_DistanceThreshold>(
                    icp->matchers().at(0));
                m)
            {
                mrpt::containers::yaml ps;
                ps["threshold"] = 0.15 * max_dim;
                m->initialize(ps);
            }

            if (auto m =
                    std::dynamic_pointer_cast<mp2p_icp::Matcher_Point2Plane>(
                        icp->matchers().at(0));
                m)
            {
                mrpt::containers::yaml ps;
                ps["distanceThreshold"]   = 0.15 * max_dim;
                ps["planeEigenThreshold"] = 10.0;
                ps["knn"]                 = 5;

                m->initialize(ps);
            }
        }

        // ICP test itself:
        mp2p_icp::Parameters icp_params;
        mp2p_icp::Results    icp_results;

        icp_params.maxIterations = 100;

        timer.Tic();

        icp->align(pc_mod, pc_ref, init_guess, icp_params, icp_results);

        const double dt = timer.Tac();

        const auto pos_error = gt_pose - icp_results.optimal_tf.mean;
        const auto err_log_n = SO<3>::log(pos_error.getRotationMatrix()).norm();
        const auto err_xyz   = pos_error.norm();
        const auto err_se3   = SE<3>::log(pos_error).norm();

        stats(rep, 2 + 0) = err_log_n;
        stats(rep, 2 + 1) = err_xyz;
        stats(rep, 2 + 2) = dt;

        if (DO_PRINT_ALL)
        {
            std::cout << "GT pose       : " << gt_pose.asString() << "\n";
            std::cout << "ICP pose      : "
                      << icp_results.optimal_tf.mean.asString() << "\n";
            std::cout << "Error SE(3)   : " << err_se3 << "\n";
            std::cout << "ICP pose stddev: "
                      << icp_results.optimal_tf.cov.asEigen()
                             .diagonal()
                             .array()
                             .sqrt()
                             .matrix()
                             .transpose()
                      << "\n";
            std::cout << "ICP quality    : " << icp_results.quality << "\n";
            std::cout << "ICP iterations : " << icp_results.nIterations << "\n";
        }

    }  // for reps

    if (DO_SAVE_STAT_FILES)
        stats.saveToTextFile(
            mrpt::system::fileNameStripInvalidChars(tstName) +
                std::string(".txt"),
            mrpt::math::MATRIX_FORMAT_ENG, true,
            "% Columns: norm GT_rot, norm_GT_XYZ, norm(SO3_error) "
            "norm(XYZ_error) icp_time\n\n");
}

int main([[maybe_unused]] int argc, [[maybe_unused]] char** argv)
{
    try
    {
        auto& rnd = mrpt::random::getRandomGenerator();
        rnd.randomize(1234);  // for reproducible tests

        const std::vector<const char*> lst_files{
            {"bunny_decim.xyz.gz", "happy_buddha_decim.xyz.gz"}};

        using lst_algos_t =
            std::vector<std::tuple<const char*, const char*, const char*>>;
        // clang-format off
        lst_algos_t lst_algos = {
             {"mp2p_icp::ICP", "mp2p_icp::Solver_Horn",        "mp2p_icp::Matcher_Points_DistanceThreshold"},
             {"mp2p_icp::ICP", "mp2p_icp::Solver_Horn",        "mp2p_icp::Matcher_Points_InlierRatio"},
             {"mp2p_icp::ICP", "mp2p_icp::Solver_Horn",        "mp2p_icp::Matcher_Point2Plane"},

             {"mp2p_icp::ICP", "mp2p_icp::Solver_OLAE",        "mp2p_icp::Matcher_Points_DistanceThreshold"},
             {"mp2p_icp::ICP", "mp2p_icp::Solver_OLAE",        "mp2p_icp::Matcher_Points_InlierRatio"},
             {"mp2p_icp::ICP", "mp2p_icp::Solver_OLAE",        "mp2p_icp::Matcher_Point2Plane"},

             {"mp2p_icp::ICP", "mp2p_icp::Solver_GaussNewton", "mp2p_icp::Matcher_Points_DistanceThreshold"},
             {"mp2p_icp::ICP", "mp2p_icp::Solver_GaussNewton", "mp2p_icp::Matcher_Points_InlierRatio"},
             {"mp2p_icp::ICP", "mp2p_icp::Solver_GaussNewton", "mp2p_icp::Matcher_Point2Plane"},
             };
        // clang-format on

        // Optional methods:
        if (mp2p_icp::ICP_LibPointmatcher::methodAvailable())
            lst_algos.push_back({"mp2p_icp::ICP_LibPointmatcher", "", ""});

        for (const auto& algo : lst_algos)
            for (const auto& fil : lst_files)
                test_icp(
                    fil, std::get<0>(algo), std::get<1>(algo),
                    std::get<2>(algo));
    }
    catch (std::exception& e)
    {
        std::cerr << mrpt::exception_to_str(e) << "\n";
        return 1;
    }
}


// ========================================================================
// rsmc-ortho  -- Calculate the ortho-rectification version of a plain
// tiff image by given set of mapping points and a new image size.
// ========================================================================
// Author: Alexander Weidauer <alex.weidauer@ifgdv.de>
// Created: 2021-09-10
// Version: 1.0
// ========================================================================
// (C) 2021 Alexander Weidauer  <alex.weidauer@ifgdv.de>
// All Rights Reserved
//
// The code is released under 3-clause BSD Licence
// ========================================================================
// Changelog:
//   DATE        WHO   ITEM
//   2021-09-10  i4w   Initial setup
//   2021-09-12  i4w   CLI-Pasrer from Test static to the CLI interface
//   2021-09-13  i4w   Add CLI-Help
//   2021-09-29  i4w   Implement image size parameter
//   2021-09-30  i4w   Code documentation
//   2021-10-04  i4w   Change the program name to rsmc-ortho
//   2021-10-11  i4w   Simplify file management
// =====================================================================
// TODO:
//   - Complete code documentation, expand to full doxygen code
//   - separate the CLI parser from the main code int cli.(hpp,cpp)
//   - Create a man page
// =====================================================================
#include <cctype>
#include <cstdio>
#include <cstdlib>
#include <cstring>
#include <cstddef>
#include <iostream>
#include <boost/filesystem.hpp>
#include "opencv2/highgui.hpp"
#include "opencv2/imgcodecs.hpp"
#include "opencv2/imgproc.hpp"

//+ NAMESPACES =============================================================
using namespace std;
using namespace cv;
namespace fs = boost::filesystem;

//+ DEFINITIONS ============================================================
#define NO_ARG 0
#define REQ_ARG 1
#define OPT_ARG 2
#define NIL nullptr

#define VERSION "1.0"
#define PROGRAM "rsmc-ortho"
#define TITLE   "RSMC Ortho-Rectification via OpenCV"

//+ DEBUG ONLY
const int CHECK_ARGC = 0;

//+ TYPES ==================================================================
typedef Point2f TPoint;
typedef vector<TPoint> TPoints;
typedef Mat    TMatrix;
typedef string TString;
typedef fs::path TPath;
typedef int    TInteger;

struct TBBox {
  double xmin;
  double xmax;
  double ymin;
  double ymax;
};

// =========================================================================
//+ SERVICE ROUTINES
// =========================================================================

void printHelp() {
  cout <<
    PROGRAM "\\\n"
    "        -B <MIN.X> <MIN.Y> <MAX.X> <MAX.Y> \\\n"
    "        -I <INCOMING-IMAGE> \\\n"
    "        -R <RECTIFIED-IMAGE> \\\n"
    "        -S <WIDTH> <HEIGHT>\\\n"
    "        -O  <X1> <Y1> .. <XN> <YN>\\\n"
    "        -M  <X1> <Y1> .. <XN> <YN>\n\n"
    "  -B  --bbox <MIN.X> <MIN.Y> <MAX.X> <MAX.Y>: Image bounding box [m]\n"
    "        <MIN.X <MIN.Y> minimum coordinates of numeric type\n"
    "        <MAX.X <MAX.Y> maximum coordinates of numeric type\n"
    "\n"
    "  -I  --incoming-image  <FILE-NAME>: incoming image \n\n"
    "  -R  --rectified-image <FILE-NAME>: Name of the produced file.\n"
    "\n"
    "  -M  --mapped-icps <X1> <Y1> .. <XN> <YN>: vector of image control\n"
    "        points (ICPS) are N mapped perspective coordinates XY in the\n"
    "        image space. The tuples <Xn> & <Yn> have a numeric type\n"
    "\n"
    "  -O  --original-icps <X1> <Y1> .. <XN> <YN> vector of image control\n"
    "        points (ICPS) are N original coordinates corresponding to the \n"
    "        mapped ICPS in the image space and have a type numeric\n"
    "\n"
    "  -S  --image-size <WIDTH> <HEIGHT>: Width & height of the resulting image.\n"
    "        <WIDTH> & <HEIGHT> have a integer type.      "
    "\n"
    "  -h  --help:          Show help\n"
    "\n"
    "  -v  --version:       Show version\n"
    "\n";
}

// -------------------------------------------------------------------------
/// \brief Print the program version
///
///
void printVersion(const char* name) {
  cout << "PROGRAM: " << name << " VERSION: " VERSION "\n"
  "DESCRIPTION: " TITLE "\n\n";
}

// -------------------------------------------------------------------------
/// \brief Checks if the next char in a null terminated char is a whitespace
///
/// aka String is empty
bool isSpace(const char* s) {
  size_t i, len;
  if(s && *s) {
       len = strlen(s);
       for(i=0; i< len; i++)
          if (! isspace(s[i]) ) return false;
   }
   return true;
}

// -------------------------------------------------------------------------
/// \brief Get one CLI argument and increase to read counter
///
/// The function tries to read on commandline argument from a defined
/// tuple of rgument counts, argument data for a given parameter tag and
/// increases the argument counter.
///
/// The function succeeds or stops the program with an error message.
///
/// \param argCount - number of arguments in the incoming CLI set
/// \param args     - array of null terminated strings containg the data
/// \param argPos   - current position in the incoming argument array
///                   and outgoing new postion after a call
/// \param tag      - parameter tag to read (info only)
///
/// \returns a constant pointer to the to the decoded string

const char* getArg(int argCount, const char **args, int &argPos,
                     const TString tag) {
  if (argPos >= argCount) {
      cerr << "ERROR: Insufficient number of CLI parameter for "
                << "parameter '" << tag <<"'!\n";
      exit(255);
  }
  const char *res = args[argPos]; argPos++;
  if ( ( res == NIL ) ||
       (strlen(res) == 0) ||
       isSpace(res) ) {
     cerr << "ERROR: Empty value for parameter '" << tag <<"'!\n";
     exit(255);
   }
  return res;
}

// -------------------------------------------------------------------------
/// \brief Checks the number of available CLI arguments

void checkArgCount(int argCount, int argPos,
                   const TString arg, int argMore) {
  if (argPos+argMore > argCount) {
     cerr << "ERROR: Insufficient number of CLI parameter for "
               << "parameter '" << arg <<"'!\n"
               << "NOTE: At least " << argMore
               << "parameter are required!\n";
     exit(255);
  }
}

// -------------------------------------------------------------------------
/// \brief Checks if a given path exists

void checkPathExists(const TPath aPath,
                     const TString context) {
  if ( !fs::is_directory(aPath) ) {
      cerr << "ERROR: The " << context << " path '"
           << aPath << "' does not exsits!";
      exit(255);
  }
}

// -------------------------------------------------------------------------
/// \brief Checks if a given file exists

void checkFileExists(const TString strFile,
                     const TString context) {
  if ( !fs::exists(strFile) ) {
      cerr << "ERROR: The " << context << " file '"
           << strFile << "' does not exsits!";
      exit(255);
  }
}

// -------------------------------------------------------------------------
/// \brief Checks if a parsed parameter was found and initialized by the CLI

void checkParamInit(bool ok, const TString param) {
  if ( ! ok ) {
      cerr << "ERROR: Missing parameter --" << param << "!\n"
           << "NOTE: Call " << PROGRAM << " --help for furhter infos!\n";
      exit(255);
  }
}

// -------------------------------------------------------------------------
/// \brief Checks if a given parameter is a long or short annotated.

bool isArgParam(const char *arg) {
   size_t len =  strlen(arg);
   if ( (len>1) &&
        (arg[0]=='-') &&
        isalpha(arg[1]) )
        return true;
   if ( (len>2) &&
        (arg[0]=='-') &&
        (arg[1]=='-') &&
        isalpha(arg[2]) )
        return true;
  return false;
}

// -------------------------------------------------------------------------
/// \brief Get a CLI parameter value as char*

const char* getArgStr(int argCount, const char **args,
                      int &argPos, const TString tag) {
   const char* res = getArg(argCount, args, argPos, tag);
   if (isArgParam(res)) {
       cerr << "ERROR: Value expected for parameter'" << tag << "', but "
            << "parameter '" << res << " found!\n";
       exit(255);
     }
   return res;
}

// -------------------------------------------------------------------------
/// \brief Get a CLI parameter value as integer

int getArgInt(int argCount, const char **args,
              int &argPos, const TString tag) {
  int res = 0;
  const char *arg = getArgStr(argCount, args, argPos, tag);
  if (sscanf(arg, "%d", &res) ==0) {
      cerr << "ERROR: Invalid integer value '"
           << arg << "' for '" << tag <<"'!\n";
      exit(255);
    }
  return res;
}

// -------------------------------------------------------------------------
/// \brief Get a CLI parameter value as double

double getArgReal(int argCount, const char **args,
                  int &argPos, const TString tag) {
  double res = 0;
  const char *arg = getArgStr(argCount, args, argPos, tag);
  if (sscanf(arg, "%lf", &res) == 0) {
      cerr << "ERROR: Invalid real parameter value '"
           << arg << "' for '" << tag <<"'!\n";
      exit(255);
    }
  return res;
}

// -------------------------------------------------------------------------
/// \brief Get a CLI parameter set value as point list char*

void getArgPoints(int argCount, const char **args,
                         int &argPos, const TString tag,
                         TPoints &list) {
   int cntPoint = 0;
   const char* arg = getArg(argCount, args, argPos, tag);
   while ( !( isArgParam(arg) || (argPos >= argCount)) ) {
       float x = 0.0;
       float y = 0.0;
       cntPoint++;
       if (sscanf(arg, "%f", &x) ==0) {
           cerr << "ERROR: Invalid real value for X["<< cntPoint << "] = '"
                << arg << "' for parameter '" << tag <<"'!\n";
           exit(255);
        }
        arg = getArg(argCount, args, argPos, tag);
        if (sscanf(arg, "%f", &y) ==0) {
            cerr << "ERROR: Invalid real value for Y["<< cntPoint << "] = '"
                 << arg << "' for parameter '" << tag <<"'!\n";
            exit(255);
         }
        const TPoint p(x,y);
        list.push_back(p);
        if ( argPos < argCount) {
           arg = getArg(argCount, args, argPos, tag);
        } else { argPos++; }
     }
     if (argPos < argCount) argPos--;
}

// -------------------------------------------------------------------------
/// \brief Read and parse the CLI parameter set for the app

void readCli(int argc, const char** argv,
                 TString  &inImage,
                 TString  &outImage,
                 TBBox    &ImageBBox,
                 TInteger &ImageWidth,
                 TInteger &ImageHeight,
                 TPoints  &IcpsOriginal,
                 TPoints  &IcpsMapped) {

   // current argument position
   int argPos = 0;

   // Check if we have enough params in the data array
   if (CHECK_ARGC && ( argc < 30 )) {
         cerr
           << "ERROR: In sufficient number of CLI parameter present!\n"
           << "NOTE: Call " PROGRAM  " --help for further  infos\n";
   }
   // Iterate over args
   while (argPos< argc) {

       TString arg(argv[argPos]); argPos++;

       // if (DEBUG) { cout << argPos << ":" << argv[argPos] << "\n"; }

       // Skip program name -------------------------------------------------
       if (argPos == 1) continue;

       // Parse bbox values -------------------------------------------------
       if ((arg == "-B") || (arg == "--bbox")) {
           checkArgCount(argc, argPos, arg, 4);
           ImageBBox.xmin = getArgReal(argc, argv, argPos, arg);
           ImageBBox.xmax = getArgReal(argc, argv, argPos, arg);
           ImageBBox.ymin = getArgReal(argc, argv, argPos, arg);
           ImageBBox.ymax = getArgReal(argc, argv, argPos, arg);
       }

       // Read capaign name ------------------------------------------------
       else if ((arg == "-I") || (arg == "--incoming-image")) {
           inImage = getArgStr(argc, argv, argPos, arg);
       }

       // Read campaign name ------------------------------------------------
       else if ((arg == "-R") || (arg == "--rectified-image")) {
           outImage = getArgStr(argc, argv, argPos, arg);
       }

       // Parse image width and height values ------------------------------
       else if ((arg == "-S") || (arg == "--image-size")) {
           checkArgCount(argc, argPos, arg, 2);
           ImageWidth  = getArgInt(argc, argv, argPos, arg);
           ImageHeight = getArgInt(argc, argv, argPos, arg);
       }

       // Read original points  --------------------------------------------
       else if ((arg == "-O") || (arg == "--original-icps")) {
           getArgPoints(argc, argv, argPos, arg, IcpsOriginal);
           // cout << IcpsOriginal << "\n" << IcpsOriginal.size() << "\n";
       }

       // Read mapped points -----------------------------------------------
       else if ((arg == "-M") || (arg == "--mapped-icps")) {
           getArgPoints(argc, argv, argPos, arg, IcpsMapped);
           // cout << IcpsMapped << "\n";
       }

       // Print help -------------------------------------------------------
       else if ((arg == "-h") || (arg == "--help")) {
           printHelp();
           exit(1);
       }

       // Print Version ----------------------------------------------------
       else if ((arg == "-v") || (arg == "--version")) {
           printVersion(argv[0]);
           exit(1);
       }
       // CLI parameter is unknown -----------------------------------------
       else {
         cerr << "ERROR: Unknown CLI parameter '" << arg << "'!\n\n";
         exit(255);
       }
     }
       // ==== ASSERTATIONS =================================================

       checkParamInit(inImage   != "", "incoming-image");
       checkParamInit(outImage  != "", "rectified-image");

       checkParamInit( ! ( (ImageBBox.xmin == 0.0) ||
                           (ImageBBox.xmax == 0.0) ||
                           (ImageBBox.ymin == 0.0) ||
                           (ImageBBox.ymax == 0.0) ),
                        "bbox");

       checkParamInit(  (ImageWidth > 0) && (ImageHeight > 0) ,
                       "image-size");

       checkParamInit( IcpsMapped.size() > 3   , "mapped-icsp");
       checkParamInit( IcpsOriginal.size() > 3 , "original-icsp");

       if ( IcpsMapped.size() != IcpsOriginal.size() ) {
           cerr << "Lists length of mapped ICPS and orgiginal " <<
                   "ICPS is not equal!\n";
           exit(255);
       }
       checkPathExists(TPath(outImage).parent_path(),"work path");
       checkFileExists(inImage,"incoming image");
}

// =========================================================================
//+ MAIN ROUTINE
// =========================================================================
int main(int argc, const char* argv[]) {

  // Init needed variables -------------------------------------------------
  TPoints  icpsOriginal;
  TPoints  icpsMapped;
  TString  camSerial = "";
  TString  inImage   = "";
  TString  outImage  = "";
  TInteger imgWidth  = -1;
  TInteger imgHeight = -1;
  TBBox    imgBBox   = { 0., 0., 0., 0.};

  // Parse the CLI parameter set -------------------------------------------
  cout << ".READ CLI\n";
  readCli(argc, argv,
          inImage, outImage,
          imgBBox, imgWidth, imgHeight,
          icpsOriginal, icpsMapped);

  // Read the image file ---------------------------------------------------
  cout << ".READ INPUT " << inImage << "\n";
  TMatrix imgOriginal = cv::imread(inImage);

  // Calculate the perspective ---------------------------------------------
  cout << ".CALC PERSPECTIVE" << outImage << "\n";
  TMatrix trfmPerpective = getPerspectiveTransform(icpsOriginal,
                                                   icpsMapped);

  // Orthorectify the image ------------------------------------------------
  TMatrix imgMapped;
  cout << ".MAP  PERSPECTIVE\n";
  warpPerspective(imgOriginal,
                  imgMapped,
                  trfmPerpective,
                  Size(imgWidth, imgHeight), //imgOriginal.size(),
                  cv::INTER_LANCZOS4);

  // Write the result ------------------------------------------------------
  cout << ".WRITE MAPPED " << outImage << "\n";
  imwrite(outImage, imgMapped);
  cout << ".ORTHO CALCULATUION OK\n\n";
  exit(0);
}
// ========================================================================
//+ EOF
// ========================================================================


#ifndef ELASTY_UTILS_HPP
#define ELASTY_UTILS_HPP

#include <Eigen/Core>
#include <memory>
#include <string>
#include <vector>

namespace elasty
{
    struct Particle;
    class AbstractConstraint;
    class ClothSimObject;

    constexpr double pi() { return 3.14159265358979323846264338327950288; }

    /// \param particles an array of N particles
    ///
    /// \return an array of 3 * N values:
    /// std::vector<float>
    /// {
    ///     p[0].x, p[0].y, p[0].z,
    ///     p[1].x, p[1].y, p[1].z,
    ///     ...
    /// }
    std::vector<float> packParticlePositions(const std::vector<std::shared_ptr<Particle>>& particles);

    void setRandomVelocities(const std::vector<std::shared_ptr<Particle>>& particles, const double scale = 1.0);

    void generateFixedPointConstraints(const Eigen::Vector3d&                            search_position,
                                       const Eigen::Vector3d&                            fixed_position,
                                       const std::vector<std::shared_ptr<Particle>>&     particles,
                                       std::vector<std::shared_ptr<AbstractConstraint>>& constraints);

    /// \brief Export the current cloth state as an obj file.
    void exportCurrentClothStateAsObj(const std::string&                    output_file_path,
                                      const std::shared_ptr<ClothSimObject> cloth_sim_object);

    /// \brief Generate rectangular plane mesh for cloth simulation in the obj string format.
    ///
    /// \param vertical_resolution Need to be an even number.
    std::string generateClothMeshObjData(const double   width                 = 2.0,
                                         const double   height                = 2.0,
                                         const unsigned horizontal_resolution = 50,
                                         const unsigned vertical_resolution   = 50);
} // namespace elasty

#endif // ELASTY_UTILS_HPP


/*=========================================================================
 *
 *  Copyright David Doria 2012 daviddoria@gmail.com
 *
 *  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.txt
 *
 *  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 SourceValidTargetValidCompare_HPP
#define SourceValidTargetValidCompare_HPP

#include <boost/graph/graph_traits.hpp>

// Parent class
#include "Visitors/AcceptanceVisitors/AcceptanceVisitorParent.h"

// Custom
#include "Mask/Mask.h"
#include "ITKHelpers/ITKHelpers.h"
#include "BoostHelpers/BoostHelpers.h"

// ITK
#include "itkImage.h"
#include "itkImageRegion.h"

/**

 */
template <typename TGraph, typename TImage, typename TFunctor>
struct SourceValidTargetValidCompare : public AcceptanceVisitorParent<TGraph>
{
  TImage* Image;
  Mask* MaskImage;

  const unsigned int HalfWidth;
  unsigned int NumberOfFinishedVertices = 0;

  TFunctor Functor;

  float DifferenceThreshold;

  typedef typename boost::graph_traits<TGraph>::vertex_descriptor VertexDescriptorType;

  SourceValidTargetValidCompare(TImage* const image, Mask* const mask,
                                const unsigned int halfWidth, TFunctor functor = TFunctor(), const float differenceThreshold = 100,
    const std::string& visitorName = "SourceValidTargetValidCompare") :
  AcceptanceVisitorParent<TGraph>(visitorName),
  Image(image), MaskImage(mask), HalfWidth(halfWidth),
    Functor(functor), DifferenceThreshold(differenceThreshold)
  {
  }

  bool AcceptMatch(VertexDescriptorType target, VertexDescriptorType source, float& computedEnergy) const override
  {
    //std::cout << "DilatedVarianceDifferenceAcceptanceVisitor::AcceptMatch" << std::endl;

    itk::Index<2> targetPixel = ITKHelpers::CreateIndex(target);
    itk::ImageRegion<2> targetRegion = ITKHelpers::GetRegionInRadiusAroundPixel(targetPixel, this->HalfWidth);

    itk::Index<2> sourcePixel = ITKHelpers::CreateIndex(source);
    itk::ImageRegion<2> sourceRegion = ITKHelpers::GetRegionInRadiusAroundPixel(sourcePixel, this->HalfWidth);

    std::vector<itk::Offset<2> > validOffsets = this->MaskImage->GetValidOffsetsInRegion(targetRegion);

    std::vector<itk::Index<2> > validPixelsIndicesTargetRegion = ITKHelpers::OffsetsToIndices(validOffsets, targetRegion.GetIndex());
    std::vector<typename TImage::PixelType> validPixelsTargetRegion = ITKHelpers::GetPixelValues(Image, validPixelsIndicesTargetRegion);

    typename TypeTraits<typename TImage::PixelType>::LargerType targetValue = Functor(validPixelsTargetRegion);

    std::vector<itk::Index<2> > validPixelsIndicesSourceRegion = ITKHelpers::OffsetsToIndices(validOffsets, sourceRegion.GetIndex());
    std::vector<typename TImage::PixelType> validPixelsSourceRegion = ITKHelpers::GetPixelValues(Image, validPixelsIndicesSourceRegion);

    typename TypeTraits<typename TImage::PixelType>::LargerType sourceValue = Functor(validPixelsSourceRegion);

    typename TypeTraits<typename TImage::PixelType>::LargerType difference = targetValue - sourceValue;
    // std::cout << this->VisitorName << " Vector of energies: : " << difference << std::endl;

    // Compute the difference
    //computedEnergy = (targetValue - sourceValue).GetNorm();
    computedEnergy = ITKHelpers::SumOfComponentMagnitudes(difference);

    // std::cout << this->VisitorName << " Energy: " << computedEnergy << std::endl;

    if(computedEnergy < this->DifferenceThreshold)
    {
      std::cout << this->VisitorName << ": Match accepted (" << computedEnergy
                << " is less than " << this->DifferenceThreshold << ")" << std::endl << std::endl;
      return true;
    }
    else
    {
      std::cout << this->VisitorName << ": Match rejected (" << computedEnergy
                << " is greater than " << this->DifferenceThreshold << ")" << std::endl << std::endl;
      return false;
    }
  }

};

#endif


// Copyright (C) 2009 The Trustees of Indiana University.

// Use, modification and distribution is subject to 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)

//  Authors: Nick Edmonds
//           Andrew Lumsdaine

#define BOOST_GRAPH_USE_NEW_CSR_INTERFACE

// A test of the distributed compressed sparse row graph type
#include <boost/graph/use_mpi.hpp>
#include <boost/config.hpp>
#include <boost/graph/distributed/compressed_sparse_row_graph.hpp>
#include <boost/graph/distributed/mpi_process_group.hpp>
#include <boost/graph/erdos_renyi_generator.hpp>
#include <boost/random/linear_congruential.hpp>
#include <boost/graph/breadth_first_search.hpp>
#include <boost/test/minimal.hpp>

/****************************************************************************
 * Edge weight generator iterator                                           *
 ****************************************************************************/
template<typename F, typename RandomGenerator>
class generator_iterator
{
public:
  typedef std::input_iterator_tag iterator_category;
  typedef typename F::result_type value_type;
  typedef const value_type&       reference;
  typedef const value_type*       pointer;
  typedef void                    difference_type;

  explicit
  generator_iterator(RandomGenerator& gen, const F& f = F())
    : f(f), gen(&gen)
  {
    value = this->f(gen);
  }

  reference operator*() const  { return value; }
  pointer   operator->() const { return &value; }

  generator_iterator& operator++()
  {
    value = f(*gen);
    return *this;
  }

  generator_iterator operator++(int)
  {
    generator_iterator temp(*this);
    ++(*this);
    return temp;
  }

  bool operator==(const generator_iterator& other) const
  { return f == other.f; }

  bool operator!=(const generator_iterator& other) const
  { return !(*this == other); }

private:
  F f;
  RandomGenerator* gen;
  value_type value;
};

template<typename F, typename RandomGenerator>
inline generator_iterator<F, RandomGenerator>
make_generator_iterator( RandomGenerator& gen, const F& f)
{ return generator_iterator<F, RandomGenerator>(gen, f); }

using namespace boost;
using boost::graph::distributed::mpi_process_group;

typedef int weight_type;

struct EdgeProperty {
  EdgeProperty(weight_type weight = 0) : weight(weight) { }

  weight_type weight;

  template<typename Archiver>
  void serialize(Archiver& ar, const unsigned int /*version*/)
  {
    ar & weight;
  }
};

struct VertexProperty {
  VertexProperty(int d = 0)
    : distance(d) { }

  int distance;

  template<typename Archiver>
  void serialize(Archiver& ar, const unsigned int /*version*/)
  {
    ar & distance;
  }
};

int test_main(int argc, char* argv[])
{
  mpi::environment env(argc, argv);

  int n = 40;
  double prob = 0.1;
  int c = 10;

  mpi_process_group pg;
//   parallel::oned_block_cyclic dist(pg, n);
  parallel::block dist(pg, n);

  minstd_rand gen;

  // No properties
  {
    typedef compressed_sparse_row_graph<directedS, no_property, no_property,
                                        no_property, distributedS<mpi_process_group> >
      Digraph;

    typedef sorted_erdos_renyi_iterator<minstd_rand, Digraph> SortedERIter;
    typedef erdos_renyi_iterator<minstd_rand, Digraph> ERIter;

    std::vector<ERIter::value_type> edges;
    std::vector<graph_traits<Digraph>::vertex_descriptor> sources, targets;
    ERIter start(gen, n, prob);
    for ( ; start != ERIter() ; ++start) {
      edges.push_back(*start);

      if (dist(start->first) == process_id(pg)) {
        sources.push_back(start->first);
        targets.push_back(start->second);
      }
    }

    Digraph g(edges_are_unsorted, ERIter(gen, n, prob), ERIter(), n, pg, dist);
    breadth_first_search(g, vertex(0, g), visitor(bfs_visitor<>()));

    Digraph g2(edges_are_sorted, SortedERIter(gen, n, prob), SortedERIter(), n, pg, dist);
    breadth_first_search(g2, vertex(0, g2), visitor(bfs_visitor<>()));

    Digraph g3(edges_are_unsorted_multi_pass, edges.begin(), edges.end(), n, pg, dist);
    breadth_first_search(g3, vertex(0, g3), visitor(bfs_visitor<>()));

    Digraph g4(distributed_construct_inplace_from_sources_and_targets, sources, targets, n, pg, dist);
    breadth_first_search(g4, vertex(0, g4), visitor(bfs_visitor<>()));

    Digraph g5(n, pg, parallel::oned_block_cyclic(pg, 1));
    breadth_first_search(g5, vertex(0, g5), visitor(bfs_visitor<>()));

    // Test structural modification
#if 0
    typedef graph_traits<Digraph>::vertex_descriptor vertex_descriptor;
    vertex_descriptor v = g.add_vertex();

    std::vector<std::pair<vertex_descriptor, vertex_descriptor> > new_edges;

    new_edges.push_back(std::make_pair(*vertices(g).first, v));
    new_edges.push_back(std::make_pair(v, *vertices(g).first));

    add_edges(new_edges.rbegin(), new_edges.rend(), g);
    add_edges_sorted(new_edges.begin(), new_edges.end(), g);
#endif
  }

  return 0;

  // Edge properties
  {
    typedef compressed_sparse_row_graph<directedS, no_property, EdgeProperty, no_property,
                                        distributedS<mpi_process_group> >
      Digraph;

    typedef sorted_erdos_renyi_iterator<minstd_rand, Digraph> SortedERIter;
    typedef erdos_renyi_iterator<minstd_rand, Digraph> ERIter;

    std::vector<ERIter::value_type> edges;
    std::vector<graph_traits<Digraph>::vertex_descriptor> sources, targets;
    std::vector<EdgeProperty> edge_props;
    uniform_int<int> edge_weight(0, c);

    ERIter start(gen, n, prob);
    for ( ; start != ERIter() ; ++start) {
      edges.push_back(*start);

      if (dist(start->first) == process_id(pg)) {
        sources.push_back(start->first);
        targets.push_back(start->second);
        edge_props.push_back(EdgeProperty(edge_weight(gen)));
      }
    }

    Digraph g(edges_are_unsorted, ERIter(gen, n, prob), ERIter(), n, pg, dist);
    breadth_first_search(g, vertex(0, g), visitor(bfs_visitor<>()));

    Digraph g2(edges_are_sorted, SortedERIter(gen, n, prob), SortedERIter(), n, pg, dist);
    breadth_first_search(g2, vertex(0, g2), visitor(bfs_visitor<>()));

    Digraph g3(edges_are_unsorted_multi_pass, edges.begin(), edges.end(),
	       make_generator_iterator(gen, uniform_int<int>(0, c)), n, pg, dist);
    breadth_first_search(g3, vertex(0, g3), visitor(bfs_visitor<>()));

     Digraph g4(distributed_construct_inplace_from_sources_and_targets, sources,
		targets, edge_props, n, pg, dist);
    breadth_first_search(g4, vertex(0, g4), visitor(bfs_visitor<>()));

    Digraph g5(n, pg, parallel::oned_block_cyclic(pg, 1));
    breadth_first_search(g5, vertex(0, g5), visitor(bfs_visitor<>()));

    // Test structural modification
#if 0
    typedef graph_traits<Digraph>::vertex_descriptor vertex_descriptor;
    vertex_descriptor v = add_vertex(g);

    std::vector<std::pair<vertex_descriptor, vertex_descriptor> > new_edges;
    std::vector<EdgeProperty> new_edge_props;

    new_edges.push_back(std::make_pair(*vertices(g).first, v));
    new_edges.push_back(std::make_pair(v, *vertices(g).first));

    new_edge_props.resize(2);

    add_edges(new_edges.rbegin(), new_edges.rend(), g);
    add_edges_sorted(new_edges.begin(), new_edges.end(), g);

    add_edges(new_edges.rbegin(), new_edges.rend(),
	      new_edge_props.begin(), new_edge_props.end(), g);
    add_edges_sorted(new_edges.begin(), new_edges.end(),
		     new_edge_props.begin(), g);
#endif
  }

  exit(-1);

  // Edge and vertex properties
  {
    typedef compressed_sparse_row_graph<directedS, VertexProperty, EdgeProperty,
                                        no_property,
                                        distributedS<mpi_process_group> >
      Digraph;

    typedef sorted_erdos_renyi_iterator<minstd_rand, Digraph> SortedERIter;
    typedef erdos_renyi_iterator<minstd_rand, Digraph> ERIter;

    std::vector<ERIter::value_type> edges;
    std::vector<graph_traits<Digraph>::vertex_descriptor> sources, targets;
    std::vector<EdgeProperty> edge_props;
    uniform_int<int> edge_weight(0, c);

    ERIter start(gen, n, prob);
    for ( ; start != ERIter() ; ++start) {
      edges.push_back(*start);

      if (dist(start->first) == process_id(pg)) {
        sources.push_back(start->first);
        targets.push_back(start->second);
        edge_props.push_back(EdgeProperty(edge_weight(gen)));
      }
    }

    Digraph g(edges_are_unsorted, ERIter(gen, n, prob), ERIter(), n, pg, dist);
    breadth_first_search(g, vertex(0, g), visitor(bfs_visitor<>()));

    Digraph g2(edges_are_sorted, SortedERIter(gen, n, prob), SortedERIter(), n, pg, dist);
    breadth_first_search(g2, vertex(0, g2), visitor(bfs_visitor<>()));

    Digraph g3(edges_are_unsorted_multi_pass, edges.begin(), edges.end(),
	       make_generator_iterator(gen, uniform_int<int>(0, c)), n, pg, dist);
    breadth_first_search(g3, vertex(0, g3), visitor(bfs_visitor<>()));

    Digraph g4(distributed_construct_inplace_from_sources_and_targets, sources,
               targets, edge_props, n, pg, dist);
    breadth_first_search(g4, vertex(0, g4), visitor(bfs_visitor<>()));

    Digraph g5(n, pg, parallel::oned_block_cyclic(pg, 1));
    breadth_first_search(g5, vertex(0, g5), visitor(bfs_visitor<>()));

    // Test structural modification
#if 0
    typedef graph_traits<Digraph>::vertex_descriptor vertex_descriptor;
    vertex_descriptor v = add_vertex(g);

    std::vector<std::pair<vertex_descriptor, vertex_descriptor> > new_edges;
    std::vector<EdgeProperty> new_edge_props;

    new_edges.push_back(std::make_pair(*vertices(g).first, v));
    new_edges.push_back(std::make_pair(v, *vertices(g).first));
    new_edge_props.resize(2);

    add_edges(new_edges.rbegin(), new_edges.rend(), g);
    add_edges_sorted(new_edges.begin(), new_edges.end(), g);

    add_edges(new_edges.rbegin(), new_edges.rend(),
	      new_edge_props.begin(), new_edge_props.end(), g);

    add_edges_sorted(new_edges.begin(), new_edges.end(),
		     new_edge_props.begin(), g);

    add_vertex(VertexProperty(), g);
#endif
  }

  return 0;
}


///////////////////////////////////////////////////////////////
//  Copyright 2012 - 2021 John Maddock.
//  Copyright 2021 Matt Borland.
//  Distributed under the Boost Software License, Version 1.0.
//  See accompanying file LICENSE_1_0.txt or copy at https://www.boost.org/LICENSE_1_0.txt

#ifndef BOOST_MP_CPP_INT_CORE_HPP
#define BOOST_MP_CPP_INT_CORE_HPP

#include <cstdint>
#include <type_traits>
#include <limits>
#include <boost/multiprecision/detail/standalone_config.hpp>
#include <boost/multiprecision/detail/number_base.hpp>
#include <boost/multiprecision/detail/assert.hpp>

namespace boost {
namespace multiprecision {

namespace detail {

//
// These traits calculate the largest type in the list
// [unsigned] long long, long, int, which has the specified number
// of bits.  Note that int_t and uint_t find the first
// member of the above list, not the last.  We want the last in the
// list to ensure that mixed arithmetic operations are as efficient
// as possible.
//

template <unsigned Bits>
struct int_t
{
   using exact = typename std::conditional<Bits == sizeof(signed char) * CHAR_BIT, signed char,
                 typename std::conditional<Bits == sizeof(short) * CHAR_BIT, short,
                 typename std::conditional<Bits == sizeof(int) * CHAR_BIT, int,
                 typename std::conditional<Bits == sizeof(long) * CHAR_BIT, long,
                 typename std::conditional<Bits == sizeof(long long) * CHAR_BIT, long long, void
                 >::type>::type>::type>::type>::type;

   static_assert(!std::is_same<void, exact>::value, "Number of bits does not match any standard data type. \
      Please file an issue at https://github.com/boostorg/multiprecision/ referencing this error from cpp_int_config.hpp");
};

template <unsigned Bits>
struct uint_t
{
   using exact = typename std::conditional<Bits == sizeof(unsigned char) * CHAR_BIT, unsigned char,
                 typename std::conditional<Bits == sizeof(unsigned short) * CHAR_BIT, unsigned short,
                 typename std::conditional<Bits == sizeof(unsigned int) * CHAR_BIT, unsigned int,
                 typename std::conditional<Bits == sizeof(unsigned long) * CHAR_BIT, unsigned long,
                 typename std::conditional<Bits == sizeof(unsigned long long) * CHAR_BIT, unsigned long long, void
                 >::type>::type>::type>::type>::type;

   static_assert(!std::is_same<void, exact>::value, "Number of bits does not match any standard data type. \
      Please file an issue at https://github.com/boostorg/multiprecision/ referencing this error from cpp_int_config.hpp");
};

template <unsigned N>
struct largest_signed_type
{
   using type = typename std::conditional<
       1 + std::numeric_limits<long long>::digits == N,
       long long,
       typename std::conditional<
           1 + std::numeric_limits<long>::digits == N,
           long,
           typename std::conditional<
               1 + std::numeric_limits<int>::digits == N,
               int,
               typename int_t<N>::exact>::type>::type>::type;
};

template <unsigned N>
struct largest_unsigned_type
{
   using type = typename std::conditional<
       std::numeric_limits<unsigned long long>::digits == N,
       unsigned long long,
       typename std::conditional<
           std::numeric_limits<unsigned long>::digits == N,
           unsigned long,
           typename std::conditional<
               std::numeric_limits<unsigned int>::digits == N,
               unsigned int,
               typename uint_t<N>::exact>::type>::type>::type;
};

} // namespace detail

#if defined(BOOST_HAS_INT128)

using limb_type = detail::largest_unsigned_type<64>::type;
using signed_limb_type = detail::largest_signed_type<64>::type;
using double_limb_type = boost::multiprecision::uint128_type;
using signed_double_limb_type = boost::multiprecision::int128_type;
constexpr const limb_type                       max_block_10        = 1000000000000000000uLL;
constexpr const limb_type                       digits_per_block_10 = 18;

inline BOOST_MP_CXX14_CONSTEXPR limb_type block_multiplier(unsigned count)
{
   constexpr const limb_type values[digits_per_block_10] = {10, 100, 1000, 10000, 100000, 1000000, 10000000, 100000000, 1000000000, 10000000000, 100000000000, 1000000000000, 10000000000000, 100000000000000, 1000000000000000, 10000000000000000, 100000000000000000, 1000000000000000000};
   BOOST_MP_ASSERT(count < digits_per_block_10);
   return values[count];
}

// Can't do formatted IO on an __int128
#define BOOST_MP_NO_DOUBLE_LIMB_TYPE_IO

// Need to specialise integer_traits for __int128 as it's not a normal native type:
} // namespace multiprecision

template <>
class integer_traits<multiprecision::double_limb_type>
    : public std::numeric_limits<multiprecision::double_limb_type>,
      public detail::integer_traits_base<multiprecision::double_limb_type, 0, ~static_cast<multiprecision::double_limb_type>(0)>
{};
template <>
class integer_traits<multiprecision::signed_double_limb_type>
    : public std::numeric_limits<multiprecision::signed_double_limb_type>,
      public detail::integer_traits_base<multiprecision::signed_double_limb_type, static_cast<multiprecision::signed_double_limb_type>((static_cast<multiprecision::double_limb_type>(1) << 127)), static_cast<multiprecision::signed_double_limb_type>(((~static_cast<multiprecision::double_limb_type>(0)) >> 1))>
{};

namespace multiprecision {

#else

using limb_type = detail::largest_unsigned_type<32>::type;
using signed_limb_type = detail::largest_signed_type<32>::type  ;
using double_limb_type = detail::largest_unsigned_type<64>::type;
using signed_double_limb_type = detail::largest_signed_type<64>::type  ;
constexpr const limb_type                       max_block_10        = 1000000000;
constexpr const limb_type                       digits_per_block_10 = 9;

inline limb_type block_multiplier(unsigned count)
{
   constexpr const limb_type values[digits_per_block_10] = {10, 100, 1000, 10000, 100000, 1000000, 10000000, 100000000, 1000000000};
   BOOST_MP_ASSERT(count < digits_per_block_10);
   return values[count];
}

#endif

constexpr const unsigned bits_per_limb = sizeof(limb_type) * CHAR_BIT;

template <class T>
inline BOOST_MP_CXX14_CONSTEXPR void minmax(const T& a, const T& b, T& aa, T& bb)
{
   if (a < b)
   {
      aa = a;
      bb = b;
   }
   else
   {
      aa = b;
      bb = a;
   }
}

enum cpp_integer_type
{
   signed_magnitude   = 1,
   unsigned_magnitude = 0,
   signed_packed      = 3,
   unsigned_packed    = 2
};

enum cpp_int_check_type
{
   checked   = 1,
   unchecked = 0
};

} // namespace multiprecision
} // namespace boost

#endif // BOOST_MP_CPP_INT_CORE_HPP


#include "TargetVelPathPlanner.hpp"
#include <Configuration.hpp>
#include <boost/range/irange.hpp>
#include <cmath>
#include "EscapeObstaclesPathPlanner.hpp"
#include "TrapezoidalPath.hpp"

using namespace std;
using namespace Geometry2d;

namespace Planning {

REGISTER_CONFIGURABLE(TargetVelPathPlanner);

ConfigDouble* TargetVelPathPlanner::_targetVelChangeReplanThreshold;

void TargetVelPathPlanner::createConfiguration(Configuration* cfg) {
    _targetVelChangeReplanThreshold = new ConfigDouble(
        cfg, "TargetVelPathPlanner/velChangeReplanThreshold", 0.05,
        "If the target velocity changes by this much (in m/s), the planner is "
        "rerun.  Otherwise the previous path may be kept.");
}

Point TargetVelPathPlanner::calculateNonblockedPathEndpoint(
    Point start, Point dir, const ShapeSet& obstacles) const {
    dir = dir.normalized();

    // TODO(justbuchanan): handle dynamic obstacles (robots)

    float minDist = 0;
    // @maxDist is the diagonal distance across the floor - no robot can travel
    // a straightline distance further than this
    float maxDist =
        sqrtf(powf(Field_Dimensions::Current_Dimensions.FloorLength(), 2) +
              powf(Field_Dimensions::Current_Dimensions.FloorWidth(), 2));

    // We iteratively test different distances from the current point in the
    // direction of the target velocity.  We choose the furthest point away that
    // is non-blocked.  Searching for the furthest non-blocked distance is done
    // through binary search using std::loser_bound.  We use boost ranges to
    // represent a numeric range.  It only works with integers, so we prescale
    // by 100, giving us cm accuracy with the result.
    constexpr float rangeScaleFactor = 100;
    const auto scaledDistRange = boost::irange(
        (int)(minDist * rangeScaleFactor), (int)(maxDist * rangeScaleFactor));
    auto val = std::lower_bound(
        scaledDistRange.begin(), scaledDistRange.end(), obstacles,
        [start, dir, rangeScaleFactor](int scaledDist,
                                       const ShapeSet& obstacles) {
            Segment pathSegment(start,
                                start + dir * (scaledDist / rangeScaleFactor));
            // Returns true if a path of the given distance doesn't hit
            // obstacles
            return !obstacles.hit(pathSegment);
        });
    const float nonblockedPathLen = *val / rangeScaleFactor;

    return start + dir * nonblockedPathLen;
}

bool TargetVelPathPlanner::shouldReplan(const PlanRequest& planRequest) const {
    const Path* prevPath = planRequest.prevPath.get();
    const ShapeSet& obstacles = planRequest.obstacles;

    const WorldVelTargetCommand& command =
        static_cast<const WorldVelTargetCommand&>(*planRequest.motionCommand);

    if (SingleRobotPathPlanner::shouldReplan(planRequest)) return true;

    // See if obstacles have changed such that the end point is significantly
    // different
    const Point newEndpoint = calculateNonblockedPathEndpoint(
        prevPath->start().motion.pos, command.worldVel, obstacles);
    const float endChange = (newEndpoint - prevPath->end().motion.pos).mag();
    if (endChange > SingleRobotPathPlanner::goalChangeThreshold()) {
        return true;
    }

    // Replan if the maxSpeed of the previous path differs too much from the
    // command velocity

    if (auto trapezoidalPath = dynamic_cast<const TrapezoidalPath*>(prevPath)) {
        const float velChange =
            command.worldVel.mag() - trapezoidalPath->maxSpeed();
        if (velChange > *_targetVelChangeReplanThreshold) {
            return true;
        }
        return false;
    } else {
        throw std::runtime_error(
            "TargetVelPathPlanner expected a prevPath of type "
            "'TrapezoidalPath'");
    }
}

// TODO(justbuchanan): Paths aren't dynamically feasible sometimes because it
// doesn't account for initial velocity
std::unique_ptr<Path> TargetVelPathPlanner::run(PlanRequest& planRequest) {
    const MotionInstant& startInstant = planRequest.start;
    const MotionCommand& cmd = *planRequest.motionCommand;
    const auto& motionConstraints = planRequest.constraints.mot;
    const Geometry2d::ShapeSet& obstacles = planRequest.obstacles;
    std::unique_ptr<Path>& prevPath = planRequest.prevPath;

    // If the start point is in an obstacle, escape from it
    if (obstacles.hit(startInstant.pos)) {
        EscapeObstaclesPathPlanner escapePlanner;
        EmptyCommand emptyCommand;
        return escapePlanner.run(planRequest);
    }

    const WorldVelTargetCommand& command =
        static_cast<const WorldVelTargetCommand&>(cmd);

    if (shouldReplan(planRequest)) {
        // Choose the furthest endpoint we can that doesn't hit obstacles
        Point endpoint = calculateNonblockedPathEndpoint(
            startInstant.pos, command.worldVel, obstacles);
        MotionConstraints moddedConstraints = motionConstraints;
        moddedConstraints.maxSpeed = command.worldVel.mag();

        // Make a path from the start point in the direction of the target vel
        // that ends at the calculated endpoint
        auto path = std::unique_ptr<Path>(
            new TrapezoidalPath(startInstant.pos, startInstant.vel.mag(),
                                endpoint, 0, moddedConstraints));
        path->setStartTime(RJ::now());
        return std::move(path);
    } else {
        return std::move(prevPath);
    }
}

}  // namespace Planning


#include <vector>
#include <sstream>
#include <ext/iostreams/transform_width_encoder.hpp>
#include <ext/iostreams/transform_width_decoder.hpp>

#include <boost/iostreams/compose.hpp>
#include <boost/iostreams/filtering_stream.hpp>
#include <boost/iostreams/device/back_inserter.hpp>
#include <boost/iostreams/filter/test.hpp>

#include <boost/archive/iterators/base64_from_binary.hpp>
#include <boost/archive/iterators/binary_from_base64.hpp>
#include <boost/archive/iterators/transform_width.hpp>

#include <boost/test/unit_test.hpp>
#include "test_files.h"

namespace
{
	struct test_case
	{
		std::string normal;
		std::string single_encoded;
		std::string double_encoded;
		std::string triple_encoded;
	};

	typedef ext::iostreams::iterator_encoder<
		boost::archive::iterators::base64_from_binary<
			boost::archive::iterators::transform_width<const char *, 6, 8>
		>
	> base64_encoder_functor;

	typedef ext::iostreams::iterator_decoder<
		boost::archive::iterators::transform_width<
			boost::archive::iterators::binary_from_base64<const char *>,
			8, 6
		>
	> base64_decoder_functor;

	typedef ext::iostreams::transform_width_encoder<
		base64_encoder_functor, 0,
		3, 4, ext::iostreams::DefaultTransformWidthEncoderBufferSize
	> base64_encoder;

	typedef ext::iostreams::transform_width_decoder<
		base64_decoder_functor,
		4, 3, ext::iostreams::DefaultTransformWidthDecoderBufferSize, true
	> base64_decoder;

	typedef ext::iostreams::transform_width_decoder <
		base64_decoder_functor,
		4, 3, ext::iostreams::DefaultTransformWidthDecoderBufferSize, false
	> strict_base64_decoder;

	template <class Filter, class Range1, class Range2>
	inline bool test_output_filter(const Filter & filter, const Range1 & input, const Range2 & output)
	{
		return boost::iostreams::test_output_filter(filter, input, output);
	}

	template <class Filter, class Range1, class Range2>
	inline bool test_input_filter(const Filter & filter, const Range1 & input, const Range2 & output)
	{
		return boost::iostreams::test_input_filter(filter, input, output);
	}

	template <class Encoder>
	void test_encoder(Encoder encoder, const test_case & test)
	{
		auto double_encoder = boost::iostreams::compose(encoder, encoder);
		auto triple_encoder = boost::iostreams::compose(double_encoder, encoder);

		BOOST_REQUIRE(::test_output_filter(encoder, test.normal, test.single_encoded));
		BOOST_REQUIRE(::test_output_filter(encoder, test.single_encoded, test.double_encoded));
		BOOST_REQUIRE(::test_output_filter(encoder, test.double_encoded, test.triple_encoded));

		BOOST_REQUIRE(::test_output_filter(double_encoder, test.normal, test.double_encoded));
		BOOST_REQUIRE(::test_output_filter(double_encoder, test.single_encoded, test.triple_encoded));
		BOOST_REQUIRE(::test_output_filter(triple_encoder, test.normal, test.triple_encoded));
	}

	template <class Decoder>
	void test_decoder(Decoder decoder, const test_case & test)
	{
		auto double_decoder = boost::iostreams::compose(decoder, decoder);
		auto triple_decoder = boost::iostreams::compose(double_decoder, decoder);

		BOOST_REQUIRE(::test_input_filter(decoder, test.single_encoded, test.normal));
		BOOST_REQUIRE(::test_input_filter(decoder, test.double_encoded, test.single_encoded));
		BOOST_REQUIRE(::test_input_filter(decoder, test.triple_encoded, test.double_encoded));

		// commented due a bug in boost::iostreams
		//BOOST_REQUIRE(::test_input_filter(double_decoder, test.double_encoded, test.normal));
		//BOOST_REQUIRE(::test_input_filter(double_decoder, test.triple_encoded, test.single_encoded));
		//BOOST_REQUIRE(::test_input_filter(triple_decoder, test.triple_encoded, test.normal));
	}

	template <class Encoder>
	void test_encdoer_in_filter_chain(Encoder encoder, const test_case & test, std::vector<int> bufSizes)
	{
		for (int bufSize : bufSizes)
		{
			std::string output;
			{
				boost::iostreams::filtering_ostream os;
				os.push(encoder, bufSize);
				os.push(boost::iostreams::back_inserter(output));

				os << test.normal;
			}
			BOOST_REQUIRE(output == test.single_encoded);

			output.clear();
			{
				boost::iostreams::filtering_ostream os;
				os.push(encoder, bufSize);
				os.push(encoder, bufSize);
				os.push(encoder, bufSize);
				os.push(boost::iostreams::back_inserter(output));

				os << test.normal;
			}
			BOOST_REQUIRE(output == test.triple_encoded);
		}
	}

	template <class Decoder>
	void test_decdoer_in_filter_chain(Decoder decoder, const test_case & test, std::vector<int> bufSizes)
	{
		for (int bufSize : bufSizes)
		{
			std::string output;
			{
				boost::iostreams::filtering_istream is;
				is.push(decoder, bufSize);
				is.push(boost::make_iterator_range(test.single_encoded));

				std::stringstream ss;
				ss << is.rdbuf();
				output = ss.str();
			}
			BOOST_REQUIRE(output == test.normal);

			output.clear();
			{
				boost::iostreams::filtering_istream is;
				is.push(decoder, bufSize);
				is.push(decoder, bufSize);
				is.push(decoder, bufSize);
				is.push(boost::make_iterator_range(test.triple_encoded));

				std::stringstream ss;
				ss << is.rdbuf();
				output = ss.str();
			}
			BOOST_REQUIRE(output == test.normal);
		}
	}

}

BOOST_AUTO_TEST_CASE(transform_width_test)
{
	test_case tc;
	LoadTestFile("Base64TestFiles/DataLink32.ps1", tc.normal, std::ios::binary);
	LoadTestFile("Base64TestFiles/DataLink32.ps1.single_encoded", tc.single_encoded, std::ios::binary);
	LoadTestFile("Base64TestFiles/DataLink32.ps1.double_encoded", tc.double_encoded, std::ios::binary);
	LoadTestFile("Base64TestFiles/DataLink32.ps1.triple_encoded", tc.triple_encoded, std::ios::binary);

	test_encoder(base64_encoder(), tc);
	test_decoder(base64_decoder(), tc);
	test_decoder(strict_base64_decoder(), tc);

	auto bufSizes = {-1, 1, 128, 1234, 4096};
	test_encdoer_in_filter_chain(base64_encoder(), tc, bufSizes);
	test_decdoer_in_filter_chain(base64_decoder(), tc, bufSizes);
	test_decdoer_in_filter_chain(strict_base64_decoder(), tc, bufSizes);
}


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

#include <boost/simd/constant/include/constants/nbdigits.hpp>
#include <boost/simd/constant/include/constants/ldexpmask.hpp>
#include <boost/simd/constant/include/constants/maxexponent.hpp>
#include <boost/simd/constant/include/constants/minexponent.hpp>
#include <boost/simd/constant/include/constants/nbmantissabits.hpp>
#include <boost/simd/constant/include/constants/nbexponentbits.hpp>

#endif


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

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

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/constant/constant.hpp>
#include <boost/simd/detail/dispatch/meta/upgrade.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd =  boost::dispatch;
  namespace bs =  boost::simd;
  BOOST_DISPATCH_OVERLOAD ( multiplies_
                          , (typename A0)
                          , bs::sse2_
                          , bs::pack_<bd::double_<A0>, bs::sse_>
                          , bs::pack_<bd::double_<A0>, bs::sse_>
                         )
  {
    BOOST_FORCEINLINE A0 operator() ( const A0 & a0
                                    , const A0 & a1 ) const BOOST_NOEXCEPT
    {
      return _mm_mul_pd(a0,a1);
    }
  };
  BOOST_DISPATCH_OVERLOAD ( multiplies_
                          , (typename A0)
                          , bs::sse2_
                          , bs::pack_<bd::ints8_<A0>, bs::sse_>
                          , bs::pack_<bd::ints8_<A0>, bs::sse_>
                         )
  {
    BOOST_FORCEINLINE A0 operator() ( const A0 & a0
                                    , const A0 & a1 ) const BOOST_NOEXCEPT
    {
     using type = bd::upgrade_t<A0,signed>;
     type mask = Constant<type,0x00FF>();
     return _mm_or_si128 ( _mm_and_si128(mask, _mm_mullo_epi16(a0,a1))
                          , _mm_slli_epi16
                            ( _mm_and_si128 ( mask
                                            , _mm_mullo_epi16
                                              ( _mm_srli_epi16(a0, 8)
                                              , _mm_srli_epi16(a1, 8)
                                              )
                                            )
                            , 8
                            )
                          );
    }
  };

  BOOST_DISPATCH_OVERLOAD ( multiplies_
                          , (typename A0)
                          , bs::sse2_
                          , bs::pack_<bd::ints16_<A0>, bs::sse_>
                          , bs::pack_<bd::ints16_<A0>, bs::sse_>
                         )
  {
    BOOST_FORCEINLINE A0 operator() ( const A0 & a0
                                    , const A0 & a1 ) const BOOST_NOEXCEPT
    {
       return _mm_mullo_epi16(a0, a1);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( multiplies_
                          , (typename A0)
                          , bs::sse2_
                          , bs::pack_<bd::ints32_<A0>, bs::sse_>
                          , bs::pack_<bd::ints32_<A0>, bs::sse_>
                         )
  {

#ifdef __clang__
#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wshadow"
#endif // __clang__

    BOOST_FORCEINLINE A0 operator() ( const A0 & a0
                                    , const A0 & a1 ) const BOOST_NOEXCEPT
    {
      return _mm_or_si128(
                    _mm_and_si128(
                       _mm_mul_epu32(a0,a1),
                       _mm_setr_epi32(0xffffffff,0,0xffffffff,0)
                    )
                  , _mm_slli_si128(
                        _mm_and_si128(
                            _mm_mul_epu32( _mm_srli_si128(a0,4)
                                         , _mm_srli_si128(a1,4)
                                         )
                          , _mm_setr_epi32(0xffffffff,0,0xffffffff,0)
                        )
                      , 4
                    )
             );
    }

#ifdef __clang__
#pragma clang diagnostic pop
#endif // __clang__

  };
} } }

#include <boost/simd/arch/x86/sse2/simd/function/multiplies_s.hpp>

#endif


//  Copyright John Maddock 2010.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_MATH_CONSTANTS_GENERATE_INCLUDED
#define BOOST_MATH_CONSTANTS_GENERATE_INCLUDED

#include <boost/math/constants/constants.hpp>
#include <boost/regex.hpp>
#include <iostream>
#include <iomanip>
#include <sstream>

#ifdef USE_MPFR
#include <boost/math/bindings/mpfr.hpp>
#elif defined(USE_MPREAL)
#include <boost/math/bindings/mpreal.hpp>
#elif defined(USE_CPP_FLOAT)
#include <boost/multiprecision/cpp_dec_float.hpp>
#else
#include <boost/math/bindings/rr.hpp>
#endif

namespace boost{ namespace math{ namespace constants{

#ifdef USE_MPFR
typedef mpfr_class generator_type;
#elif defined(USE_MPREAL)
typedef mpfr::mpreal generator_type;
#elif defined(USE_CPP_FLOAT)
typedef boost::multiprecision::number<boost::multiprecision::cpp_dec_float<500> > generator_type;
#else
typedef ntl::RR generator_type;
#endif

inline void print_constant(const char* name, generator_type(*f)(const std::integral_constant<int, 0>&))
{
#ifdef USE_MPFR
   mpfr_class::set_dprec(((200 + 1) * 1000L) / 301L);
#elif defined(USE_MPREAL)
   mpfr::mpreal::set_default_prec(((200 + 1) * 1000L) / 301L);
#elif defined(USE_CPP_FLOAT)
   // Nothing to do, precision is already set.
#else
   ntl::RR::SetPrecision(((200 + 1) * 1000L) / 301L);
   ntl::RR::SetOutputPrecision(102);
#endif
   generator_type value = f(std::integral_constant<int, 0>());
   std::stringstream os;
   os << std::setprecision(110) << std::scientific;
   os << value;
   std::string s = os.str();
   static const regex e("([+-]?\\d+(?:\\.\\d{0,36})?)(\\d*)(?:e([+-]?\\d+))?");
   smatch what;
   if(regex_match(s, what, e))
   {
      std::cout <<
         "BOOST_DEFINE_MATH_CONSTANT(" << name << ", "
         << what[1] << "e" << (what[3].length() ? what[3].str() : std::string("0")) << ", "
         << "\"" << what[1] << what[2] << "e" << (what[3].length() ? what[3].str() : std::string("0"))
         << "\");" << std::endl;
   }
   else
   {
      std::cout << "Format of numeric constant was not recognised!!" << std::endl;
   }
}

#define BOOST_CONSTANTS_GENERATE(name) \
   boost::math::constants::print_constant(#name, \
   & boost::math::constants::detail::BOOST_JOIN(constant_, name)<boost::math::constants::generator_type>::get)

}}} // namespaces

#endif // BOOST_MATH_CONSTANTS_GENERATE_INCLUDED


//
// Expansion Hunter
// Copyright 2016-2019 Illumina, Inc.
// All rights reserved.
//
// Author: Egor Dolzhenko <edolzhenko@illumina.com>
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//      http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
//

#include "sample/IndexBasedDepthEstimate.hh"

#include <cassert>
#include <iostream>
#include <unordered_set>

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/median.hpp>
#include <boost/accumulators/statistics/stats.hpp>

extern "C"
{
#include "htslib/hts.h"
#include "htslib/sam.h"
}

#include "core/HtsHelpers.hh"

using std::string;
using std::unordered_set;
using namespace boost::accumulators;

namespace ehunter
{

static bool isAutosome(const string& contigName)
{
    static unordered_set<string> autosomeNames
        = { "chr1",  "chr2",  "chr3",  "chr4",  "chr5",  "chr6",  "chr7",  "chr8",  "chr9",  "chr10", "chr11",
            "chr12", "chr13", "chr14", "chr15", "chr16", "chr17", "chr18", "chr19", "chr20", "chr21", "chr22",
            "1",     "2",     "3",     "4",     "5",     "6",     "7",     "8",     "9",     "10",    "11",
            "12",    "13",    "14",    "15",    "16",    "17",    "18",    "19",    "20",    "21",    "22" };

    auto contigNameIter = autosomeNames.find(contigName);
    if (contigNameIter != autosomeNames.end())
    {
        return true;
    }

    return false;
}

double estimateDepthFromHtsIndex(const std::string& htsFilePath, int readLength)
{
    htsFile* htsFilePtr = sam_open(htsFilePath.c_str(), "r");
    if (!htsFilePtr)
    {
        throw std::runtime_error("Failed to open HTS file " + htsFilePath);
    }

    bam_hdr_t* htsHeaderPtr = sam_hdr_read(htsFilePtr);
    if (!htsHeaderPtr)
    {
        throw std::runtime_error("Failed to load header of " + htsFilePath);
    }

    hts_idx_t* htsIndexPtr = sam_index_load(htsFilePtr, htsFilePath.c_str());
    if (!htsIndexPtr)
    {
        throw std::runtime_error("Failed to load index of " + htsFilePath);
    }

    const auto contigInfo = htshelpers::decodeContigInfo(htsHeaderPtr);

    accumulator_set<double, features<tag::median>> contigDepths;

    for (int contigIndex = 0; contigIndex != contigInfo.numContigs(); ++contigIndex)
    {
        uint64_t numMappedReads, numUnmappedReads;
        hts_idx_get_stat(htsIndexPtr, contigIndex, &numMappedReads, &numUnmappedReads);
        if (isAutosome(contigInfo.getContigName(contigIndex)))
        {
            const int64_t contigLength = contigInfo.getContigSize(contigIndex);
            const double contigDepth = (readLength * numMappedReads) / static_cast<double>(contigLength);
            contigDepths(contigDepth);
        }
    }

    return median(contigDepths);
}

}


// #include <boost/math/distributions/normal.hpp> // for normal_distribution
// using boost::math::normal; // typedef provides default type is double.

// #include <iostream>

// int main() {
//     normal s;

// }

#include <boost/random.hpp>
#include <boost/random/normal_distribution.hpp>

#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <netdb.h>

// NOTE TO SELF: SEE THE ntohs() function to swap bytes to proper endian order.
#define OSSY_PORT "60000"	// the port users will be connecting to
#define OSSY_ADDR "127.0.0.1"   // local test address
//#define OSSY_DETECTOR_SIZE 8192 // Max detector X and Y limits
#define OSSY_DETECTOR_SIZE 16384 // Max detector X and Y limits
#define OSSY_PHD_SIZE 256        // Max PHD limit

#define PACKET_SIZE 1470
#define PACKET_LEN PACKET_SIZE/2
#define PACKET_ROWS PACKET_LEN/3
#define PACKET_COLS 3

#define COLX 0
#define COLY 1
#define COLP 2

int main() {
    int sockfd;
    struct addrinfo hints, *servinfo, *p;
    int rv;
    int numbytes;
    uint16_t packet[PACKET_LEN];
    int i = 0;
    //int j = 0;
    //int k = 0;
    int go = 1;
    //unsigned int iseed = 0;
    uint16_t p_count = 0;

    boost::mt19937 rng; // I don't seed it on purpouse (it's not relevant)
    rng.seed(static_cast<unsigned int>(std::time(0)));
    boost::normal_distribution<> nd(8192.0, 400.0);
    boost::normal_distribution<> phd(120.0, 30.0);

    boost::variate_generator<boost::mt19937&,
                             boost::normal_distribution<> > var_nor(rng, nd);

    boost::variate_generator<boost::mt19937&,
                             boost::normal_distribution<> > var_phd(rng, phd);

    //std::cout << nd.max() << std::endl;

    memset(&hints, 0, sizeof hints);
    hints.ai_family = AF_UNSPEC;
    hints.ai_socktype = SOCK_DGRAM;

    if ((rv = getaddrinfo(OSSY_ADDR, OSSY_PORT, &hints, &servinfo)) != 0) {
        fprintf(stderr, "getaddrinfo: %s\n", gai_strerror(rv));
        return 1;
    }

    /* loop through all the results and make a socket */
    for(p = servinfo; p != NULL; p = p->ai_next) {
        if ((sockfd = socket(p->ai_family, p->ai_socktype,
                             p->ai_protocol)) == -1) {
            perror("talker: socket");
            continue;
        }

        break;
    }

    if (p == NULL) {
        fprintf(stderr, "talker: failed to bind socket\n");
        return 2;
    }

    // int i = 0;
    // for (; i < 10; ++i) {
    //     double d = var_nor();
    //     std::cout << d << std::endl;
    // }

    printf("About to loop the datas\n");
    printf("PACKET ROWS:%i \n", PACKET_ROWS);
    while(go) {

        packet[0*PACKET_COLS + 0] = 244;
        packet[0*PACKET_COLS + 1] = p_count;
        packet[0*PACKET_COLS + 2] = 0x0000;

        /* generate fake data */
        for (i = 1; i < PACKET_ROWS; i++) {
            double x = var_nor();
            double y = var_nor();
            double p = var_phd();
            packet[i*PACKET_COLS + COLX] = ((uint16_t)x)%OSSY_DETECTOR_SIZE;
            packet[i*PACKET_COLS + COLY] = ((uint16_t)y)%OSSY_DETECTOR_SIZE;
            packet[i*PACKET_COLS + COLP] = ((uint16_t)p)%OSSY_PHD_SIZE;
        }

        /* shoot it to the listener */
        if ((numbytes = sendto(sockfd, packet, sizeof(packet), 0,
                               p->ai_addr, p->ai_addrlen)) == -1) {
            perror("talker: sendto");
            exit(1);
        }

        p_count++;

        usleep(80);
        //usleep(500000);
        //i = 0;
    }

    freeaddrinfo(servinfo);

    printf("talker: sent %d bytes to %s\n", numbytes, OSSY_ADDR);
    close(sockfd);

    return 0;

}


//-*****************************************************************************
// Copyright 2015 Christopher Jon Horvath
//
// 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.
//-*****************************************************************************

//-*****************************************************************************
// The basic architecture of these Waves is based on the TweakWaves application
// written by Chris Horvath for Tweak Films in 2001.  This, in turn, was based
// on the SIGGRAPH papers and courses by Jerry Tessendorf, and by the paper
// "A Simple Fluid Solver based on the FTT" by Jos Stam.
//
// The TMA, JONSWAP, and Pierson Moskowitz Wave Spectra, as well as the
// directional spreading functions are formulated based on the descriptions
// given in "Ocean Waves: The Stochastic Approach",
// by Michel K. Ochi, published by Cambridge Ocean Technology Series, 1998,2005.
//
// This library is written as a working implementation of the paper:
// Christopher J. Horvath. 2015.
// Empirical directional wave spectra for computer graphics.
// In Proceedings of the 2015 Symposium on Digital Production (DigiPro '15),
// Los Angeles, Aug. 8, 2015, pp. 29-39.
//-*****************************************************************************

#include <EncinoWaves/All.h>

#include <boost/program_options.hpp>

#include <iostream>
#include <stdio.h>
#include <stdlib.h>

namespace ewav = EncinoWaves;
namespace po   = boost::program_options;

//-*****************************************************************************
void doTest(const ewav::Parametersf& i_params) {
  ewav::InitialStatef istate(i_params);
  int w = istate.HSpectralPos.width();
  int h = istate.HSpectralPos.height();
  int N = h;
  std::cout << "A: Computed initial state." << std::endl
            << "Size: " << w << " by " << h << std::endl
            << "HspecPos: " << istate.HSpectralPos[N / 4][N / 4] << std::endl
            << "HspecNeg: " << istate.HSpectralNeg[N / 4][N / 4] << std::endl
            << "Omega: " << istate.Omega[N / 4][N / 4] << std::endl;
}

//-*****************************************************************************
int main(int argc, char* argv[]) {
  ewav::Parametersf params;

  int threads              = -1;
  int dispersion           = 2;
  int spectrum             = 2;
  int directionalSpreading = 0;
  int filter               = 0;
  int random               = 0;
  int seed                 = 54321;

  po::options_description desc("Tweak Waves 2013 : Initial State Test");
  desc.add_options()

    // clang-format off

    ( "help,h", "prints this help message" )

    ( "threads",
      po::value<int>( &threads )
      ->default_value( threads ),
      "Threads to use. Default of -1 means use all." )

    ( "resolution",
      po::value<int>( &params.resolutionPowerOfTwo )
      ->default_value( params.resolutionPowerOfTwo ),
      "Power of Two of the sim resolution" )

    ( "domain",
      po::value<float>( &params.domain )
      ->default_value( params.domain ),
      "Size, in meters, of largest wave" )

    ( "gravity",
      po::value<float>( &params.gravity )
      ->default_value( params.gravity ),
      "Gravitational constant, in meters per second squared" )

    ( "surfaceTension",
      po::value<float>( &params.surfaceTension )
      ->default_value( params.surfaceTension ),
      "Surface tension constant, in Newtons per meter" )

    ( "density",
      po::value<float>( &params.density )
      ->default_value( params.density ),
      "Water density, in kilograms per meter cubed" )

    ( "depth",
      po::value<float>( &params.depth )
      ->default_value( params.depth ),
      "Average depth of the ocean, in meters" )

    ( "windSpeed",
      po::value<float>( &params.windSpeed )
      ->default_value( params.windSpeed ),
      "Average wind speed, in meters per second" )

    ( "fetch",
      po::value<float>( &params.fetch )
      ->default_value( params.fetch ),
      "Wind fetch, in KILOMETERS" )

    ( "pinch",
      po::value<float>( &params.pinch )
      ->default_value( params.pinch ),
      "Lateral displacement, normalized" )

    ( "amplitudeGain",
      po::value<float>( &params.amplitudeGain )
      ->default_value( params.amplitudeGain ),
      "Gain on the wave height" )

    ( "dispersion",
      po::value<int>( &dispersion )
      ->default_value( dispersion ),
      "Dispersion: 0 for Deep, 1 for Finite Depth, 2 for Capillary" )

    ( "spectrum",
      po::value<int>( &spectrum )
      ->default_value( spectrum ),
      "Spectrum: 0 for Pierson-Moskowitz, 1 for JONSWAP, 2 for TMA" )

    ( "directionalSpreading",
      po::value<int>( &directionalSpreading )
      ->default_value( directionalSpreading ),
      "Directional Spreading: 0 for Balanced Cos2 Theta, "
      "1 for Mitsuyasu, 2 for Hasselmann, 3 for Donelan-Banner" )

    ( "swell",
      po::value<float>( &params.directionalSpreading.swell )
      ->default_value( params.directionalSpreading.swell ),
      "The mix between a wind-driven local sea and a swell caused by a "
      "distant storm." )

    ( "filter",
      po::value<int>( &filter )
      ->default_value( filter ),
      "Filter: 0 for nullptr, 1 for Smoothed Invertible Band-Pass" )

    ( "filterSoftWidth",
      po::value<float>( &params.filter.softWidth )
      ->default_value( params.filter.softWidth ),
      "Size in meters of the softness of wavelength filter falloff." )

    ( "filterSmall",
      po::value<float>( &params.filter.smallWavelength )
      ->default_value( params.filter.smallWavelength ),
      "Size in meters of the smallest kept wavelengths." )

    ( "filterBig",
      po::value<float>( &params.filter.bigWavelength )
      ->default_value( params.filter.bigWavelength ),
      "Size in meters of the biggest kept wavelengths." )

    ( "filterMin",
      po::value<float>( &params.filter.min )
      ->default_value( params.filter.min ),
      "Minimum value of filter." )

    ( "filterInvert", "Invert the filter" )

    ( "random",
      po::value<int>( &random )
      ->default_value( random ),
      "Random Distribution: 0 for Normal, 1 for Log-Normal" )

    ( "seed",
      po::value<int>( &params.random.seed )
      ->default_value( params.random.seed ),
      "Random Seed" )

    ;

  // clang-format on

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

  //-*************************************************************************
  if (vm.count("help")) {
    std::cout << desc << std::endl;
    return 0;
  }

  if (vm.count("filterInvert")) {
    params.filter.invert = true;
  }

  params.dispersion.type = (ewav::DispersionType)dispersion;
  params.spectrum.type = (ewav::SpectrumType)spectrum;
  params.directionalSpreading.type =
    (ewav::DirectionalSpreadingType)directionalSpreading;
  params.filter.type = (ewav::FilterType)filter;
  params.random.type = (ewav::RandomType)random;
  params.random.seed = seed;

  doTest(params);

  return 0;
}


#include "polynomial.hpp"
#include "vectorize.hpp"
#include <boost/python.hpp>
#include <sstream>

namespace gd {
using namespace boost::python;

template<int degree, class Ctor>
void py_export_polynomial_lagrange() {
	typedef Polynomial<1, degree, LagrangeBasis<degree>> P;
	stringstream name;
	name << "PolynomialLagrange" << degree;
	object c = class_< P, boost::noncopyable >(name.str().c_str(), Ctor())
		.def("__call__", &P::operator())
		;
	c.attr("degree") = degree;
}

template<int N>
void py_export_hermite_basis() {
	typedef HermiteBasis<N> B;
	stringstream name;
	name << "HermiteBasis" << N;
	object c = class_< B, boost::noncopyable >(name.str().c_str(), init<>())
		.def("__call__", vectorize(&B::operator()))
		;
	//c.attr("degree") = degree;
}

void py_export_polynomial() {
	//py_export_polynomial_lagrange<0>();
	py_export_polynomial_lagrange<0, init<double>>();
	py_export_polynomial_lagrange<1, init<double, double>>();
	py_export_polynomial_lagrange<2, init<double, double, double>>();
	py_export_polynomial_lagrange<3, init<double, double, double, double>>();
	py_export_polynomial_lagrange<4, init<double, double, double, double, double>>();
	py_export_polynomial_lagrange<5, init<double, double, double, double, double, double>>();
	py_export_polynomial_lagrange<6, init<double, double, double, double, double, double, double>>();
	py_export_hermite_basis<0>();
	py_export_hermite_basis<1>();
	py_export_hermite_basis<2>();
	py_export_hermite_basis<3>();
	py_export_hermite_basis<4>();
	py_export_hermite_basis<5>();
	py_export_hermite_basis<6>();
}

}


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file    testMagFactor.cpp
 * @brief   Unit test for MagFactor
 * @author  Frank Dellaert
 * @date   January 29, 2014
 */

#include <gtsam/navigation/MagFactor.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>

#include <boost/bind/bind.hpp>

#include <CppUnitLite/TestHarness.h>

#include <GeographicLib/LocalCartesian.hpp>

using namespace std::placeholders;
using namespace std;
using namespace gtsam;
using namespace GeographicLib;

// *************************************************************************
// Convert from Mag to ENU
// ENU Origin is where the plane was in hold next to runway
// const double lat0 = 33.86998, lon0 = -84.30626, h0 = 274;

// Get field from http://www.ngdc.noaa.gov/geomag-web/#igrfwmm
// Declination = -4.94 degrees (West), Inclination = 62.78 degrees Down
// As NED vector, in nT:
Point3 nM(22653.29982, -1956.83010, 44202.47862);
// Let's assume scale factor,
double scale = 255.0 / 50000.0;
// ...ground truth orientation,
Rot3 nRb = Rot3::Yaw(-0.1);
Rot2 theta = nRb.yaw();
// ...and bias
Point3 bias(10, -10, 50);
// ... then we measure
Point3 scaled = scale * nM;
Point3 measured = nRb.inverse() * (scale * nM) + bias;

double s(scale * nM.norm());
Unit3 dir(nM);

SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25);

using boost::none;

// *************************************************************************
TEST( MagFactor, unrotate ) {
  Matrix H;
  Point3 expected(22735.5, 314.502, 44202.5);
  EXPECT( assert_equal(expected, MagFactor::unrotate(theta,nM,H),1e-1));
  EXPECT( assert_equal(numericalDerivative11<Point3,Rot2> //
      (std::bind(&MagFactor::unrotate, _1, nM, none), theta), H, 1e-6));
}

// *************************************************************************
TEST( MagFactor, Factors ) {

  Matrix H1, H2, H3;

  // MagFactor
  MagFactor f(1, measured, s, dir, bias, model);
  EXPECT( assert_equal(Z_3x1,f.evaluateError(theta,H1),1e-5));
  EXPECT( assert_equal((Matrix)numericalDerivative11<Vector,Rot2> //
      (std::bind(&MagFactor::evaluateError, &f, _1, none), theta), H1, 1e-7));

// MagFactor1
  MagFactor1 f1(1, measured, s, dir, bias, model);
  EXPECT( assert_equal(Z_3x1,f1.evaluateError(nRb,H1),1e-5));
  EXPECT( assert_equal(numericalDerivative11<Vector,Rot3> //
      (std::bind(&MagFactor1::evaluateError, &f1, _1, none), nRb), H1, 1e-7));

// MagFactor2
  MagFactor2 f2(1, 2, measured, nRb, model);
  EXPECT( assert_equal(Z_3x1,f2.evaluateError(scaled,bias,H1,H2),1e-5));
  EXPECT( assert_equal(numericalDerivative11<Vector,Point3> //
      (std::bind(&MagFactor2::evaluateError, &f2, _1, bias, none, none), scaled),//
      H1, 1e-7));
  EXPECT( assert_equal(numericalDerivative11<Vector,Point3> //
      (std::bind(&MagFactor2::evaluateError, &f2, scaled, _1, none, none), bias),//
      H2, 1e-7));

// MagFactor2
  MagFactor3 f3(1, 2, 3, measured, nRb, model);
  EXPECT(assert_equal(Z_3x1,f3.evaluateError(s,dir,bias,H1,H2,H3),1e-5));
  EXPECT(assert_equal((Matrix)numericalDerivative11<Vector,double> //
      (std::bind(&MagFactor3::evaluateError, &f3, _1, dir, bias, none, none, none), s),//
      H1, 1e-7));
  EXPECT(assert_equal(numericalDerivative11<Vector,Unit3> //
      (std::bind(&MagFactor3::evaluateError, &f3, s, _1, bias, none, none, none), dir),//
      H2, 1e-7));
  EXPECT(assert_equal(numericalDerivative11<Vector,Point3> //
      (std::bind(&MagFactor3::evaluateError, &f3, s, dir, _1, none, none, none), bias),//
      H3, 1e-7));
}

// *************************************************************************
int main() {
  TestResult tr;
  return TestRegistry::runAllTests(tr);
}
// *************************************************************************