#include <UNet/UNetDataset.hpp>
#include <UNet/DarknetParser.hpp>

#include <torch/script.h>

#ifdef _MSC_VER
#include <filesystem>
namespace fs = std::filesystem;
#else
#include <experimental/filesystem>
namespace fs = std::experimental::filesystem;
#endif

#include <boost/algorithm/string/split.hpp>

auto diceLoss(torch::Tensor predict, torch::Tensor masks, double smooth = 1.0)
{
    predict = predict.contiguous();
    masks   = masks.contiguous();

    auto intersection = (predict * masks).sum(2).sum(2);
    auto loss = (1. - ((2. * intersection + smooth) / (predict.sum(2).sum(2) + masks.sum(2).sum(2) + smooth)));

    return loss.mean();
}

auto calcLoss(torch::Tensor const& predict, torch::Tensor const& masks, std::map<std::string, float>& metrics,
              double bce_weight = 0.5) -> torch::Tensor
{
    using namespace torch::nn::functional;
    auto bce  = torch::binary_cross_entropy_with_logits(predict, masks);
    auto dice = diceLoss(predict, masks);
    auto loss = bce * bce_weight + dice * (1 - bce_weight);

    metrics["bce"]  += bce.cpu().template item<float>();
    metrics["dice"] += dice.cpu().template item<float>();
    metrics["loss"] += loss.cpu().template item<float>();

    return loss;
}

void printMetrics(std::map<std::string, float>& metrics, size_t datasetSize)
{
   std::cout << "bce: " << metrics["bce"]/datasetSize << ", dice: " << metrics["dice"]/datasetSize << ", loss: " << metrics["loss"]/datasetSize << std::endl;
}

auto toClassesMapsThreshold(cv::Mat const& score,
                            cv::Size const& inputSize,
                            std::vector<float> threshold) -> std::vector<cv::Mat>
{
   auto const rows = score.size[3];
   auto const cols = score.size[2];
   auto const channels = score.size[1];
   auto const batches = score.size[0];
   auto step0 = score.step.p[0];
   auto classesMaps = std::vector<cv::Mat>(static_cast<size_t>(channels) * batches);
   for (auto bt = 0; bt < batches; ++bt)
   {
       for (auto ch = 0; ch < channels; ++ch)
       {
           cv::Mat channelScore = cv::Mat(rows, cols, CV_32FC1, const_cast<float *>(score.ptr<float>(bt, ch, 0)));
           cv::inRange(channelScore, threshold[ch], 1000000.0, classesMaps[ch + (bt*channels)]);
           if ((inputSize.width != 0) && (inputSize.height != 0))
           {
               cv::resize(classesMaps[ch], classesMaps[ch], inputSize);
           }
       }
   }
   return classesMaps;
}

template <typename DataLoader>
void train(size_t epoch, Darknet& model, torch::Device device, DataLoader& data_loader, torch::optim::Optimizer& optimizer, std::function<void(int32_t, int32_t)>&& step = [](int32_t, int32_t){})
{
   std::cout << "=======================" << std::endl;
   std::cout << "Epoch: " << epoch << std::endl;
   std::map<std::string, float> metrics;

   std::cout << "device:" << device << std::endl;
   model->train();
   auto const count = std::distance(data_loader.begin(), data_loader.end());
   auto current = 0;
   for (auto& batch : data_loader)
   {
      torch::Tensor data = batch.data.to(device);
      auto targets = batch.target.to(device);

      optimizer.zero_grad();
      auto output = model->forward(data);
      auto loss = calcLoss(output, targets, metrics);
      loss.backward();
      optimizer.step();
      step(count, current++);
   }
   printMetrics(metrics, std::distance(data_loader.begin(), data_loader.end()));
}

template <typename DataLoader>
void valid(Darknet& model,
           torch::Device device,
           DataLoader& data_loader,
           std::function<void(torch::Tensor&, torch::Tensor&)>&& handler = [](torch::Tensor&, torch::Tensor&){},
           std::function<void(std::map<std::string, float>)>&& metricsHandler = [](auto&&){})
{
   std::cout << "-----------------------" << std::endl;
   std::cout << "Validation: " << std::endl;
   std::map<std::string, float> metrics;

   model->eval();
   torch::NoGradGuard no_grad;
   for (const auto& batch : data_loader)
   {
      auto data = batch.data.to(device);
      auto targets = batch.target.to(device);
      auto output = model->forward(data);

      calcLoss(output, targets, metrics);
      handler(output, targets);
   }
   auto datasetSize = std::distance(data_loader.begin(), data_loader.end());
   printMetrics(metrics, datasetSize);
   metrics["bce"]/=datasetSize;
   metrics["dice"]/=datasetSize;
   metrics["loss"]/=datasetSize;
   metricsHandler(metrics);
   std::cout << "=======================" << std::endl;
}

auto toColorMask(std::vector<cv::Mat> const& masks, std::vector<cv::Scalar> const& colors) -> cv::Mat
{
   cv::Mat coloredMasks(masks[0].rows, masks[0].cols, CV_8UC3, cv::Scalar(0, 0, 0));
   for (size_t i = 0; i < masks.size(); ++i)
   {
      coloredMasks.setTo(colors[i], masks[i]);
   }
   return coloredMasks;
}

#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/json_parser.hpp>

bool CountingLabeledObjects(std::map<std::string, uint32_t>& map, std::string const& polygonInfo, bool forImage = false)
{
   boost::property_tree::ptree pt;
   try
   {
      boost::property_tree::read_json(polygonInfo, pt);
   }
   catch (boost::property_tree::json_parser::json_parser_error& ex)
   {
      return false;
   }

   std::set<std::string> classesSetForImage;
   auto shapes = pt.get_child("shapes");
   for (auto const& shape : shapes)
   {
      auto label = shape.second.get<std::string>("label");
      if (forImage)
      {
         classesSetForImage.insert(label);
      }
      else
      {
         map[label]++;
      }
   }
   for (auto const& item : classesSetForImage)
   {
      map[item]++;
   }
   return true;
}

void LabelMeDeleteImage(std::string const& polygonInfo)
{
  boost::property_tree::ptree pt;
  try
  {
    boost::property_tree::read_json(polygonInfo, pt);
    pt.put<std::string>("imageData", {});
    boost::property_tree::write_json(polygonInfo, pt);
  }
  catch (boost::property_tree::json_parser::json_parser_error& ex)
  {
    return;
  }
}

auto ConvertPolygonsToMask(std::string const& polygonInfo, std::map<std::string, cv::Scalar> colorToClass) -> cv::Mat
{
   boost::property_tree::ptree pt;
   try
   {
     boost::property_tree::read_json(polygonInfo, pt);
   }
   catch (boost::property_tree::json_parser::json_parser_error& ex)
   {
     return {};
   }

   cv::Mat mask = cv::Mat::zeros(pt.get<int>("imageHeight"), pt.get<int>("imageWidth"), CV_8UC3);
   auto shapes = pt.get_child("shapes");
   for (auto const& shape : shapes)
   {
      auto points = shape.second.get_child("points");
      std::vector<cv::Point> cv_points;
      for (auto const& point : points)
      {
         std::vector<float> vecxy;
         for (const auto& pointxy : point.second) {
            vecxy.emplace_back(pointxy.second.get_value<float>());
         }
         cv_points.emplace_back(cv::Point{static_cast<int>(vecxy[0]), static_cast<int>(vecxy[1])});
      }
      auto label = shape.second.get<std::string>("label");
      auto color = colorToClass[label];
      cv::fillPoly(mask, std::vector<std::vector<cv::Point>>{cv_points}, color);
   }
   return mask.clone();
}

auto CountingLabeledObjectsInMultipleDatasetFolders(std::vector<std::string> const& datasetFolderPathes) -> std::map<std::string, uint32_t>
{
   std::map<std::string, uint32_t> countResult;
   for (auto const& datasetFolderPath : datasetFolderPathes)
   {
      for (auto file : fs::directory_iterator(datasetFolderPath))
      {
         CountingLabeledObjects(countResult, file.path().string(), true);
      }
   }
   return countResult;
}

void ConvertPolygonsToMaskInMultipleDatasetFolders(std::vector<std::pair<std::string, std::string>> const& datasetFolderPathes,
                                                   cv::Rect const& roi = {},
                                                   std::map<std::string, cv::Scalar> const& colorToClass = {},
                                                   std::function<bool(cv::Mat const&)>&& skipPredicat = [](cv::Mat const&) -> bool { return false; })
{
   for (auto datasetFolderPath : datasetFolderPathes)
   {
      fs::create_directories(datasetFolderPath.second);
      for (auto file : fs::directory_iterator(datasetFolderPath.first))
      {
         if (fs::is_directory(file))
         {
           continue;
         }
         cv::Mat mask = ConvertPolygonsToMask(file.path().string(), colorToClass);
         if (skipPredicat(mask))
         {
            continue;
         }
         auto filename = file.path().filename().string();
         filename = filename.substr(0, filename.size() - 4);
         cv::Mat maskCropped = roi.empty() ? mask : mask(roi);
         cv::imwrite(datasetFolderPath.second + filename + "png", maskCropped);
      }
   }
}

void ConvertImagesInMultipleDatasetFolders(std::vector<std::pair<std::string, std::string>> const& datasetFolderPathes,
                                           cv::Rect const& roi = {}, bool isClahe = false, cv::Size const& newSize = {},
                                           std::function<bool(cv::Mat const&)>&& skipPredicat = [](cv::Mat const&) -> bool { return false; })
{
   for (auto datasetFolderPath : datasetFolderPathes)
   {
      fs::create_directories(datasetFolderPath.second);
      for (auto file : fs::directory_iterator(datasetFolderPath.first))
      {
         cv::Mat image = cv::imread(file.path().string());
         if (skipPredicat(image))
         {
            continue;
         }
         if (isClahe)
         {
           auto clahe = cv::createCLAHE();
           cv::cvtColor(image, image, cv::COLOR_BGR2GRAY);
           clahe->apply(image, image);
         }
         auto filename = file.path().filename().string();
         filename = filename.substr(0, filename.size() - 4);
         cv::Mat imageCropped = roi.empty() ? image : image(roi);
         if (!newSize.empty())
         {
             cv::resize(imageCropped, imageCropped, newSize, cv::INTER_NEAREST);
         }
         cv::imwrite(datasetFolderPath.second + filename + ".png", imageCropped);
      }
   }
}

void iterateOverLinesInContours(std::vector<cv::Point> contour, std::function<void(cv::Point const& p1, cv::Point const& p2)>&& callback)
{
   if (contour.size() < 2)
   {
      return;
   }
   for (size_t i = 0; i < (contour.size() - 1); ++i)
   {
      callback(contour[i], contour[i + 1]);
   }
}

void TestDatasetCreateList(std::pair<std::string, std::string> const& outputDirs,
                           cv::Size const& size,
                           std::vector<cv::Scalar> const& classColors,
                           size_t count = 100)
{
   fs::create_directories(outputDirs.first);
   fs::create_directories(outputDirs.second);
   for (auto current = 0U; current < count; ++current)
   {
      auto datasetTest = TestDatasetCreate(size, classColors);
      datasetTest.first *= 255;
      datasetTest.first.convertTo(datasetTest.first, CV_8UC3);

      cv::imwrite(outputDirs.first + std::to_string(current) + ".png", datasetTest.first);
      cv::imwrite(outputDirs.second + std::to_string(current) + ".png", datasetTest.first);
   }
}

char const* const help = "\nUsage:\n\ttrain_unet_darknet2d options"
                         "\n\nNote:"
                         "\n\t <path-to-checkpoints-output>            - Should have enough storage capacity."
                         "\n\t"
                         "\n\nOptions"
                         "\n\t--help                                   - Prints this information."
                         "\n\t"
                         "\n\t--generate-custom-unet=<input-channels-count>,<output-classes-count>,<encoder-decoder-levels-count>,<initial-feature-count>,<model path>"
                         "\n\t                                         - Creates custom unet model config file(NOTE: without weights!)"
                         "\n\t"
                         "\n\t--test-dataset-create=<path-images>,<path-masks>,<width>,<height>,<count>,<b>,<g>,<r>,<b>,<g>,<r>..."
                         "\n\t                                         - Creates test dataset with circles, rectangles etc to specified "
                         "\n\t                                         folder pathes."
                         "\n\t"
                         "\n\t--count-labels=<path>,<path>,...         - Select directories for counting labels."
                         "\n\t"
                         "\n\t--convert-polygons-to-masks=<left>,<top>,<width>,<height>,<path-polygons>,<path-out-masks>,<path-polygons>,<path-out-masks>,..."
                         "\n\t                                         - Select directories for convertation polygons to masks."
                         "\n\t"
                         "\n\t--convert-images=<left>,<top>,<width>,<height>,<path-images>,<path-masks>..."
                         "\n\t                                         - Select directories for convertation images to needed size."
                         "\n\t"
                         "\n\t--model-darknet=<path-to-darknet-model>[,<pretrained-weights-path>]"
                         "\n\t                                         - Path to darknet unet model."
                         "\n\t                                           Optionally - path to weights learned previous."
                         "\n\t"
                         "\n\t--epochs=<count>                         - Epochs count."
                         "\n\t"
                         "\n\t--batch-count=<count>                    - Batch count."
                         "\n\t"
                         "\n\t--checkpoints-output=<path-to-checkpoints-output>"
                         "\n\t                                         - Path to output where will be created checkpoints folder."
                         "\n\t"
                         "\n\t--train-directories=<path-images>,<path-masks>,<path-images>,<path-masks>,..."
                         "\n\t                                         - List with pairs paths to images and to masks."
                         "\n\t"
                         "\n\t--valid-directories=<path-images>,<path-masks>,<path-images>,<path-masks>,..."
                         "\n\t                                         - List with pairs paths to images and to masks."
                         "\n\t"
                         "\n\t--colors-to-class-map=<classname>,<b>,<g>,<r>,..."
                         "\n\t                                         - List with tuple of 4 values class name and 3 color component "
                         "\n\t                                           coresponded to its mask color in the dataset."
                         "\n\t"
                         "\n\t--selected-classes-and-thresholds=<classname>,<threshold>,<classname>,<threshold>,..."
                         "\n\t                                         - List with pairs class name and threshold."
                         "\n\t"
                         "\n\t--size-downscaled=<width>,<height>       - Width and height."
                         "\n\t"
                         "\n\t--grayscale=yes|no                       - Turn on|off color mode."
                         "\n\t"
                         "\n\t--best_weights_only=yes|no               - Turn on|off saving best weights only."
                         "\n\t"
                         "\n\nExample:"
                         "\n\t./build_host/train_unet_darknet2d \\"
                         "\n\t --model-darknet=./model/unet3c2cl2l8f.cfg \\"
                         "\n\t --epochs=100 \\"
                         "\n\t --checkpoints-output=./checkpoints_128x128_test \\"
                         "\n\t --train-directories=./dataset/train/imgs,./dataset/train/masks \\"
                         "\n\t --valid-directories=./dataset/valid/imgs,./dataset/valid/masks \\"
                         "\n\t --colors-to-class-map=\"circle,0,0,255,rectangle,0,0,255,disk,0,0,255\" \\"
                         "\n\t --selected-classes-and-thresholds=circle,0.3,rectangle,0.3,disk,0.3 \\"
                         "\n\t --batch-count=1 \\"
                         "\n\t --size-downscaled=128,128 \\"
                         "\n\t --best-weights-only=no";

auto throw_an_error = [](std::string const& message)
{
  std::cout << message + "\nSee usage:\n\n" + help << std::endl;
};

auto ParseOptions(int argc, char *argv[]) -> std::map<std::string, std::vector<std::string>>
{
   std::map<std::string, std::vector<std::string>> params;
   for (auto i = 0; i < argc; ++i)
   {
      auto currentOption = std::string(argv[i]);
      auto delimiter = currentOption.find_first_of('=');
      if (delimiter != std::string::npos)
      {
         auto input = currentOption.substr(delimiter + 1);
         boost::split(params[currentOption.substr(0, delimiter)], input, boost::is_any_of(","));
      }
   }
   return params;
}

void runOpts(std::map<std::string, std::vector<std::string>> params)
{
   if (params.count("--help"))
   {
       std::cout << help << std::endl;
       return;
   }
   if (params.count("--generate-custom-unet"))
   {
      if (params["--generate-custom-unet"].size() != 5)
      {
         throw_an_error(std::string("Expected 5 parameters but provided ") + std::to_string(params["--generate-custom-unet"].size()));
         return;
      }
      fs::create_directories(params["--generate-custom-unet"][4]);
      auto isGrayscale = (std::stoi(params["--generate-custom-unet"][0]) == 1);
      auto classCount = std::stoi(params["--generate-custom-unet"][1]);
      auto levelsCount = std::stoi(params["--generate-custom-unet"][2]);
      auto featuresCount = std::stoi(params["--generate-custom-unet"][3]);
      auto modelFile = std::ofstream(params["--generate-custom-unet"][4] + "/unet_" +
                                     params["--generate-custom-unet"][0] + "c" +
                                     params["--generate-custom-unet"][1] + "cl" +
                                     params["--generate-custom-unet"][2] + "l" +
                                     params["--generate-custom-unet"][3] + "f" + ".cfg");
      modelFile << "[net]\n"
                   "# Training\n"
                   "width=256\n"
                   "height=256\n"
                   "#now supported only grayscale and 3 color\n"
                   "channels=" << (isGrayscale ? 1 : 3) << "\n"
                   "learning_rate=1e-7\n"
                   "batch=20\n"
                   "eps=1e-05\n"
                   "momentum=0.1\n"
                   "\n"
                   "decay=0.0005\n"
                   "adam=0\n"
                   "B1=0.9\n"
                   "B2=0.999\n"
                   "max_batches = 400\n";
      for (auto currentLevel = 0; currentLevel < levelsCount; ++currentLevel)
      {
         modelFile << "\n###encoder" << (currentLevel + 1) <<"\n"
                      "[convolutional]\n"
                      "batch_normalize=1\n"
                      "filters=" << featuresCount << "\n"
                      "size=3\n"
                      "stride=1\n"
                      "pad=1\n"
                      "activation=leaky\n"
                      "\n"
                      "[convolutional]\n"
                      "batch_normalize=1\n"
                      "filters=" << featuresCount << "\n"
                      "size=3\n"
                      "stride=1\n"
                      "pad=1\n"
                      "activation=leaky\n"
                      "\n"
                      "[maxpool]\n"
                      "size=2\n"
                      "stride=2\n";
         featuresCount *= 2;
      }
      modelFile << "\n###bottleneck\n"
                   "[convolutional]\n"
                   "batch_normalize=1\n"
                   "filters=" << featuresCount << "\n"
                   "size=3\n"
                   "stride=1\n"
                   "pad=1\n"
                   "activation=leaky\n"
                   "\n"
                   "[convolutional]\n"
                   "batch_normalize=1\n"
                   "filters=" << featuresCount << "\n"
                   "size=3\n"
                   "stride=1\n"
                   "pad=1\n"
                   "activation=leaky\n";
      for (auto currentLevel = 0; currentLevel < levelsCount; ++currentLevel)
      {
         featuresCount /= 2;
         modelFile << "\n###decoder" << levelsCount - currentLevel << "\n"
                      "[upsample]\n"
                      "stride=2\n"
                      "\n"
                      "[convolutional]\n"
                      "filters=" << featuresCount << "\n"
                      "size=3\n"
                      "stride=1\n"
                      "pad=1\n"
                      "activation=leaky\n"
                      "\n"
                      "[route]\n"
                      "layers = -1, -" << (2 + (4 * (currentLevel + 1)) + ((1 + 1 + 2) * currentLevel)) << "\n"
                      "\n"
                      "[convolutional]\n"
                      "batch_normalize=1\n"
                      "filters=" << featuresCount << "\n"
                      "size=3\n"
                      "stride=1\n"
                      "pad=1\n"
                      "activation=leaky\n"
                      "\n"
                      "[convolutional]\n"
                      "batch_normalize=1\n"
                      "filters=" << featuresCount << "\n"
                      "size=3\n"
                      "stride=1\n"
                      "pad=1\n"
                      "activation=leaky\n";
      }
      modelFile << "\n################################\n"
                   "[convolutional]\n"
                   "filters=" << classCount << "\n"
                   "size=1\n"
                   "stride=1\n"
                   "activation=logistic\n"
                   "################################";
      return;
   }

   if ((params["--test-dataset-create"].size() >= 8) && (((params["--test-dataset-create"].size() - 5) % 3) == 0))
   {
      std::vector<cv::Scalar> classColors;
      for (auto colorsIt = std::next(params["--test-dataset-create"].cbegin(), 5);
           colorsIt != params["--test-dataset-create"].cend();
           colorsIt += 3)
      {
         classColors.emplace_back(cv::Scalar(std::stoi(*colorsIt),
                                             std::stoi(*std::next(colorsIt, 1)),
                                             std::stoi(*std::next(colorsIt, 2))));
      }
      TestDatasetCreateList(std::make_pair(params["--test-dataset-create"][0],
                                           params["--test-dataset-create"][1]),
                            cv::Size{std::stoi(params["--test-dataset-create"][2]),
                                     std::stoi(params["--test-dataset-create"][3])},
                            classColors,
                            std::stoi(params["--test-dataset-create"][4]));
      return;
   }
   if (!params["--count-labels"].empty())
   {
      auto map = CountingLabeledObjectsInMultipleDatasetFolders(params["--count-labels"]);
      std::cout << "Count labels list: " << std::endl;
      for (auto const& item : map)
      {
         std::cout << "\t" << item.first << " : " << item.second << std::endl;
      }
   }
   std::map<std::string, cv::Scalar> colorsToClass;
   if (!params["--colors-to-class-map"].empty() && (params["--colors-to-class-map"].size() % 4) == 0)
   {
      for (auto colorToClassIt = params["--colors-to-class-map"].cbegin();
           colorToClassIt != params["--colors-to-class-map"].cend();
           colorToClassIt += 4)
      {
         colorsToClass[*colorToClassIt] = cv::Scalar(
            std::stoi(*std::next(colorToClassIt, 1)), std::stoi(*std::next(colorToClassIt, 2)), std::stoi(*std::next(colorToClassIt, 3)));
      }
   }
   if ((params["--convert-polygons-to-masks"].size() >= 6) && (((params["--convert-polygons-to-masks"].size() - 4) % 2) == 0))
   {
      auto roi = cv::Rect{std::stoi(params["--convert-polygons-to-masks"][0]),
                          std::stoi(params["--convert-polygons-to-masks"][1]),
                          std::stoi(params["--convert-polygons-to-masks"][2]),
                          std::stoi(params["--convert-polygons-to-masks"][3])};
      if (!colorsToClass.empty())
      {
         std::vector<std::pair<std::string, std::string>> polygonsPathsInputAndOutput;
         for (auto polygonsInputAndOutputIt = std::next(params["--convert-polygons-to-masks"].cbegin(), 4);
              polygonsInputAndOutputIt != params["--convert-polygons-to-masks"].cend();
              polygonsInputAndOutputIt += 2)
         {
            polygonsPathsInputAndOutput.emplace_back(std::make_pair(*polygonsInputAndOutputIt, *std::next(polygonsInputAndOutputIt)));
         }
         ConvertPolygonsToMaskInMultipleDatasetFolders(polygonsPathsInputAndOutput, roi, colorsToClass);
      }
      else
      {
         std::cout << "Skipped apply option --convert-polygons-to-masks since no color to masks applied!" << std::endl;
      }
   }
   else
   {
      std::cout << "Skipped apply option --convert-polygons-to-masks since values count less than 6 or values count - 4 not even!" << std::endl;
   }
   if ((params["--convert-images"].size() >= 6) && (((params["--convert-images"].size() - 4) % 2) == 0))
   {
      auto newSize = cv::Size{};
      if (params["--size-downscaled"].size() == 2)
      {
          newSize = cv::Size{std::stoi(params["--size-downscaled"][0]),
                             std::stoi(params["--size-downscaled"][1])};
      }

      auto roi = cv::Rect{std::stoi(params["--convert-images"][0]),
                          std::stoi(params["--convert-images"][1]),
                          std::stoi(params["--convert-images"][2]),
                          std::stoi(params["--convert-images"][3])};
      std::vector<std::pair<std::string, std::string>> imagesPathsInputAndOutput;
      for (auto imagesInputAndOutputIt = std::next(params["--convert-images"].cbegin(), 4);
           imagesInputAndOutputIt != params["--convert-images"].cend();
           imagesInputAndOutputIt += 2)
      {
         imagesPathsInputAndOutput.emplace_back(std::make_pair(*imagesInputAndOutputIt, *std::next(imagesInputAndOutputIt)));
      }
      ConvertImagesInMultipleDatasetFolders(imagesPathsInputAndOutput, roi, (!params["--clahe"].empty() && params["--clahe"][0] == "yes"), newSize);
   }
   else
   {
      std::cout << "Skipped apply option --convert-images since values count less than 6 or values count - 4 not even!" << std::endl;
   }

   if (params["--epochs"].empty() || (std::stoi(params["--epochs"][0]) == 0))
   {
      std::cout << "Skip training since epochs option absent count equal to 0!" << std::endl;
     // return;
   }
   const auto kNumberOfEpochs = std::stoi(params["--epochs"][0]);

   if (params["--checkpoints-output"].empty())
   {
      std::cout <<"Skip training since --checkpoints-output has not been set!" << std::endl;
      //return;
   }
   if (colorsToClass.empty())
   {
      throw_an_error("Skip training since --colors-to-class-map has not been set!");
      return;
   }
   std::vector<std::tuple<std::string, std::string>> trainDirectories;
   if (params["--train-directories"].size() % 2 != 0)
   {
      throw_an_error("Train directories should be even count!\n");
      return;
   }
   for (auto i = 0U; i < params["--train-directories"].size(); i += 2)
   {
      trainDirectories.push_back(std::make_tuple(params["--train-directories"][i],
                                                 params["--train-directories"][i + 1]));
   }

   std::vector<std::tuple<std::string, std::string>> validDirectories;
   if (!(params["--valid-directories"].size() % 2 == 0))
   {
      throw_an_error("--valid-directories should be even count!\n");
      return;
   }
   for (auto i = 0U; i < params["--valid-directories"].size(); i += 2)
   {
      validDirectories.push_back(std::make_tuple(params["--valid-directories"][i], params["--valid-directories"][i + 1]));
   }

   if (params["--model-darknet"].empty())
   {
      throw_an_error("Option --model-darknet does not set!\n");
      return;
   }

   if ((params["--selected-classes-and-thresholds"].size() < 2) ||
       ((params["--selected-classes-and-thresholds"].size() % 2) != 0))
   {
      // TODO: Check for each class selected exist in classesToColor
      throw_an_error("--selected-classes-and-thresholds has not been set!\n");
      return;
   }

   std::vector<cv::Scalar> classColors;
   std::vector<float> thresholds(classColors.size(), 0.3);
   for (auto selectedClassAndThresholdIt = params["--selected-classes-and-thresholds"].cbegin();
        selectedClassAndThresholdIt != params["--selected-classes-and-thresholds"].cend();
        selectedClassAndThresholdIt += 2)
   {
      classColors.push_back(colorsToClass[*selectedClassAndThresholdIt]);
      thresholds.push_back(std::stof(*std::next(selectedClassAndThresholdIt)));
   }

   int batchSize = params["--batch-count"].empty() ? 1 : std::stoi(params["--batch-count"][0]);

   if (params["--size-downscaled"].size() != 2)
   {
      throw_an_error("--size-downscaled should be set!");
      return;
   }

   bool isGrayscale = false;
   if (!params["--grayscale"].empty())
   {
      isGrayscale = (params["--grayscale"][0] == "yes");
   }

   bool isEval = false;
   std::vector<float> thresholdEvalSteps(thresholds.size());
   if (!params["--eval"].empty())
   {
       isEval = (params["--eval"][0] == "yes");
       for (auto i = 0; i < thresholds.size(); ++i)
       {
           thresholdEvalSteps[i] = (1.0f - thresholds[i]) / kNumberOfEpochs;
       }
   }

   auto size = cv::Size{std::stoi(params["--size-downscaled"][0]),
                        std::stoi(params["--size-downscaled"][1])};

   torch::Device device(torch::cuda::is_available() ? torch::kCUDA : torch::kCPU);
   std::cout << "Using device: " << device << std::endl;

   auto train_dataset = UNetDataset(trainDirectories, classColors, size, isGrayscale).map(torch::data::transforms::Stack<>());
   auto valid_dataset = UNetDataset(validDirectories, classColors, size, isGrayscale).map(torch::data::transforms::Stack<>());

   auto train_loader = torch::data::make_data_loader<torch::data::samplers::SequentialSampler>(std::move(train_dataset), batchSize);
   auto valid_loader = torch::data::make_data_loader<torch::data::samplers::SequentialSampler>(std::move(valid_dataset), batchSize);

   if (!fs::exists(params["--model-darknet"][0]))
   {
       std::cout << "ERROR: " << params["--model-darknet"][0] << " does not exist!" << std::endl;
       return;
   }
   Darknet model = Darknet(params["--model-darknet"][0], cv::Size{0, 0}, true);
   std::cout << model->_moduleList << std::endl;

   if (params["--model-darknet"].size() == 2)
   {
       model->load_weights(params["--model-darknet"][1]);
       model->save_weights(params["--model-darknet"][1] + "saved");
   }
   model->to(device);

   std::cout << "UNet2d: " << c10::str(model) << std::endl;

   torch::optim::Adam optimizer(model->parameters(), torch::optim::AdamOptions(0.00001));
   std::map<std::string, std::vector<cv::Point>> trainingLossData;
   std::string outputDirectory = params["--checkpoints-output"][0];
   for (size_t epoch = 1U; epoch <= kNumberOfEpochs; ++epoch)
   {
      if (!isEval)
      {
          train(epoch, model, device, *train_loader, optimizer, [](int32_t count, int32_t current) {
              if ((current % (count / 10)) == 0)
              {
                  std::cout << "\rProgress: [" << count << " / " << current << "]" << std::endl;
              }
              cv::waitKey(1);
          });
          std::cout << std::endl;
      }

      auto i = 0;
      std::vector<double> lossesByClass(classColors.size());
      valid(model, device, *valid_loader, [&](torch::Tensor& predict, torch::Tensor& targets)
      {
        auto const batches = predict.size(0);
        auto const height = predict.size(2);
        auto const width = predict.size(3);
        int sz[] = {batches, static_cast<int>(classColors.size()), width, height};
        auto predictCpu = predict.contiguous().cpu();
        cv::Mat predictMat = cv::Mat(4, sz, CV_32FC1, predictCpu.data_ptr());
        auto masksPredict = toClassesMapsThreshold(predictMat, {}, thresholds);
        for (auto ii = 0U; ii < masksPredict.size(); ++ii)
        {
           cv::imshow(std::string("Predict_") + std::to_string(ii % classColors.size()) + "_" + std::to_string(ii / classColors.size()), masksPredict[ii]);
        }

        auto targetsCpu = targets.contiguous().cpu();
        cv::Mat targetMat = cv::Mat(4, sz, CV_32FC1, targetsCpu.data_ptr());
        auto masksTarget = toClassesMapsThreshold(targetMat, {}, thresholds);
        for (auto ii = 0U; ii < masksTarget.size(); ++ii)
        {
           cv::imshow(std::string("Target_") + std::to_string(ii % classColors.size()) + "_" + std::to_string(ii / classColors.size()), masksTarget[ii]);
        }

        for (auto ii = 0U; ii < masksPredict.size(); ++ii)
        {
            cv::Mat diffMat;
            cv::absdiff(masksTarget[ii], masksPredict[ii], diffMat);
            lossesByClass[ii % classColors.size()] += (cv::sum(diffMat/255.0) / (masksTarget[ii].rows * masksTarget[ii].cols))[0] / classColors.size();
        }
        cv::waitKey(1);
        ++i;
      }, [&](std::map<std::string, float> metrics)
      {
        for (auto ii = 0; ii < lossesByClass.size(); ++ii)
        {
            lossesByClass[ii] /= i;
            std::cout << "Diff for " << std::to_string(ii % lossesByClass.size()) + "_" + std::to_string(ii / lossesByClass.size()) << ": " << lossesByClass[ii] << std::endl;
        }
        static std::map<std::string, cv::Scalar> chartColors = {std::make_pair("bce", cv::Scalar(0x00, 0x00, 0xFF)),
                                                                std::make_pair("dice", cv::Scalar(0x00, 0xFF, 0x00)),
                                                                std::make_pair("loss", cv::Scalar(0xFF, 0x00, 0x00))};
        static std::map<std::string, std::vector<cv::Point>> validationLossData;
        cv::Mat validationChart = cv::Mat(500, 500, CV_8UC3, cv::Scalar{255, 255, 255});
        for (auto const& item : metrics)
        {
           validationLossData[item.first].emplace_back(static_cast<int>((static_cast<float>(500)/kNumberOfEpochs) * epoch),
                                                       500 - (item.second * 500));
           // MSVC lead to error when in lambda below was auto const& parameters(for x86 compiler)
           iterateOverLinesInContours(validationLossData[item.first], [&](cv::Point const& p1, cv::Point const& p2){
             cv::line(validationChart, p1, p2, chartColors[item.first], 3);
           });
           cv::imshow(std::string("Validation chart"), validationChart);
           cv::imwrite(outputDirectory + "/chart.png", validationChart);
           cv::waitKey(1);
        }
        static auto minLoss = 10000.0f;
        static auto bestEpoch = 0;
        if (metrics["loss"] < minLoss)
        {
           if (!isEval)
           {
               fs::remove(outputDirectory + "/" + "best_" + std::to_string(bestEpoch) + ".weights");
               model->save_weights(outputDirectory + "/" + "best_" + std::to_string(epoch) + ".weights");
           }
           minLoss = metrics["loss"];
           bestEpoch = epoch;
           std::cout << "Best epoch: " << epoch << std::endl;
        }
      });
      if (isEval)
      {
          std::cout << "Thresholds: ";
          for (auto const& threshold : thresholds)
          {
              std::cout << threshold << ", ";
          }
          for (auto ii = 0; ii < thresholds.size(); ++ii)
          {
              thresholds[ii] += thresholdEvalSteps[ii];
          }
          std::cout << std::endl;
          continue;
      }
      fs::create_directory(outputDirectory);
      if (params["best_weights_only"].empty() || (params["best_weights_only"][0] == "no"))
      {
          model->save_weights(outputDirectory + "/" + std::to_string(epoch) + ".weights");
      }
   }
}


#include <iostream>
#include <map>
#include <string>
#include <memory>
#include <deque>
#include <vector>
#include <functional>

#include <boost/numeric/mtl/mtl.hpp>

#include "print_compiler.hpp"

using namespace std;
using namespace mtl;

using Mat= matrix<double>;
using Vec= mtl::vector<double>;
using Linear= function<Vec(const Vec&)>;

struct named_linear
{
    void insert(const string& name, const Linear& op) {
	ops[name]= op; }
    void insert(const string& name, Linear&& op) {
	ops[name]= move(op); }
    Vec apply(const string& name, const Vec& x) {
	return ops[name](x); }

    map<string, Linear> ops;
};

deque<Linear> d;

void insert_hilbert(named_linear& nl)
{
    auto F= make_unique<Mat>(Mat{{1., 0.5},{0.5,1./3.}});
    // nl.insert("hilbert", [&F](const Vec& x){ return Vec(*F * x); });
    // nl.insert("hilbert", [F](const Vec& x){ return Vec(*F * x); });
    // nl.insert("hilbert", [F= move(F)](const Vec& x){ return Vec(*F * x); });
    // nl.ops["hilbert"]= move([F= move(F)](const Vec& x){ return Vec(*F * x); });
    // nl.ops.emplace("hilbert", [F= move(F)](const Vec& x){ return Vec(*F * x); });
    // d.emplace_back([F{ move(F) }](const Vec& x) { return Vec(*F * x); });
    auto func= [F= move(F)](const Vec& x){ return Vec(*F * x); };
    // nl.ops.emplace("hilbert", move(func));
    //nl.insert("hilbert", move(func));
}

struct dings
{
    dings() = default;
    dings(const dings&) = delete;
    dings(dings&&) = default;
    dings& operator=(const dings&) = delete;
    dings& operator=(dings&&) = default;

    // unique_ptr<int> p;
};

struct evil
{
    evil() = default;
    evil(int) {}
    evil(const evil&) = delete;
    evil(evil&&) = delete;
    evil& operator=(const evil&) = delete;
    evil& operator=(evil&&) = delete;
};

// struct foo { unique_ptr<int> p; };

int main (int argc, char* argv[])
{
    print_compiler();

    Mat A= {{2, 3}, {4, 5}};
    Mat I= {{1, 0}, {0, 1}};

    named_linear nl;
    nl.insert("scale", [&A](const Vec& x){ return Vec(A * x); });
    nl.insert("trans" , [&A](const Vec& x){ return Vec(trans(A) * x); });
    nl.insert("identity" , [&I](const Vec& x){ return Vec(I * x); });

    Vec x= {3, 4};
    // cout << "identity(x) = " << nl.apply("identity", x) << "\n";
    // cout << "trans(x) = " << nl.apply("trans", x) << "\n";
    // cout << "hilbert(x) = " << nl.apply("hilbert", x) << "\n";

    dings bums;
    deque<dings> dd;
    dd.push_back(move(bums));
    dd.push_back(dings());
    dd.emplace_back(dings());

    map<string, dings> dm;
    dm.emplace("hallo", dings());
    dm["huhu"]= dings();

    nl.insert("test1", [&bums](const Vec& x){ return x; });
    // nl.insert("test2", [b= move(bums)](const Vec& x){ return x; });

    auto up= make_unique<int>(3);
    std::vector<function<int(int)>> v;
    v.push_back([&up](int i){return i;});
    // v.push_back([up= move(up)](int i){return i;});

    deque<function<int(int)>> di;
    // di.emplace_back([up= move(up)](int i){return i;});

    deque<evil> de;
    de.emplace_back();
    de.emplace_back(7);

    struct foo { unique_ptr<int> p; };
    std::vector<foo> v2;
    v2.push_back(foo{});

    { // to avoid redeclaration of x
    int x= 4;
    auto y=
        [&r= x, x= x + 1]()
        {
            r+= 2;          // increment r, a reference to outer x
            return x + 2;   // return x + 2 where x is the outer x + 1
        }();  // Updates ::x to 6, and initializes y to 7.
    cout << "x = " << x << ", y = " << y << endl;
    }

    return 0;
}


#ifndef WAVE_PLANE_HPP
#define WAVE_PLANE_HPP

#include <Eigen/Core>
#include <ceres/ceres.h>
#include "wave/odometry/geometry/assign_jacobians.hpp"
#include "wave/odometry/feature_track.hpp"

namespace wave {

template <typename Scalar, int... states>
class PlaneResidual : public ceres::SizedCostFunction<1, 6, states...> {
 public:
    virtual ~PlaneResidual() {}

    PlaneResidual(const Scalar *pt,
                  VecE<const MatX*> &jacsw1,
                  VecE<const MatX*> &jacsw2,
                  const Scalar &w1,
                  const Scalar &w2)
            : pt(pt),
              jacsw1(std::move(jacsw1)),
              jacsw2(std::move(jacsw2)),
              w1(w1),
              w2(w2) { }

    virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const;

    template<typename Derived>
    MatX getDerivativeWpT(const Eigen::MatrixBase<Derived> &normal) const;

    template<typename Derived>
    void setWeight(const Eigen::MatrixBase<Derived> &Weight) {
        this->weight = Weight;
    }

 private:
    // updated by evaluation callback
    const Scalar *pt;
    const VecE<const MatX*> jacsw1, jacsw2;

    const Scalar w1, w2;
    Eigen::Matrix<double, 1, 1> weight = Eigen::Matrix<double, 1, 1>::Identity();
};
}

#include "wave/odometry/geometry/impl/plane_impl.hpp"

#endif  // WAVE_PLANE_HPP


//==================================================================================================
/**
  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_COMMON_SIMD_FUNCTION_IS_LTZ_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_IS_LTZ_HPP_INCLUDED

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/function/is_less.hpp>
#include <boost/simd/meta/as_logical.hpp>
#include <boost/simd/constant/false.hpp>
#include <boost/simd/constant/zero.hpp>

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
   namespace bs = boost::simd;

  BOOST_DISPATCH_OVERLOAD_IF ( is_ltz_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::pack_<bd::signed_<A0>, X>
                          )
  {
    BOOST_FORCEINLINE auto operator()( const A0& a0) const BOOST_NOEXCEPT_DECLTYPE_BODY
    (
      is_less(a0, Zero<A0>())
    )
  };

  BOOST_DISPATCH_OVERLOAD_IF ( is_ltz_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::pack_<bd::unsigned_<A0>, X>
                          )
  {
    BOOST_FORCEINLINE auto operator()(const A0&) const BOOST_NOEXCEPT_DECLTYPE_BODY
    (
      bs::False<bs::as_logical_t<A0>>()
    )
  };
} } }

#endif


//
// $Id$
//
//
// Original author: Austin Keller <atkeller .@. uw.edu>
//
// 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 "CubicHermiteSpline.hpp"
#include "CSpline.h"
#include <boost/make_shared.hpp>

using namespace std;

namespace pwiz
{
namespace analysis
{
    CubicHermiteSpline::CubicHermiteSpline(const std::vector<double>& points, const std::vector<double>& values) :
        cSpline_(new cSpline(points, values))
    {
        CheckForError();
    }

    CubicHermiteSpline::CubicHermiteSpline(const Eigen::VectorXd& points, const Eigen::VectorXd& values)
    {
        vector<double> pointsCopied;
        vector<double> valuesCopied;
        for (int i = 0; i < points.size(); ++i)
        {
            pointsCopied.push_back(points[i]);
            valuesCopied.push_back(values[i]);
        }
        cSpline_ = new cSpline(pointsCopied, valuesCopied);
        CheckForError();
    }

    CubicHermiteSpline::~CubicHermiteSpline()
    {
        if (cSpline_ != nullptr)
            delete cSpline_;
    }

    bool CubicHermiteSpline::IsDifferentiable()
    {
        return false;
    }

    bool CubicHermiteSpline::IsIntegrable()
    {
        return false;
    }

    double CubicHermiteSpline::Differentiate(double x)
    {
        throw runtime_error("[CubicHermiteSpline] attempted to differentiate a non-differentiable implementation of IInterpolation.");
    }

    double CubicHermiteSpline::Integrate(double a, double b)
    {
        throw runtime_error("[CubicHermiteSpline] attempted to integrate a non-integrable implementation of IInterpolation.");
    }

    double CubicHermiteSpline::Interpolate(double x)
    {
        return cSpline_->getY(x);
    }

    void CubicHermiteSpline::CheckForError() const
    {
        if (cSpline_->IsError())
            throw runtime_error("[CubicHermiteSpline] unusable values were provided to the spline function.");
    }
} // namespace analysis
} // namespace pwiz


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

// 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)

#ifndef BOOST_GEOMETRY_MULTI_ALGORITHMS_WITHIN_HPP
#define BOOST_GEOMETRY_MULTI_ALGORITHMS_WITHIN_HPP

#include <boost/range.hpp>

#include <boost/geometry/algorithms/within.hpp>
#include <boost/geometry/multi/core/closure.hpp>
#include <boost/geometry/multi/core/point_order.hpp>
#include <boost/geometry/multi/core/tags.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace within
{

template
<
    typename Geometry,
    typename MultiGeometry,
    typename Strategy,
    typename Policy
>
struct geometry_multi_within_code
{
    static inline int apply(Geometry const& geometry,
            MultiGeometry const& multi,
            Strategy const& strategy)
    {
        for (typename boost::range_iterator<MultiGeometry const>::type it
                    = boost::begin(multi);
            it != boost::end(multi);
            ++it)
        {
            // Geometry coding on multi: 1 (within) if within one of them;
            // 0 (touch) if on border of one of them
            int const code = Policy::apply(geometry, *it, strategy);
            if (code != -1)
            {
                return code;
            }
        }
        return -1;
    }
};

}} // namespace detail::within
#endif // DOXYGEN_NO_DETAIL

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

template <typename Point, typename MultiPolygon>
struct within<Point, MultiPolygon, point_tag, multi_polygon_tag>
{
    template <typename Strategy>
    static inline bool apply(Point const& point,
                MultiPolygon const& multi_polygon, Strategy const& strategy)
    {
        return detail::within::geometry_multi_within_code
            <
                Point,
                MultiPolygon,
                Strategy,
                detail::within::point_in_polygon
                        <
                            Point,
                            typename boost::range_value<MultiPolygon>::type,
                            order_as_direction
                                <
                                    geometry::point_order<MultiPolygon>::value
                                >::value,
                            geometry::closure<MultiPolygon>::value,
                            Strategy
                        >
            >::apply(point, multi_polygon, strategy) == 1;
    }
};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_MULTI_ALGORITHMS_WITHIN_HPP


//---------------------------------------------------------------------------//
// Copyright (c) 2013 Kyle Lutz <kyle.r.lutz@gmail.com>
//
// 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
//
// See http://boostorg.github.com/compute for more information.
//---------------------------------------------------------------------------//

#ifndef BOOST_COMPUTE_ALGORITHM_INNER_PRODUCT_HPP
#define BOOST_COMPUTE_ALGORITHM_INNER_PRODUCT_HPP

#include <boost/compute/system.hpp>
#include <boost/compute/functional.hpp>
#include <boost/compute/command_queue.hpp>
#include <boost/compute/algorithm/accumulate.hpp>
#include <boost/compute/container/vector.hpp>
#include <boost/compute/iterator/transform_iterator.hpp>
#include <boost/compute/iterator/zip_iterator.hpp>
#include <boost/compute/functional/detail/unpack.hpp>

namespace boost {
namespace compute {

/// Returns the inner product of the elements in the range
/// [\p first1, \p last1) with the elements in the range beginning
/// at \p first2.
template<class InputIterator1, class InputIterator2, class T>
inline T inner_product(InputIterator1 first1,
                       InputIterator1 last1,
                       InputIterator2 first2,
                       T init,
                       command_queue &queue = system::default_queue())
{
    typedef typename std::iterator_traits<InputIterator1>::value_type input_type;

    ptrdiff_t n = std::distance(first1, last1);

    return ::boost::compute::accumulate(
        ::boost::compute::make_transform_iterator(
            ::boost::compute::make_zip_iterator(
                boost::make_tuple(first1, first2)
            ),
            detail::unpack(multiplies<input_type>())
        ),
        ::boost::compute::make_transform_iterator(
            ::boost::compute::make_zip_iterator(
                boost::make_tuple(last1, first2 + n)
            ),
            detail::unpack(multiplies<input_type>())
        ),
        init,
        queue
    );
}

/// \overload
template<class InputIterator1,
         class InputIterator2,
         class T,
         class BinaryAccumulateFunction,
         class BinaryTransformFunction>
inline T inner_product(InputIterator1 first1,
                       InputIterator1 last1,
                       InputIterator2 first2,
                       T init,
                       BinaryAccumulateFunction accumulate_function,
                       BinaryTransformFunction transform_function,
                       command_queue &queue = system::default_queue())
{
    typedef typename std::iterator_traits<InputIterator1>::value_type value_type;

    size_t count = detail::iterator_range_size(first1, last1);
    vector<value_type> result(count, queue.get_context());
    transform(first1,
              last1,
              first2,
              result.begin(),
              transform_function,
              queue);

    return ::boost::compute::accumulate(result.begin(),
                                        result.end(),
                                        init,
                                        accumulate_function,
                                        queue);
}

} // end compute namespace
} // end boost namespace

#endif // BOOST_COMPUTE_ALGORITHM_INNER_PRODUCT_HPP


#include "moja/datarepository/aspatialtileinfocollection.h"

#include "moja/datarepository/aspatialtileinfo.h"
#include "moja/datarepository/datarepositoryexceptions.h"
#include "moja/datarepository/iproviderrelationalinterface.h"

#include <moja/dynamic.h>

#include <boost/format.hpp>

#include <cmath>

namespace moja {
namespace datarepository {

AspatialTileInfoCollection::AspatialTileInfoCollection(const IProviderRelationalInterface& provider, int maxTileSize,
                                                       int tileCacheSize) {
   if (maxTileSize < 1) {
      BOOST_THROW_EXCEPTION(LandscapeDefinitionException() << Component("maxTileSize") << Constraint("> 0"));
   }

   if (tileCacheSize < 1) {
      BOOST_THROW_EXCEPTION(LandscapeDefinitionException() << Component("tileCacheSize") << Constraint("> 0"));
   }

   _provider = &provider;
   _maxTileSize = maxTileSize;
   _tileCacheSize = tileCacheSize;
}

int AspatialTileInfoCollection::tileCount() const {
   if (_tileCount == -1) {
      _tileCount = int(std::ceil(getTotalLUCount() / double(_maxTileSize)));
   }

   return _tileCount;
}

std::vector<AspatialTileInfo> AspatialTileInfoCollection::getTiles() const {
   std::vector<AspatialTileInfo> tiles;
   for (int i = 1; i <= tileCount(); i++) {
      tiles.push_back(AspatialTileInfo(*this, i));
   }

   return tiles;
}

int AspatialTileInfoCollection::luCount(int tileId) const {
   auto sql = (boost::format("SELECT COUNT(1) AS lu_count FROM inventory WHERE (id + (%1% - %2%)) %% %1% = 0") %
               tileCount() % tileId)
                  .str();

   auto result = _provider->GetDataSet(sql).extract<std::vector<DynamicObject>>();
   return result[0]["lu_count"];
}

std::shared_ptr<std::map<Int64, DynamicVar>> AspatialTileInfoCollection::fetchLUBlock(Int64 id) const {
   auto block = std::make_shared<std::map<Int64, DynamicVar>>();
   int tileId = id % tileCount();
   auto sql =
       (boost::format("SELECT id, area FROM inventory WHERE id >= %1% AND (id + (%2% - %3%)) %% %2% = 0 LIMIT %4%") %
        id % tileCount() % tileId % _tileCacheSize)
           .str();

   auto result = _provider->GetDataSet(sql).extract<std::vector<DynamicObject>>();
   for (const auto row : result) {
      (*block)[row["id"]] = row;
   }

   return block;
}

Int64 AspatialTileInfoCollection::getTotalLUCount() const {
   std::string sql = "SELECT COUNT(1) AS lu_count FROM inventory";
   auto result = _provider->GetDataSet(sql).extract<std::vector<DynamicObject>>();
   return result[0]["lu_count"];
}

}  // namespace datarepository
}  // namespace moja


#include "humanoids.hpp"
#include "sco/modeling_utils.hpp"
#include "sco/expr_ops.hpp"
#include "sco/expr_vec_ops.hpp"
#include "sco/expr_op_overloads.hpp"
#include <boost/foreach.hpp>
#include "utils/eigen_conversions.hpp"
#include "trajopt/problem_description.hpp"
#include "humanoids/hull2d.hpp"
using namespace util;
using namespace Eigen;
using namespace std;
using namespace Json;
namespace trajopt {

/**
import openravepy
env = openravepy.Environment()
env.Load("atlas.xml")
robot = env.GetRobots()[0]
rfoot = robot.GetLink("r_foot")
aabb = rfoot.ComputeLocalAABB(rfoot)
aabb.pos()+aabb.extents()
aabb.pos()-aabb.extents()
 */
const double footpolydata[8] = {
    0.17939, 0.062707,
    0.17939, -0.061646,
    -0.08246, -0.061646,
    -0.08246, 0.061646};
MatrixX2d local_aabb_poly = Map<const MatrixX2d>(footpolydata,4,2);

/**
px,py,pz=aabb.pos()
ex,ey,ez=aabb.extents()
np.array([(px+xsgn*ex,py+ysgn*ey,pz) for xsgn in (-1,1) for ysgn in (-1,1)])
 *
 */
const double footbottomdata[12] = {
    0.17939  ,  0.062707 , -0.0353015,
    0.17939  , -0.061646 , -0.0353015,
    -0.08246  , -0.061646 , -0.0353015,
    -0.08246  ,  0.062707 , -0.0353015
};
MatrixX3d footbottompoints = Map<const MatrixX3d>(footbottomdata, 4, 3);

void PolygonToEquations(const MatrixX2d& pts, MatrixX2d& ab, VectorXd& c) {
  // ax + by + c <= 0
  // assume polygon is convex
  ab.resize(pts.rows(),2);
  c.resize(pts.rows());
  Vector2d p0 = pts.row(0);

  for (int i=0; i < pts.rows(); ++i) {
    int i1 = (i+1) % pts.rows();
    double x0 = pts(i,0),
        y0 = pts(i,1),
        x1 = pts(i1,0),
        y1 = pts(i1,1);
    ab(i,0) = -(y1 - y0);
    ab(i,1) = x1 - x0;
    ab.row(i).normalize();
    c(i) = -ab.row(i).dot(pts.row(i));
  }

  Vector2d centroid = pts.colwise().mean();
  if (ab.row(0) * centroid + c(0) >= 0) {
    ab *= -1;
    c *= -1;
  }

}

ZMPConstraint::ZMPConstraint(RobotAndDOFPtr rad, const MatrixX2d& hullpts, const VarVector& vars) :
          m_rad(rad), m_vars(vars), m_pts(hullpts) {

  // find the equations representing the polygon
  PolygonToEquations(m_pts, m_ab, m_c);
}

DblVec ZMPConstraint::value(const DblVec& x) {
  m_rad->SetDOFValues(getDblVec(x,m_vars));
  OR::Vector moment(0,0,0);
  // calculate center of mass
  float totalmass = 0;
  BOOST_FOREACH(const KinBody::LinkPtr& link, m_rad->GetRobot()->GetLinks()) {
    if (!link->GetGeometries().empty()) {
      moment += link->GetGlobalCOM() * link->GetMass();
      totalmass += link->GetMass();
    }
  }
  moment /= totalmass;
  Vector2d xy;
  xy(0) = moment.x;
  xy(1) = moment.y;
  return toDblVec(m_ab * xy + m_c);
}

ConvexConstraintsPtr ZMPConstraint::convex(const DblVec& x, Model* model) {
  DblVec curvals = getDblVec(x, m_vars);
  m_rad->SetDOFValues(curvals);
  DblMatrix jacmoment = DblMatrix::Zero(3, curvals.size());
  OR::Vector moment(0,0,0);
  float totalmass = 0;
  BOOST_FOREACH(const KinBody::LinkPtr& link, m_rad->GetRobot()->GetLinks()) {
    if (!link->GetGeometries().empty()) {
      OR::Vector cm = link->GetGlobalCOM();
      moment += cm * link->GetMass();
      jacmoment += m_rad->PositionJacobian(link->GetIndex(), cm) * link->GetMass();
      totalmass += link->GetMass();
    }
  }
  moment /= totalmass;
  jacmoment /= totalmass;

  AffExpr x_expr = AffExpr(moment.x) + varDot(jacmoment.row(0), m_vars) - jacmoment.row(0).dot(toVectorXd(curvals));
  AffExpr y_expr = AffExpr(moment.y) + varDot(jacmoment.row(1), m_vars) - jacmoment.row(1).dot(toVectorXd(curvals));

  ConvexConstraintsPtr out(new ConvexConstraints(model));
  for (int i=0; i < m_ab.rows(); ++i) {
    out->addIneqCnt(m_ab(i,0) * x_expr + m_ab(i,1) * y_expr + m_c(i));
  }
  return out;
}

void ZMPConstraint::Plot(const DblVec& x, OR::EnvironmentBase& env, std::vector<OR::GraphHandlePtr>& handles) {
  Matrix<float, Dynamic, 3, RowMajor> xyz(m_ab.rows()*2, 3);
  xyz.col(2).setZero();
  int npts = m_pts.rows();
  for (int i=0; i < npts; ++i) {
    xyz(2*i,0) = m_pts(i,0);
    xyz(2*i,1) = m_pts(i,1);
    xyz(2*i+1,0) = m_pts((i+1)%npts,0);
    xyz(2*i+1,1) = m_pts((i+1)%npts,1);
  }
  handles.push_back(env.drawlinelist(xyz.data(), xyz.rows(), sizeof(float)*3, 4, OR::RaveVector<float>(1,0,0,1)));

  m_rad->SetDOFValues(getDblVec(x,m_vars));
  OR::Vector moment(0,0,0);
  float totalmass = 0;
  BOOST_FOREACH(const KinBody::LinkPtr& link, m_rad->GetRobot()->GetLinks()) {
    if (!link->GetGeometries().empty()) {
      moment += link->GetGlobalCOM() * link->GetMass();
      totalmass += link->GetMass();
    }
  }
  moment /= totalmass;
  OR::Vector moment_ground = moment; moment_ground.z = 0;
  handles.push_back(env.drawarrow(moment, moment_ground, .005, OR::RaveVector<float>(1,0,0,1)));
}

struct StaticTorqueCostCalc : public VectorOfVector {
  RobotAndDOFPtr m_rad;
  StaticTorqueCostCalc(const RobotAndDOFPtr rad) : m_rad(rad) {}
  VectorXd operator()(const VectorXd& x) const {
    m_rad->SetDOFValues(toDblVec(x));
    VectorXd out = VectorXd::Zero(m_rad->GetDOF());
    BOOST_FOREACH(const KinBody::LinkPtr& link, m_rad->GetRobot()->GetLinks()) {
      if (!link->GetGeometries().empty()) {
        OR::Vector cm = link->GetGlobalCOM();
        DblMatrix jac = m_rad->PositionJacobian(link->GetIndex(), cm) * link->GetMass();
        out += jac.row(2).transpose();
      }
    }
    return out;
  }
};

StaticTorqueCost::StaticTorqueCost(RobotAndDOFPtr rad, const VarVector& vars, double coeff) :
  CostFromErrFunc(VectorOfVectorPtr(new StaticTorqueCostCalc(rad)), vars, VectorXd::Ones(vars.size())*coeff,
    SQUARED,  "static_torque") {
}

struct PECalc : public VectorOfVector {
  RobotAndDOFPtr m_rad;
  PECalc(const RobotAndDOFPtr rad) : m_rad(rad) {}
  VectorXd operator()(const VectorXd& x) const {
    m_rad->SetDOFValues(toDblVec(x));
    VectorXd out = VectorXd::Zero(1);
    BOOST_FOREACH(const KinBody::LinkPtr& link, m_rad->GetRobot()->GetLinks()) {
      if (!link->GetGeometries().empty()) {
        OR::Vector cm = link->GetGlobalCOM();
        out(0) += cm.z;
      }
    }
    return out;
  }
};

PECost::PECost(RobotAndDOFPtr rad, const VarVector& vars, double coeff) :
  CostFromErrFunc(VectorOfVectorPtr(new PECalc(rad)), vars, VectorXd::Ones(1)*coeff,
    SQUARED,  "PE") {
}

inline OpenRAVE::Vector toRaveVector(const Vector3d& x) {
  return OpenRAVE::Vector(x[0], x[1], x[2]);
}

struct FootHeightCalc : public VectorOfVector {
  RobotAndDOFPtr m_rad;
  KinBody::LinkPtr m_link;
  double m_height;
  FootHeightCalc(const RobotAndDOFPtr rad, KinBody::LinkPtr link, double height) : m_rad(rad), m_link(link), m_height(height) {}
  VectorXd operator()(const VectorXd& x) const {
    m_rad->SetDOFValues(toDblVec(x));
    OpenRAVE::Transform T = m_link->GetTransform();
    Eigen::Vector4d out(
        m_height - (T * toRaveVector(footbottompoints.row(0))).z,
        m_height - (T * toRaveVector(footbottompoints.row(1))).z,
        m_height - (T * toRaveVector(footbottompoints.row(2))).z,
        m_height - (T * toRaveVector(footbottompoints.row(3))).z);
    cout << out.transpose() << endl;
    cout << footbottompoints.row(0) << endl;
    return out;
  }
};

FootHeightConstraint::FootHeightConstraint(RobotAndDOFPtr rad, KinBody::LinkPtr link, double height, const VarVector& vars) :
  ConstraintFromFunc(VectorOfVectorPtr(new FootHeightCalc(rad, link, height)), vars, INEQ,  "FootHeight") {
}

extern ProblemConstructionInfo* gPCI;

struct PECostInfo : public CostInfo {
  double coeff;
  int timestep;
  void fromJson(const Value& v) {
    FAIL_IF_FALSE(v.isMember("params"));
    const Value& params = v["params"];
    childFromJson(params, timestep, "timestep");
    childFromJson(params, coeff, "coeff");
    int n_steps = gPCI->basic_info.n_steps;
    FAIL_IF_FALSE((timestep >= 0) && (timestep < n_steps));
  }
  void hatch(TrajOptProb& prob) {
    prob.addCost(CostPtr(new PECost(prob.GetRAD(), prob.GetVarRow(timestep), coeff)));
    prob.getCosts().back()->setName(name);
  }
  static CostInfoPtr create() {
    return CostInfoPtr(new PECostInfo());
  }
};
struct StaticTorqueCostCostInfo : public CostInfo {
  double coeff;
  int timestep;
  void fromJson(const Value& v) {
    FAIL_IF_FALSE(v.isMember("params"));
    const Value& params = v["params"];
    childFromJson(params, timestep, "timestep");
    childFromJson(params, coeff, "coeff");
    int n_steps = gPCI->basic_info.n_steps;
    FAIL_IF_FALSE((timestep >= 0) && (timestep < n_steps));
  }
  void hatch(TrajOptProb& prob) {
    prob.addCost(CostPtr(new StaticTorqueCost(prob.GetRAD(), prob.GetVarRow(timestep), coeff)));
    prob.getCosts().back()->setName(name);
  }
  static CostInfoPtr create() {
    return CostInfoPtr(new StaticTorqueCostCostInfo());
  }
};

template <typename MatrixT>
MatrixT concat0(const MatrixT& x, const MatrixT& y) {
  MatrixT out(x.rows() + y.rows(), x.cols());
  out.topRows(x.rows()) = x;
  out.bottomRows(y.rows()) = y;
  return out;
}

MatrixX2d GetFootPoly(const KinBody::Link& link) {
  OpenRAVE::Vector v = link.GetTransform().trans;
  v.z = 0;
  return local_aabb_poly.rowwise() + Vector2d(v.x, v.y).transpose();
}
MatrixX2d GetFeetPoly(const vector<KinBody::LinkPtr>& links) {
  FAIL_IF_FALSE(links.size() > 0);
  MatrixX2d allpoly = GetFootPoly(*links[0]);
  for (int i=1; i < links.size(); ++i) {
    allpoly = concat0(allpoly, GetFootPoly(*links[i]));
  }
  return hull2d(allpoly);
}

struct ZMPConstraintCntInfo : public CntInfo {
  int timestep;
  vector<string> planted_link_names;
  void fromJson(const Value& v) {
    FAIL_IF_FALSE(v.isMember("params"));
    const Value& params = v["params"];
    childFromJson(params, timestep, "timestep");
    int n_steps = gPCI->basic_info.n_steps;
    FAIL_IF_FALSE((timestep >= 0) && (timestep < n_steps));
    childFromJson(params, planted_link_names, "planted_links");
  }
  void hatch(TrajOptProb& prob) {
    vector<KinBody::LinkPtr> planted_links;
    BOOST_FOREACH(const string& linkname, planted_link_names) {
      KinBody::LinkPtr link = prob.GetRAD()->GetRobot()->GetLink(linkname);
      if (!link) {
        PRINT_AND_THROW(boost::format("invalid link name: %s")%linkname);
      }
      planted_links.push_back(link);
    }
    prob.addConstr(ConstraintPtr(new ZMPConstraint(prob.GetRAD(), GetFeetPoly(planted_links), prob.GetVarRow(timestep))));
    prob.getIneqConstraints().back()->setName(name);
  }
  static CntInfoPtr create() {
    return CntInfoPtr(new ZMPConstraintCntInfo());
  }
};

struct FootHeightCntInfo : public CntInfo {
  int timestep;
  double height;
  string link_name;
  void fromJson(const Value& v) {
    FAIL_IF_FALSE(v.isMember("params"));
    const Value& params = v["params"];
    childFromJson(params, timestep, "timestep");
    childFromJson(params, height, "height");
    int n_steps = gPCI->basic_info.n_steps;
    FAIL_IF_FALSE((timestep >= 0) && (timestep < n_steps));
    childFromJson(params, link_name, "link");
  }
  void hatch(TrajOptProb& prob) {
    KinBody::LinkPtr link = prob.GetRAD()->GetRobot()->GetLink(link_name);
    if (!link) {
      PRINT_AND_THROW(boost::format("invalid link name: %s")%link_name);
    }
    prob.addConstr(ConstraintPtr(new FootHeightConstraint(prob.GetRAD(), link, height, prob.GetVarRow(timestep))));
    prob.getIneqConstraints().back()->setName(name);
  }
  static CntInfoPtr create() {
    return CntInfoPtr(new FootHeightCntInfo());
  }
};

TRAJOPT_API void RegisterHumanoidCostsAndCnts() {
  CostInfo::RegisterMaker("potential_energy", &PECostInfo::create);
  CostInfo::RegisterMaker("static_torque", &StaticTorqueCostCostInfo::create);
  CntInfo::RegisterMaker("zmp", &ZMPConstraintCntInfo::create);
  CntInfo::RegisterMaker("foot_height", &FootHeightCntInfo::create);
}

}


#ifndef BOOST_GEOMETRY_PROJECTIONS_CHAMB_HPP
#define BOOST_GEOMETRY_PROJECTIONS_CHAMB_HPP

// Boost.Geometry - extensions-gis-projections (based on PROJ4)
// This file is automatically generated. DO NOT EDIT.

// Copyright (c) 2008-2015 Barend Gehrels, Amsterdam, the Netherlands.

// 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)

// This file is converted from PROJ4, http://trac.osgeo.org/proj
// PROJ4 is originally written by Gerald Evenden (then of the USGS)
// PROJ4 is maintained by Frank Warmerdam
// PROJ4 is converted to Boost.Geometry by Barend Gehrels

// Last updated version of proj: 4.9.1

// Original copyright notice:

// Permission is hereby granted, free of charge, to any person obtaining a
// copy of this software and associated documentation files (the "Software"),
// to deal in the Software without restriction, including without limitation
// the rights to use, copy, modify, merge, publish, distribute, sublicense,
// and/or sell copies of the Software, and to permit persons to whom the
// Software is furnished to do so, subject to the following conditions:

// The above copyright notice and this permission notice shall be included
// in all copies or substantial portions of the Software.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
// OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.

#include <boost/geometry/util/math.hpp>
#include <cstdio>

#include <boost/geometry/extensions/gis/projections/impl/base_static.hpp>
#include <boost/geometry/extensions/gis/projections/impl/base_dynamic.hpp>
#include <boost/geometry/extensions/gis/projections/impl/projects.hpp>
#include <boost/geometry/extensions/gis/projections/impl/factory_entry.hpp>
#include <boost/geometry/extensions/gis/projections/impl/aasincos.hpp>

namespace boost { namespace geometry { namespace projections
{
    #ifndef DOXYGEN_NO_DETAIL
    namespace detail { namespace chamb
    {

            static const double THIRD = 0.333333333333333333;
            static const double TOL = 1e-9;

            // specific for 'chamb'
            struct VECT { double r, Az; };
            struct XY { double x, y; };

            struct par_chamb
            {
                struct { /* control point data */
                double phi, lam;
                double cosphi, sinphi;
                VECT v;
                XY    p;
                double Az;
                } c[3];
                XY p;
                double beta_0, beta_1, beta_2;
            };

                static VECT /* distance and azimuth from point 1 to point 2 */
            vect(double dphi, double c1, double s1, double c2, double s2, double dlam) {
                VECT v;
                double cdl, dp, dl;

                cdl = cos(dlam);
                if (fabs(dphi) > 1. || fabs(dlam) > 1.)
                    v.r = aacos(s1 * s2 + c1 * c2 * cdl);
                else { /* more accurate for smaller distances */
                    dp = sin(.5 * dphi);
                    dl = sin(.5 * dlam);
                    v.r = 2. * aasin(sqrt(dp * dp + c1 * c2 * dl * dl));
                }
                if (fabs(v.r) > TOL)
                    v.Az = atan2(c2 * sin(dlam), c1 * s2 - s1 * c2 * cdl);
                else
                    v.r = v.Az = 0.;
                return v;
            }
                static double /* law of cosines */
            lc(double b,double c,double a) {
                return aacos(.5 * (b * b + c * c - a * a) / (b * c));
            }

            // template class, using CRTP to implement forward/inverse
            template <typename Geographic, typename Cartesian, typename Parameters>
            struct base_chamb_spheroid : public base_t_f<base_chamb_spheroid<Geographic, Cartesian, Parameters>,
                     Geographic, Cartesian, Parameters>
            {

                 typedef double geographic_type;
                 typedef double cartesian_type;

                par_chamb m_proj_parm;

                inline base_chamb_spheroid(const Parameters& par)
                    : base_t_f<base_chamb_spheroid<Geographic, Cartesian, Parameters>,
                     Geographic, Cartesian, Parameters>(*this, par) {}

                // FORWARD(s_forward)  spheroid
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(geographic_type& lp_lon, geographic_type& lp_lat, cartesian_type& xy_x, cartesian_type& xy_y) const
                {
                    double sinphi, cosphi, a;
                    VECT v[3];
                    int i, j;

                    sinphi = sin(lp_lat);
                    cosphi = cos(lp_lat);
                    for (i = 0; i < 3; ++i) { /* dist/azimiths from control */
                        v[i] = vect(lp_lat - this->m_proj_parm.c[i].phi, this->m_proj_parm.c[i].cosphi, this->m_proj_parm.c[i].sinphi,
                            cosphi, sinphi, lp_lon - this->m_proj_parm.c[i].lam);
                        if ( ! v[i].r)
                            break;
                        v[i].Az = adjlon(v[i].Az - this->m_proj_parm.c[i].v.Az);
                    }
                    if (i < 3) /* current point at control point */
                        { xy_x = this->m_proj_parm.c[i].p.x; xy_y = this->m_proj_parm.c[i].p.y; }
                    else { /* point mean of intersepts */
                        { xy_x = this->m_proj_parm.p.x; xy_y = this->m_proj_parm.p.y; }
                        for (i = 0; i < 3; ++i) {
                            j = i == 2 ? 0 : i + 1;
                            a = lc(this->m_proj_parm.c[i].v.r, v[i].r, v[j].r);
                            if (v[i].Az < 0.)
                                a = -a;
                            if (! i) { /* coord comp unique to each arc */
                                xy_x += v[i].r * cos(a);
                                xy_y -= v[i].r * sin(a);
                            } else if (i == 1) {
                                a = this->m_proj_parm.beta_1 - a;
                                xy_x -= v[i].r * cos(a);
                                xy_y -= v[i].r * sin(a);
                            } else {
                                a = this->m_proj_parm.beta_2 - a;
                                xy_x += v[i].r * cos(a);
                                xy_y += v[i].r * sin(a);
                            }
                        }
                        xy_x *= THIRD; /* mean of arc intercepts */
                        xy_y *= THIRD;
                    }
                }

                static inline std::string get_name()
                {
                    return "chamb_spheroid";
                }

            };

            // Chamberlin Trimetric
            template <typename Parameters>
            void setup_chamb(Parameters& par, par_chamb& proj_parm)
            {
                int i, j;
                char line[10];

                for (i = 0; i < 3; ++i) { /* get control point locations */
                    (void)sprintf(line, "rlat_%d", i+1);
                    proj_parm.c[i].phi = pj_param(par.params, line).f;
                    (void)sprintf(line, "rlon_%d", i+1);
                    proj_parm.c[i].lam = pj_param(par.params, line).f;
                    proj_parm.c[i].lam = adjlon(proj_parm.c[i].lam - par.lam0);
                    proj_parm.c[i].cosphi = cos(proj_parm.c[i].phi);
                    proj_parm.c[i].sinphi = sin(proj_parm.c[i].phi);
                }
                for (i = 0; i < 3; ++i) { /* inter ctl pt. distances and azimuths */
                    j = i == 2 ? 0 : i + 1;
                    proj_parm.c[i].v = vect(proj_parm.c[j].phi - proj_parm.c[i].phi, proj_parm.c[i].cosphi, proj_parm.c[i].sinphi,
                        proj_parm.c[j].cosphi, proj_parm.c[j].sinphi, proj_parm.c[j].lam - proj_parm.c[i].lam);
                    if (! proj_parm.c[i].v.r) throw proj_exception(-25);
                    /* co-linearity problem ignored for now */
                }
                proj_parm.beta_0 = lc(proj_parm.c[0].v.r, proj_parm.c[2].v.r, proj_parm.c[1].v.r);
                proj_parm.beta_1 = lc(proj_parm.c[0].v.r, proj_parm.c[1].v.r, proj_parm.c[2].v.r);
                proj_parm.beta_2 = geometry::math::pi<double>() - proj_parm.beta_0;
                proj_parm.p.y = 2. * (proj_parm.c[0].p.y = proj_parm.c[1].p.y = proj_parm.c[2].v.r * sin(proj_parm.beta_0));
                proj_parm.c[2].p.y = 0.;
                proj_parm.c[0].p.x = - (proj_parm.c[1].p.x = 0.5 * proj_parm.c[0].v.r);
                proj_parm.p.x = proj_parm.c[2].p.x = proj_parm.c[0].p.x + proj_parm.c[2].v.r * cos(proj_parm.beta_0);
                par.es = 0.;
            }

        }} // namespace detail::chamb
    #endif // doxygen

    /*!
        \brief Chamberlin Trimetric projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Miscellaneous
         - Spheroid
         - no inverse
        \par Projection parameters
         - lat_1: Latitude of control point 1 (degrees)
         - lon_1: Longitude of control point 1 (degrees)
         - lat_2: Latitude of control point 2 (degrees)
         - lon_2: Longitude of control point 2 (degrees)
         - lat_3: Latitude of control point 3 (degrees)
         - lon_3: Longitude of control point 3 (degrees)
        \par Example
        \image html ex_chamb.gif
    */
    template <typename Geographic, typename Cartesian, typename Parameters = parameters>
    struct chamb_spheroid : public detail::chamb::base_chamb_spheroid<Geographic, Cartesian, Parameters>
    {
        inline chamb_spheroid(const Parameters& par) : detail::chamb::base_chamb_spheroid<Geographic, Cartesian, Parameters>(par)
        {
            detail::chamb::setup_chamb(this->m_par, this->m_proj_parm);
        }
    };

    #ifndef DOXYGEN_NO_DETAIL
    namespace detail
    {

        // Factory entry(s)
        template <typename Geographic, typename Cartesian, typename Parameters>
        class chamb_entry : public detail::factory_entry<Geographic, Cartesian, Parameters>
        {
            public :
                virtual projection<Geographic, Cartesian>* create_new(const Parameters& par) const
                {
                    return new base_v_f<chamb_spheroid<Geographic, Cartesian, Parameters>, Geographic, Cartesian, Parameters>(par);
                }
        };

        template <typename Geographic, typename Cartesian, typename Parameters>
        inline void chamb_init(detail::base_factory<Geographic, Cartesian, Parameters>& factory)
        {
            factory.add_to_factory("chamb", new chamb_entry<Geographic, Cartesian, Parameters>);
        }

    } // namespace detail
    #endif // doxygen

}}} // namespace boost::geometry::projections

#endif // BOOST_GEOMETRY_PROJECTIONS_CHAMB_HPP


// Copyright (C) Point One Navigation - All Rights Reserved

#include <iostream>

#include <boost/asio.hpp>

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

#include <point_one/polaris_asio_client.h>

#include "septentrio_service.h"

// Polaris options:
DEFINE_string(polaris_api_key, "",
              "The service API key. Contact account administrator or "
              "sales@pointonenav.com if unknown.");

DEFINE_string(unique_id, "this_is_an_id",
              "Unique ID for debugging purposes.");

// Septentrio/output options:
DEFINE_string(device, "/dev/ttyACM0",
              "The serial device on which the Septentrio is connected.");

DEFINE_string(
    udp_host, "",
    "If set, forward RTCM corrections over UDP to the specified hostname/IP in "
    "addition to the Septentrio receiver (over serial).");

DEFINE_uint32(
    udp_port, 0,
    "The UDP port to which RTCM corrections will be forwarded.");

#ifndef RAD2DEG
#define RAD2DEG (57.295779513082320876798154814105)  //!< 180.0/PI
#endif

// Simple callback to print position when received and update Polaris Client.
void PvtCallback(const PVTGeodetic_2_2_t &pvt,
                 point_one::polaris::PolarisAsioClient *polaris_client) {
  LOG_EVERY_N(INFO, 10) << "Week: " << pvt.WNc << " Tow: " << pvt.TOW / 1000.0
                        << " Solution Type: " << (int)pvt.Mode
                        << " Hacc: " << pvt.HAccuracy
                        << " Vacc: " << pvt.VAccuracy
                        << " Lat: " << pvt.Lat * RAD2DEG
                        << " Lon: " << pvt.Lon * RAD2DEG << " Alt: " << pvt.Alt;
  polaris_client->SetPositionLLA(pvt.Lat * RAD2DEG, pvt.Lon * RAD2DEG, pvt.Alt);
}

// Allows for prebuilt versions of gflags/google that don't have gflags/google
// namespace.
namespace gflags {}
namespace google {}
using namespace gflags;
using namespace google;

int main(int argc, char *argv[], char *envp[]) {
  ParseCommandLineFlags(&argc, &argv, true);
  InitGoogleLogging(argv[0]);

  boost::asio::io_service io_loop;
  boost::asio::io_service::work work(io_loop);

  bool udp_enabled = false;
  boost::asio::ip::udp::socket udp_socket(io_loop);
  boost::asio::ip::udp::endpoint udp_endpoint;

  // Validate arguments.
  if (FLAGS_polaris_api_key == "") {
    LOG(ERROR) << "You must supply a Polaris API key to connect to the server.";
    return 1;
  }

  if (!FLAGS_udp_host.empty()) {
    if (FLAGS_udp_port < 1 || FLAGS_udp_port > 65535) {
      LOG(ERROR)
          << "You must specify a valid UDP port when enabling UDP output.";
      return 1;
    }
    else {
      udp_enabled = true;
      udp_endpoint = boost::asio::ip::udp::endpoint(
          boost::asio::ip::address_v4::from_string(FLAGS_udp_host),
          static_cast<unsigned short>(FLAGS_udp_port));
    }
  }

  // Connect to the Septentrio receiver.
  point_one::gpsreceiver::SeptentrioService septentrio(FLAGS_device);
  LOG(INFO) << "Connecting to receiver...";
  if (!septentrio.Connect(io_loop)) {
    LOG(FATAL) << "Could not connect to the receiver at path: " << FLAGS_device;
  }

  // Create the Polaris client.
  point_one::polaris::PolarisAsioClient polaris_client(
      io_loop, FLAGS_polaris_api_key, FLAGS_unique_id);

  // This callback will forward RTCM correction bytes received from Polaris to
  // the Septentrio, as well as to an output UDP port if enabled.
  polaris_client.SetPolarisBytesReceived(
      [&](const uint8_t *data, size_t length) {
        septentrio.SendRtcm(data, length);
        if (udp_enabled) {
          udp_socket.send_to(boost::asio::buffer(data, length), udp_endpoint);
        }
      });

  // This callback will send position updates from the Septentrio to Polaris.
  // Polaris uses the positions to associate the connection with a corrections
  // stream.
  //
  // Position updates should be sent periodically to ensure the incoming
  // corrections are from the most appropriate stream.
  //
  // Note that positions are supplied by the Septentrio over serial. The UDP
  // interface does not support receiving positions.
  septentrio.SetPvtCallback(
      std::bind(PvtCallback, std::placeholders::_1, &polaris_client));

  // Connect to Polaris.
  polaris_client.Connect();

  // Run indefinitely.
  LOG(INFO) << "Starting services...";
  io_loop.run();

  return 0;
}


/* -*- c++ -*- */
/*
 * Copyright 2014 Free Software Foundation, Inc.
 *
 * This file is part of GNU Radio
 *
 * GNU Radio 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, or (at your option)
 * any later version.
 *
 * GNU Radio 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 GNU Radio; see the file COPYING.  If not, write to
 * the Free Software Foundation, Inc., 51 Franklin Street,
 * Boston, MA 02110-1301, USA.
 */

#ifdef HAVE_CONFIG_H
#include "config.h"
#endif

#include "repetition_decoder_impl.h"
#include <math.h>
#include <boost/assign/list_of.hpp>
#include <volk/volk.h>
#include <sstream>
#include <stdio.h>
#include <vector>

namespace gr {
  namespace fec {
    namespace code {

      generic_decoder::sptr
      repetition_decoder::make(int frame_size, int rep,
                               float ap_prob)
      {
        return generic_decoder::sptr
          (new repetition_decoder_impl(frame_size, rep,
                                       ap_prob));
      }

      repetition_decoder_impl::repetition_decoder_impl(int frame_size, int rep,
                                                       float ap_prob)
        : generic_decoder("repetition_decoder")
      {
        // Set max frame size here; all buffers and settings will be
        // based on this value.
        d_max_frame_size = frame_size;
        set_frame_size(frame_size);

        if(rep < 0)
          throw std::runtime_error("repetition_encoder: repetition rate must be >= 0");
        if((ap_prob < 0) || (ap_prob > 1.0))
          throw std::runtime_error("repetition_encoder: a priori probability rate must be in [0, 1]");

        d_rep = rep;
        d_ap_prob = ap_prob;
        d_trials.resize(d_rep);
      }

      repetition_decoder_impl::~repetition_decoder_impl()
      {
      }

      int
      repetition_decoder_impl::get_output_size()
      {
        //unpacked bits
        return d_frame_size;
      }

      int
      repetition_decoder_impl::get_input_size()
      {
        return d_frame_size*d_rep;
      }

      int
      repetition_decoder_impl::get_input_item_size()
      {
        return sizeof(float);
      }

      const char*
      repetition_decoder_impl::get_input_conversion()
      {
        return "none";
      }

      float
      repetition_decoder_impl::get_shift()
      {
        return 0;
      }

      bool
      repetition_decoder_impl::set_frame_size(unsigned int frame_size)
      {
        bool ret = true;
        if(frame_size > d_max_frame_size) {
          GR_LOG_INFO(d_logger, boost::format("tried to set frame to %1%; max possible is %2%") \
                      % frame_size % d_max_frame_size);
          frame_size = d_max_frame_size;
          ret = false;
        }

        d_frame_size = frame_size;

        return ret;
      }

      double
      repetition_decoder_impl::rate()
      {
        return 1.0/static_cast<double>(d_rep);
      }

      void
      repetition_decoder_impl::generic_work(void *inbuffer, void *outbuffer)
      {
        const float *in = (const float*)inbuffer;
        unsigned char *out = (unsigned char *) outbuffer;

        for(unsigned int i = 0; i < d_frame_size; i++) {
          for(unsigned int r = 0; r < d_rep; r++) {
            d_trials[r] = (in[d_rep*i + r] > 0) ? 1.0f : 0.0f;
          }
          float res = std::count(d_trials.begin(), d_trials.end(), 1.0f);
          if((res / static_cast<float>(d_rep)) > d_ap_prob)
            out[i] = 1;
          else
            out[i] = 0;
        }
      }

    } /* namespace code */
  } /* namespace fec */
} /* namespace gr */