/* Copyright (c) 2014, Julian Straub <jstraub@csail.mit.edu>
 * Licensed under the MIT license. See the license file LICENSE.
 */

#include <iostream>
#include <fstream>
#include <string>

// Utilities and system includes
//#include <helper_functions.h>
#include <boost/program_options.hpp>

#include <opencv2/highgui/highgui.hpp>
#include <opencv2/contrib/contrib.hpp>

#include <cudaPcl/depthGuidedFilter.hpp>
#include <cudaPcl/normalExtractSimpleGpu.hpp>

#include <Eigen/Dense>
#include <pcl/io/ply_io.h>

namespace po = boost::program_options;
using namespace Eigen;
using std::cout;
using std::endl;

int main (int argc, char** argv)
{

  // Declare the supported options.
  po::options_description desc("Allowed options");
  desc.add_options()
    ("help,h", "produce help message")
    ("input,i", po::value<string>(), "path to input depth image (16bit .png)")
    ("output,o", po::value<string>(), "path to output surface normals (csv)")
    ("f_d,f", po::value<double>(), "focal length of depth camera")
    ("eps,e", po::value<double>(), "sqrt of the epsilon parameter of the guided filter")
    ("B,b", po::value<int>(), "guided filter windows size (size will be (2B+1)x(2B+1))")
    ("compress,c", "compress the computed normals")
    ("display,d", "display the computed normals")
//    ("out,o", po::value<std::string>(), "output path where surfae normal images are saved to")
    ;

  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, desc), vm);
  po::notify(vm);

  if (vm.count("help")) {
    cout << desc << "\n";
    return 1;
  }

  double f_d = 520.;
  double eps = 0.2*0.2;
  int32_t B = 10;
  bool compress = false;
  bool display = false;
  string inputPath = "";
  string outputPath = "";
  if(vm.count("input")) inputPath = vm["input"].as<string>();
  if(vm.count("output")) outputPath = vm["output"].as<string>();
  if(vm.count("f_d")) f_d = vm["f_d"].as<double>();
  if(vm.count("eps")) eps = vm["eps"].as<double>();
  if(vm.count("B")) B = vm["B"].as<int>();
  if(vm.count("compress")) compress = true;
  if(vm.count("display")) display = true;
//  std::string outPath = ""
//  if(vm.count("out")) outPath = vm["out"].as<std::string>();

  if(inputPath.compare("") == 0)
  {
    cout<<"provide an input to a depth image file"<<endl;
    exit(1);
  }

  findCudaDevice(argc,(const char**)argv);
  cv::Mat depth = cv::imread(inputPath,CV_LOAD_IMAGE_ANYDEPTH);
  std::cout << "reading depth from " << inputPath << std::endl;
  inputPath.replace(inputPath.end()-5, inputPath.end(), "rgb.png");

  std::cout << "reading rgb from " << inputPath << std::endl;
  cv::Mat rgb = cv::imread(inputPath);
  std::cout << "reading gray from " << inputPath << std::endl;
  cv::Mat gray = cv::imread(inputPath, CV_LOAD_IMAGE_GRAYSCALE);
  uint32_t w = depth.cols;
  uint32_t h = depth.rows;

  if (w==0 || h==0) {
    std::cout << "could not load images" << std::endl;
    return (1);
  }

  cudaPcl::DepthGuidedFilterGpu<float>* depthFilter =
    new cudaPcl::DepthGuidedFilterGpu<float>(w,h,eps,B);
  cudaPcl::NormalExtractSimpleGpu<float>* normalExtract =
    new cudaPcl::NormalExtractSimpleGpu<float>(f_d,w,h,compress);

  depthFilter->filter(depth);
  normalExtract->computeGpu(depthFilter->getDepthDevicePtr());
  cv::Mat normalsImg = normalExtract->normalsImg();

  if(outputPath.compare("") != 0)
  {
    std::ofstream out((outputPath+".normals").data(), std::ofstream::out | std::ofstream::binary);
    out<<h<<" "<<w<<" "<<3<<endl;
    char* data = reinterpret_cast<char*>(normalsImg.data);
    out.write(data, w*h*3*sizeof(float));
//    for (uint32_t i=0; i<h; ++i)
//      for (uint32_t j=0; j<w; ++j)
//        out<< normalsImg.at<cv::Vec3f>(i,j)[0] << " "
//          << normalsImg.at<cv::Vec3f>(i,j)[1] << " "
//          << normalsImg.at<cv::Vec3f>(i,j)[2] <<endl;
    out.close();
    cout << "surface normals writen to "<<outputPath<< ".normals" <<endl;

    cv::Mat dSmooth = depthFilter->getOutput();
    cv::Mat dSmoothS;
    dSmooth.convertTo(dSmoothS, CV_16U, 1000.);
    cv::imwrite((outputPath+"_dSmooth.png").data(), dSmoothS);
    std::cout << "smooth depth written to " << (outputPath+"_dSmooth.png") << std::endl;

//    cv::Mat depth2 = cv::imread((outputPath+"_dSmooth.png").data(),CV_LOAD_IMAGE_ANYDEPTH);
//    cv::imshow("n",dSmoothS);
//    cv::imshow("d",depth);
//    cv::imshow("d2",depth2);
//    while(42) cv::waitKey(10);
//    uint32_t count = 0;
//    for (uint32_t i=0; i<h; ++i)
//      for (uint32_t j=0; j<w; ++j) {
//        pcl::PointXYZINormal p;
//        p.z = depth.at<uint16_t>(i,j)*0.001;
//        p.normal[0] = normalsImg.at<cv::Vec3f>(i,j)[0];
//        if (p.z == p.z && p.normal[0] == p.normal[0]) {
//          ++count;
//        }
//      }
//    out.open((outputPath+".ply").c_str());
//    out << "ply" << std::endl;
//    out << "format ascii 1.0" << std::endl;
//    out << "comment generated by openniSmoothNormals_file of the cudaPcl package" << std::endl;
//    out << "element vertex " << count << std::endl;
//    out << "property float x " << std::endl;
//    out << "property float y " << std::endl;
//    out << "property float z " << std::endl;
//    out << "property float nx " << std::endl;
//    out << "property float ny " << std::endl;
//    out << "property float nz " << std::endl;
//    out << "property uchar red " << std::endl;
//    out << "property uchar green " << std::endl;
//    out << "property uchar blue " << std::endl;
//    out << "end_header" << std::endl;
    pcl::PointCloud<pcl::PointXYZRGBNormal> pcOut;
    for (uint32_t i=0; i<h; ++i)
      for (uint32_t j=0; j<w; ++j) {
        pcl::PointXYZRGBNormal p;
        p.z = depth.at<uint16_t>(i,j)*0.001;
        p.x = (j-float(w/2)+0.5)*p.z/f_d;
        p.y = (i-float(h/2)+0.5)*p.z/f_d;
        p.normal[0] = normalsImg.at<cv::Vec3f>(i,j)[0];
        p.normal[1] = normalsImg.at<cv::Vec3f>(i,j)[1];
        p.normal[2] = normalsImg.at<cv::Vec3f>(i,j)[2];
        p.rgb = ((int)rgb.at<cv::Vec3b>(i,j)[2]) << 16
          | ((int)rgb.at<cv::Vec3b>(i,j)[1]) << 8
          | ((int)rgb.at<cv::Vec3b>(i,j)[0]);
        if (p.x == p.x && p.normal[0] == p.normal[0]) {
          pcOut.push_back(p);
        }
      }
    pcl::PLYWriter writer;
    writer.write(outputPath+".ply", pcOut, false, false);
    std::cout << "output pc ("<<pcOut.size() << ") written to " << (outputPath+".ply") << std::endl;
//    out.close();
  }

  if(display)
  {
    cv::imshow("d",depth);
    cv::imshow("n",normalsImg);
    cv::waitKey(0);
  }

  cout<<cudaDeviceReset()<<endl;
  return (0);
}


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

#include <csignal>
#include <vector>

#include "caffe/filler.hpp"
#include "caffe/layer.hpp"
#include "caffe/util/im2col.hpp"
#include "caffe/util/math_functions.hpp"
#include "caffe/util/rng.hpp"
#include "caffe/malabar_layers.hpp"

namespace caffe {

/*
Setup function
*/
template <typename Dtype>
void PixelFeatureLayer<Dtype>::LayerSetUp(
    const vector<Blob<Dtype>*>& bottom,
    const vector<Blob<Dtype>*>& top) {
  ran_once = false;

  count_ = bottom[0]->count();
  num_ = bottom[0]->num();
  height_ = bottom[0]->height();
  width_ = bottom[0]->width();

  const PixelFeatureParameter& parameter =
    this->layer_param_.pixel_feature_param();

  switch (parameter.type()) {
    case PixelFeatureParameter_Feature_POSITION:
    case PixelFeatureParameter_Feature_RANDOM_POSITION:
    case PixelFeatureParameter_Feature_RANDOM_ROTATE:
      channels_ = 2;
      break;
    case PixelFeatureParameter_Feature_POSITION_AND_RGB:
      channels_ = 2 + bottom[0]->channels();
      break;
    case PixelFeatureParameter_Feature_RGB_AND_POSITION:
      channels_ = 2 + bottom[0]->channels();
      break;
    case PixelFeatureParameter_Feature_RGB:
      channels_ = bottom[0]->channels();
      break;
    case PixelFeatureParameter_Feature_WARPED_POSITION:
      // Location and the rotation angle.
      channels_ = 3;
      break;
    case PixelFeatureParameter_Feature_NUM_RANDOM_POSITION:
      height_ = 1;
      width_ = parameter.num_random_pos();
      channels_ = 2;
      break;
  }
  top[0]->Reshape(num_, channels_, height_, width_);
}

template <typename Dtype>
void PixelFeatureLayer<Dtype>::Reshape(
  const vector<Blob<Dtype>*>& bottom,
  const vector<Blob<Dtype>*>& top) {
    top[0]->Reshape(num_, channels_, height_, width_);
}

/*
Forward CPU function
*/
template <typename Dtype>
void PixelFeatureLayer<Dtype>::Forward_cpu(
  const vector<Blob<Dtype>*>& bottom, const vector<Blob<Dtype>*>& top) {
  const PixelFeatureParameter& parameter =
    this->layer_param_.pixel_feature_param();

  Dtype* top_data = top[0]->mutable_cpu_data();

  switch (this->layer_param_.pixel_feature_param().type()) {
  case PixelFeatureParameter_Feature_POSITION: {
    if (!ran_once) {
      const Dtype scale = parameter.pos_scale();
      const Dtype offset_h = parameter.offset_h();
      const Dtype offset_w = parameter.offset_w();

      for (unsigned int n = 0; n < num_; ++n) {
        for (unsigned int y = 0; y < height_; ++y) {
          Dtype y_offset = scale * y + offset_h;
          for (unsigned int x = 0; x < width_; ++x) {
            top_data[top[0]->offset(n, 0, y, x)] = y_offset;
            top_data[top[0]->offset(n, 1, y, x)] = scale * x + offset_w;
          }
        }
      }
    }

    break;
  }
  case PixelFeatureParameter_Feature_POSITION_AND_RGB: {
    const Dtype scale = parameter.pos_scale();
    const Dtype color_scale = parameter.color_scale();
    const Dtype offset_h = parameter.offset_h();
    const Dtype offset_w = parameter.offset_w();

    for (unsigned int n = 0; n < num_; ++n) {
      for (unsigned int y = 0; y < height_; ++y) {
        Dtype y_offset = scale * y + offset_h;
        for (unsigned int x = 0; x < width_; ++x) {
          top_data[top[0]->offset(n, 0, y, x)] = y_offset;
          top_data[top[0]->offset(n, 1, y, x)] = scale * x + offset_w;
          for (unsigned int c = 0; c < bottom[0]->channels(); ++c) {
            top_data[top[0]->offset(n, c+2, y, x)] =
              color_scale * bottom[0]->data_at(n, c, y, x);
          }
        }
      }
    }
    break;
  }
  case PixelFeatureParameter_Feature_RGB_AND_POSITION: {
    const Dtype scale = parameter.pos_scale();
    const Dtype color_scale = parameter.color_scale();
    const Dtype offset_h = parameter.offset_h();
    const Dtype offset_w = parameter.offset_w();

    for (unsigned int n = 0; n < num_; ++n) {
      for (unsigned int y = 0; y < height_; ++y) {
        Dtype y_offset = scale * y + offset_h;
        for (unsigned int x = 0; x < width_; ++x) {
          for (unsigned int c = 0; c < bottom[0]->channels(); ++c) {
            top_data[top[0]->offset(n, c, y, x)] =
              color_scale * bottom[0]->data_at(n, c, y, x);
          }
          top_data[top[0]->offset(n, bottom[0]->channels(), y, x)] = y_offset;
          top_data[top[0]->offset(n, bottom[0]->channels() + 1, y, x)] =
            scale * x + offset_w;
        }
      }
    }
    break;
  }
  case PixelFeatureParameter_Feature_RGB: {
    const Dtype color_scale = parameter.color_scale();
    for (unsigned int n = 0; n < num_; ++n) {
      for (unsigned int y = 0; y < height_; ++y) {
        for (unsigned int x = 0; x < width_; ++x) {
          for (unsigned int c = 0; c < bottom[0]->channels(); ++c) {
            top_data[top[0]->offset(n, c, y, x)] =
              color_scale * bottom[0]->data_at(n, c, y, x);
          }
        }
      }
    }
    break;
  }
  case PixelFeatureParameter_Feature_RANDOM_POSITION:
  case PixelFeatureParameter_Feature_NUM_RANDOM_POSITION: {
    if (!ran_once) {
      const int input_height = bottom[0]->height();
      const int input_width = bottom[0]->width();
      const Dtype scale = parameter.pos_scale();

      boost::uniform_real<Dtype> random_height(0, input_height);
      boost::variate_generator<caffe::rng_t*, boost::uniform_real<Dtype> >
        variate_height(caffe_rng(), random_height);

      boost::uniform_real<Dtype> random_width(0, input_width);
      boost::variate_generator<caffe::rng_t*, boost::uniform_real<Dtype> >
        variate_width(caffe_rng(), random_width);

      for (unsigned int n = 0; n < num_; ++n) {
        for (unsigned int y = 0; y < height_; ++y) {
          for (unsigned int x = 0; x < width_; ++x) {
            top_data[top[0]->offset(n, 0, y, x)] = scale * variate_height();
            top_data[top[0]->offset(n, 1, y, x)] = scale * variate_width();
          }
        }
      }
    }
    break;
  }
  case PixelFeatureParameter_Feature_WARPED_POSITION: {
    if (!ran_once) {
      const Dtype angle = -parameter.rotation_angle() / 180.0 * M_PI;
      const Dtype scale = parameter.pos_scale();
      const Dtype angle_sigma = parameter.rotation_sigma();

      const Dtype cosAngle = std::cos(angle);
      const Dtype sinAngle = std::sin(angle);

      const Dtype mid_y = height_ / 2;
      const Dtype mid_x = width_ / 2;

      Dtype scaled_y, scaled_x;
      for (unsigned int n = 0; n < num_; ++n) {
        for (unsigned int y = 0; y < height_; ++y) {
          scaled_y = y * scale - mid_y;
          for (unsigned int x = 0; x < width_; ++x) {
            scaled_x = x * scale - mid_x;

            top_data[top[0]->offset(n, 0, y, x)] =
              sinAngle * scaled_x + cosAngle * scaled_y + mid_y;
            top_data[top[0]->offset(n, 1, y, x)] =
              cosAngle * scaled_x - sinAngle * scaled_y + mid_x;
            top_data[top[0]->offset(n, 2, y, x)] =
              angle / angle_sigma;
          }
        }
      }
    }
    break;
  }
  case PixelFeatureParameter_Feature_RANDOM_ROTATE: {
    if (!ran_once) {
      boost::uniform_real<Dtype> random_angle(-10, 10);
      boost::variate_generator<caffe::rng_t*, boost::uniform_real<Dtype> >
        variate_angle(caffe_rng(), random_angle);
      const Dtype angle = variate_angle() / 180.0 * M_PI;
      const Dtype scale = 1;

      const Dtype cosAngle = std::cos(angle);
      const Dtype sinAngle = std::sin(angle);

      const Dtype mid_y = height_ / 2;
      const Dtype mid_x = width_ / 2;

      Dtype scaled_y, scaled_x;
      for (unsigned int n = 0; n < num_; ++n) {
        for (unsigned int y = 0; y < height_; ++y) {
          scaled_y = y * scale - mid_y;
          for (unsigned int x = 0; x < width_; ++x) {
            scaled_x = x * scale - mid_x;

            top_data[top[0]->offset(n, 0, y, x)] =
              sinAngle * scaled_x + cosAngle * (scaled_y - mid_y) + mid_y;
            top_data[top[0]->offset(n, 1, y, x)] =
              cosAngle * scaled_x - sinAngle * scaled_y + mid_x;
          }
        }
      }
    }
    break;
  }
  default:
    LOG(FATAL) << "Undefined feature type of pixel feature layer";
  }

  ran_once = true;
}

/*
Backward CPU function
 */
template <typename Dtype>
void PixelFeatureLayer<Dtype>::Backward_cpu(
    const vector<Blob<Dtype>*>& top, const vector<bool>& propagate_down,
    const vector<Blob<Dtype>*>& bottom) {
}

#ifdef CPU_ONLY
STUB_GPU(PixelFeatureLayer);
#endif

INSTANTIATE_CLASS(PixelFeatureLayer);
REGISTER_LAYER_CLASS(PixelFeature);

}  // namespace caffe


/*
 * Copyright 2015 David A. Boyuka II
 *
 * 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.
 */

/*
 * test-index-binorder.cpp
 *
 *  Created on: Jun 4, 2014
 *      Author: David A. Boyuka II
 */

#include <cstdint>
#include <cassert>
#include <vector>
#include <boost/smart_ptr.hpp>
#include <boost/make_shared.hpp>

#include "pique/data/grid.hpp"
#include "pique/data/dataset.hpp"
#include "pique/region/region-encoding.hpp"
#include "pique/region/ii/ii.hpp"
#include "pique/region/ii/ii-encode.hpp"
#include "pique/indexing/binned-index.hpp"
#include "pique/indexing/index-builder.hpp"
#include "pique/util/universal-value.hpp"

static const std::vector<double>	DATASET_1C = {1.0, -0.0, 0.0, -1.0};
static const std::vector<int>		DATASET_2C = {1, -0, 0, -1};
static const std::vector<uint16_t>	DATASET_US = {1, 0, 0, 2};

template<typename RegionEncoderT, typename DatatypeT>
static void
test_index(typename RegionEncoderT::RegionEncoderConfig conf, const std::vector< DatatypeT > &data, const std::vector< uint64_t > &expected_bins) {
	typedef typename RegionEncoderT::RegionEncodingOutT RegionEncodingT;
	using SigbitsBinningSpec = SigbitsBinningSpecification< DatatypeT >;
	using BinningKeyType = typename SigbitsBinningSpec::QKeyType;
	BOOST_STATIC_ASSERT(boost::is_same< BinningKeyType, uint64_t >::value);

	InMemoryDataset< DatatypeT > dataset((std::vector< DatatypeT >(data)), Grid{data.size()});

	IndexBuilder< DatatypeT, RegionEncoderT, SigbitsBinningSpec > builder(conf, boost::make_shared< SigbitsBinningSpec >(sizeof(DatatypeT)*8));
	boost::shared_ptr< BinnedIndex > index_ptr = builder.build_index(dataset);

	assert(index_ptr->get_num_bins() == expected_bins.size());
	for (BinnedIndexTypes::bin_id_t i = 0; i < index_ptr->get_num_bins(); i++)
		assert(boost::get< BinningKeyType >(index_ptr->get_bin_key(i)) == expected_bins[i]);
}

int main(int argc, char **argv) {
	test_index< IIRegionEncoder, uint16_t >(IIRegionEncoderConfig(), DATASET_US, { 0UL, 1UL, 2UL });
	test_index< IIRegionEncoder, int      >(IIRegionEncoderConfig(), DATASET_2C, { 0xFFFFFFFFUL, 0UL, 1UL });
	test_index< IIRegionEncoder, double   >(IIRegionEncoderConfig(), DATASET_1C, { (1ULL << 63) | (1023UL << 52), (1ULL << 63), 0, (1023UL << 52) });
}


/* boost random/geometric_distribution.hpp header file
 *
 * Copyright Jens Maurer 2000-2001
 * 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://www.boost.org for most recent version including documentation.
 *
 * $Id: geometric_distribution.hpp 60755 2010-03-22 00:45:06Z steven_watanabe $
 *
 * Revision history
 *  2001-02-18  moved to individual header files
 */

#ifndef BOOST_RANDOM_GEOMETRIC_DISTRIBUTION_HPP
#define BOOST_RANDOM_GEOMETRIC_DISTRIBUTION_HPP

#include <boost/config/no_tr1/cmath.hpp>          // std::log
#include <cassert>
#include <iostream>
#include <boost/random/detail/config.hpp>
#include <boost/random/uniform_01.hpp>

namespace boost {

#if defined(__GNUC__) && (__GNUC__ < 3)
// Special gcc workaround: gcc 2.95.x ignores using-declarations
// in template classes (confirmed by gcc author Martin v. Loewis)
  using std::log;
#endif

/**
 * An instantiation of the class template @c geometric_distribution models
 * a \random_distribution.  The distribution produces positive
 * integers which are the number of bernoulli trials
 * with probability @c p required to get one that fails.
 *
 * For the geometric distribution, \f$p(i) = (1-p) p^{i-1}\f$.
 */
template<class IntType = int, class RealType = double>
class geometric_distribution
{
public:
  typedef RealType input_type;
  typedef IntType result_type;

  /**
   * Contructs a new geometric_distribution with the paramter @c p.
   *
   * Requires: 0 < p < 1
   */
  explicit geometric_distribution(const RealType& p = RealType(0.5))
    : _p(p)
  {
    assert(RealType(0) < _p && _p < RealType(1));
    init();
  }

  // compiler-generated copy ctor and assignment operator are fine

  /**
   * Returns: the distribution parameter @c p
   */
  RealType p() const { return _p; }
  void reset() { }

  template<class Engine>
  result_type operator()(Engine& eng)
  {
#ifndef BOOST_NO_STDC_NAMESPACE
    using std::log;
    using std::floor;
#endif
    return IntType(floor(log(RealType(1)-eng()) / _log_p)) + IntType(1);
  }

#ifndef BOOST_RANDOM_NO_STREAM_OPERATORS
  template<class CharT, class Traits>
  friend std::basic_ostream<CharT,Traits>&
  operator<<(std::basic_ostream<CharT,Traits>& os, const geometric_distribution& gd)
  {
    os << gd._p;
    return os;
  }

  template<class CharT, class Traits>
  friend std::basic_istream<CharT,Traits>&
  operator>>(std::basic_istream<CharT,Traits>& is, geometric_distribution& gd)
  {
    is >> std::ws >> gd._p;
    gd.init();
    return is;
  }
#endif

private:

  /// \cond hide_private_functions

  void init()
  {
#ifndef BOOST_NO_STDC_NAMESPACE
    using std::log;
#endif
    _log_p = log(_p);
  }

  /// \endcond

  RealType _p;
  RealType _log_p;
};

} // namespace boost

#endif // BOOST_RANDOM_GEOMETRIC_DISTRIBUTION_HPP


// Copyright Louis Dionne 2013-2017
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#include <boost/hana/assert.hpp>
#include <boost/hana/equal.hpp>
#include <boost/hana/front.hpp>
#include <boost/hana/greater.hpp>
#include <boost/hana/integral_constant.hpp>
#include <boost/hana/negate.hpp>
#include <boost/hana/ordering.hpp>
#include <boost/hana/sort.hpp>
#include <boost/hana/tuple.hpp>

#include <string>
namespace hana = boost::hana;
using namespace hana::literals;
using namespace std::literals;

// sort without a predicate
BOOST_HANA_CONSTANT_CHECK(
    hana::sort(hana::make_tuple(1_c, -2_c, 3_c, 0_c)) ==
               hana::make_tuple(-2_c, 0_c, 1_c, 3_c)
);

// sort with a predicate
BOOST_HANA_CONSTANT_CHECK(
    hana::sort(hana::make_tuple(1_c, -2_c, 3_c, 0_c), hana::greater) ==
               hana::make_tuple(3_c, 1_c, 0_c, -2_c)
);

int main() {
    // sort.by is syntactic sugar
    auto tuples = hana::make_tuple(
        hana::make_tuple(2_c, 'x', nullptr),
        hana::make_tuple(1_c, "foobar"s, hana::int_c<4>)
    );

    BOOST_HANA_RUNTIME_CHECK(
        hana::sort.by(hana::ordering(hana::front), tuples)
            == hana::make_tuple(
                hana::make_tuple(1_c, "foobar"s, hana::int_c<4>),
                hana::make_tuple(2_c, 'x', nullptr)
            )
    );
}


#include <iostream>
#include <string>

#include <boost/program_options.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/json_parser.hpp>
#include <boost/preprocessor/seq/for_each.hpp>

#if defined(SOLVER_BACKEND_VEXCL)
#  include <amgcl/backend/vexcl.hpp>
   typedef amgcl::backend::vexcl<double> Backend;
#elif defined(SOLVER_BACKEND_VIENNACL)
#  include <amgcl/backend/viennacl.hpp>
   typedef amgcl::backend::viennacl< viennacl::compressed_matrix<double> > Backend;
#elif defined(SOLVER_BACKEND_CUDA)
#  include <amgcl/backend/cuda.hpp>
#  include <amgcl/relaxation/cusparse_ilu0.hpp>
   typedef amgcl::backend::cuda<double> Backend;
#else
#  ifndef SOLVER_BACKEND_BUILTIN
#    define SOLVER_BACKEND_BUILTIN
#  endif
#  include <amgcl/backend/builtin.hpp>
#  include <amgcl/value_type/static_matrix.hpp>
#  include <amgcl/adapter/block_matrix.hpp>
#  include <amgcl/make_block_solver.hpp>
   typedef amgcl::backend::builtin<double> Backend;
#endif

#include <amgcl/make_solver.hpp>
#include <amgcl/amg.hpp>
#include <amgcl/solver/runtime.hpp>
#include <amgcl/coarsening/runtime.hpp>
#include <amgcl/relaxation/runtime.hpp>
#include <amgcl/preconditioner/schur_pressure_correction.hpp>
#include <amgcl/preconditioner/runtime.hpp>
#include <amgcl/adapter/crs_tuple.hpp>

#include <amgcl/io/mm.hpp>
#include <amgcl/io/binary.hpp>
#include <amgcl/profiler.hpp>

#ifndef AMGCL_BLOCK_SIZES
#  define AMGCL_BLOCK_SIZES (3)(4)
#endif

namespace amgcl { profiler<> prof; }
using amgcl::prof;
using amgcl::precondition;

template <class USolver, class PSolver, class Matrix>
typename std::enable_if<
    (
        amgcl::math::static_rows<
            typename amgcl::preconditioner::detail::common_backend<
                typename USolver::backend_type,
                typename PSolver::backend_type
            >::type::value_type
        >::value > 1
    ),
    void
    >::type
solve_schur(const Matrix&, const std::vector<double>&, boost::property_tree::ptree&)
{}

//---------------------------------------------------------------------------
template <class USolver, class PSolver, class Matrix>
typename std::enable_if<
    (
        amgcl::math::static_rows<
            typename amgcl::preconditioner::detail::common_backend<
                typename USolver::backend_type,
                typename PSolver::backend_type
            >::type::value_type
        >::value == 1
    ),
    void>::type
solve_schur(const Matrix &K, const std::vector<double> &rhs, boost::property_tree::ptree &prm)
{
    Backend::params bprm;

#if defined(SOLVER_BACKEND_VEXCL)
    vex::Context ctx(vex::Filter::Env);
    std::cout << ctx << std::endl;
    bprm.q = ctx;
#elif defined(SOLVER_BACKEND_VIENNACL)
    std::cout
        << viennacl::ocl::current_device().name()
        << " (" << viennacl::ocl::current_device().vendor() << ")\n\n";
#elif defined(SOLVER_BACKEND_CUDA)
    cusparseCreate(&bprm.cusparse_handle);
    {
        int dev;
        cudaGetDevice(&dev);

        cudaDeviceProp prop;
        cudaGetDeviceProperties(&prop, dev);
        std::cout << prop.name << std::endl << std::endl;
    }
#endif

    auto t1 = prof.scoped_tic("schur_complement");

    prof.tic("setup");
    amgcl::make_solver<
        amgcl::preconditioner::schur_pressure_correction<USolver, PSolver>,
        amgcl::runtime::solver::wrapper<Backend>
        > solve(K, prm, bprm);
    prof.toc("setup");

    std::cout << solve.precond() << std::endl;

    auto f = Backend::copy_vector(rhs, bprm);
    auto x = Backend::create_vector(rhs.size(), bprm);
    amgcl::backend::clear(*x);

    size_t iters;
    double error;

    prof.tic("solve");
    std::tie(iters, error) = solve(*f, *x);
    prof.toc("solve");

    std::cout << "Iterations: " << iters << std::endl
              << "Error:      " << error << std::endl;
}

#define AMGCL_BLOCK_PSOLVER(z, data, B)                                        \
  case B: {                                                                    \
    typedef amgcl::backend::builtin<amgcl::static_matrix<double, B, B> >       \
        BBackend;                                                              \
    typedef amgcl::make_block_solver<                                          \
        amgcl::runtime::preconditioner<BBackend>,                              \
        amgcl::runtime::solver::wrapper<BBackend> >                            \
        PSolver;                                                               \
    solve_schur<USolver, PSolver>(K, rhs, prm);                                \
  } break;

//---------------------------------------------------------------------------
template <class USolver, class Matrix>
void solve_schur(int pb, const Matrix &K, const std::vector<double> &rhs, boost::property_tree::ptree &prm)
{
    switch (pb) {
        case 1:
            {
                typedef
                    amgcl::make_solver<
                        amgcl::runtime::preconditioner<Backend>,
                        amgcl::runtime::solver::wrapper<Backend>
                        >
                    PSolver;
                solve_schur<USolver, PSolver>(K, rhs, prm);
            }
            break;
#if defined(SOLVER_BACKEND_BUILTIN)
        BOOST_PP_SEQ_FOR_EACH(AMGCL_BLOCK_PSOLVER, ~, AMGCL_BLOCK_SIZES)
#endif
        default:
            precondition(false, "Unsupported block size for pressure");
    }
}

#define AMGCL_BLOCK_USOLVER(z, data, B)                                        \
  case B: {                                                                    \
    typedef amgcl::backend::builtin<amgcl::static_matrix<double, B, B> >       \
        BBackend;                                                              \
    typedef amgcl::make_block_solver<                                          \
        amgcl::runtime::preconditioner<BBackend>,                              \
        amgcl::runtime::solver::wrapper<BBackend> >                            \
        USolver;                                                               \
    solve_schur<USolver>(pb, K, rhs, prm);                                     \
  } break;

//---------------------------------------------------------------------------
template <class Matrix>
void solve_schur(int ub, int pb, const Matrix &K, const std::vector<double> &rhs, boost::property_tree::ptree &prm)
{
    precondition(ub == 1 || pb == 1,
            "At least one of the flow/pressure subproblems has to be scalar");

    switch (ub) {
        case 1:
            {
                typedef
                    amgcl::make_solver<
                        amgcl::runtime::preconditioner<Backend>,
                        amgcl::runtime::solver::wrapper<Backend>
                        >
                    USolver;
                solve_schur<USolver>(pb, K, rhs, prm);
            }
            break;
#if defined(SOLVER_BACKEND_BUILTIN)
        BOOST_PP_SEQ_FOR_EACH(AMGCL_BLOCK_USOLVER, ~, AMGCL_BLOCK_SIZES)
#endif
        default:
            precondition(false, "Unsupported block size for flow");
    }
}

//---------------------------------------------------------------------------
int main(int argc, char *argv[]) {
    using std::string;
    using std::vector;

    namespace po = boost::program_options;
    namespace io = amgcl::io;

    po::options_description desc("Options");

    desc.add_options()
        ("help,h", "show help")
        (
         "binary,B",
         po::bool_switch()->default_value(false),
         "When specified, treat input files as binary instead of as MatrixMarket. "
         "It is assumed the files were converted to binary format with mm2bin utility. "
        )
        (
         "matrix,A",
         po::value<string>()->required(),
         "The system matrix in MatrixMarket format"
        )
        (
         "rhs,f",
         po::value<string>(),
         "The right-hand side in MatrixMarket format"
        )
        (
         "pmask,m",
         po::value<string>(),
         "The pressure mask in MatrixMarket format. Or, if the parameter has "
         "the form '%n:m', then each (n+i*m)-th variable is treated as pressure."
        )
        (
         "ub",
         po::value<int>()->default_value(1),
         "Block-size of the 'flow'/'non-pressure' part of the matrix"
        )
        (
         "pb",
         po::value<int>()->default_value(1),
         "Block-size of the 'pressure' part of the matrix"
        )
        (
         "params,P",
         po::value<string>(),
         "parameter file in json format"
        )
        (
         "prm,p",
         po::value< vector<string> >()->multitoken(),
         "Parameters specified as name=value pairs. "
         "May be provided multiple times. Examples:\n"
         "  -p solver.tol=1e-3\n"
         "  -p precond.coarse_enough=300"
        )
        ;

    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);

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

    po::notify(vm);

    boost::property_tree::ptree prm;
    if (vm.count("params")) read_json(vm["params"].as<string>(), prm);

    if (vm.count("prm")) {
        for(const string &v : vm["prm"].as<vector<string> >()) {
            amgcl::put(prm, v);
        }
    }

    size_t rows;
    vector<ptrdiff_t> ptr, col;
    vector<double> val, rhs;
    std::vector<char> pm;

    {
        auto t = prof.scoped_tic("reading");

        string Afile  = vm["matrix"].as<string>();
        bool   binary = vm["binary"].as<bool>();

        if (binary) {
            io::read_crs(Afile, rows, ptr, col, val);
        } else {
            size_t cols;
            std::tie(rows, cols) = io::mm_reader(Afile)(ptr, col, val);
            precondition(rows == cols, "Non-square system matrix");
        }

        if (vm.count("rhs")) {
            string bfile = vm["rhs"].as<string>();

            size_t n, m;

            if (binary) {
                io::read_dense(bfile, n, m, rhs);
            } else {
                std::tie(n, m) = io::mm_reader(bfile)(rhs);
            }

            precondition(n == rows && m == 1, "The RHS vector has wrong size");
        } else {
            rhs.resize(rows, 1.0);
        }

        if(vm.count("pmask")) {
            std::string pmask = vm["pmask"].as<string>();
            prm.put("precond.pmask_size", rows);

            switch (pmask[0]) {
                case '%':
                case '<':
                case '>':
                    prm.put("precond.pmask_pattern", pmask);
                    break;
                default:
                    {
                        size_t n, m;

                        if (binary) {
                            io::read_dense(pmask, n, m, pm);
                        } else {
                            std::tie(n, m) = amgcl::io::mm_reader(pmask)(pm);
                        }

                        precondition(n == rows && m == 1, "Mask file has wrong size");

                        prm.put("precond.pmask", static_cast<void*>(&pm[0]));
                    }
            }
        }
    }

    solve_schur(vm["ub"].as<int>(), vm["pb"].as<int>(),
            std::tie(rows, ptr, col, val), rhs, prm);

    std::cout << prof << std::endl;
}


#include <sm/eigen/gtest.hpp>

#include <numeric>

#include <aslam/backend/test/SampleDvAndError.hpp>

#include <aslam/backend/DenseQrLinearSystemSolver.hpp>
#include <aslam/backend/SparseCholeskyLinearSystemSolver.hpp>
#include <aslam/backend/SparseQrLinearSystemSolver.hpp>
#include <aslam/backend/BlockCholeskyLinearSystemSolver.hpp>
#include <boost/lexical_cast.hpp>
#include <aslam/backend/Optimizer2.hpp>
#include <aslam/backend/OptimizationProblem.hpp>

using namespace aslam::backend;

template<typename S1_TYPE, typename S2_TYPE>
void compareSolvers(int D, int E, bool useM, bool useDiag, int nThreads)
{
  std::string S1Name = typeid(S1_TYPE).name();
  std::string S2Name = typeid(S2_TYPE).name();
  SCOPED_TRACE(("Comparing S1: " + S1Name + " and S2: " + S2Name).c_str());
  std::vector<DesignVariable*> dvs;
  std::vector<ErrorTerm*> errs;
  try {
    buildSystem(D, E, dvs, errs);
    S1_TYPE S1;
    S1.initMatrixStructure(dvs, errs, useDiag);
    S2_TYPE S2;
    S2.initMatrixStructure(dvs, errs, useDiag);
    ASSERT_EQ(S1.JRows(), S2.JRows());
    ASSERT_EQ(S1.JCols(), S2.JCols());
    Eigen::VectorXd diag(S1.JCols());
    diag.setRandom();
    if (useDiag) {
      S1.setConditioner(diag);
      S2.setConditioner(diag);
    }
    Eigen::VectorXd dxS1, dxS2, eS2, eS1, rhsS2, rhsS1;
    double e2S1, e2S2;
    e2S1 = S1.evaluateError(nThreads, useM);
    e2S2 = S2.evaluateError(nThreads, useM);
    ASSERT_NEAR(e2S1, e2S2, 1e-9);
    eS2 = S2.e();
    eS1 = S1.e();
    ASSERT_DOUBLE_MX_EQ(eS2, eS1, 1e-6, "Checking the error vectors");
    ASSERT_EQ((size_t)S2.e().size(), S2.JRows());
    ASSERT_EQ((size_t)S1.e().size(), S1.JRows());
    S1.buildSystem(nThreads, useM);
    S2.buildSystem(nThreads, useM);
    rhsS1 = S1.rhs();
    rhsS2 = S2.rhs();
    ASSERT_DOUBLE_MX_EQ(rhsS1, rhsS2, 1e-6, "Checking right-hand sides");
    S1.solveSystem(dxS1);
    S2.solveSystem(dxS2);
    ASSERT_DOUBLE_MX_EQ(dxS2, dxS1, 1e-6, "Checking the solutions");
    deleteSystem(dvs, errs);
  } catch (const std::exception& e) {
    deleteSystem(dvs, errs);
    FAIL() << e.what();
  }
}
/*
TEST(LinearSolverTestSuite, testDenseQr)
{
   using namespace aslam::backend;
   std::vector<DesignVariable *> dvs;
   std::vector<ErrorTerm *> errs;
   const int D = 4;
   const int E = 20;
   const bool useM = false;
   bool useDiag = true;

   for(int nThreads = 0; nThreads < 16; ++nThreads)
   {
       {
           useDiag = false;
           SCOPED_TRACE(("No Diagonal and " + boost::lexical_cast<std::string>(nThreads) + " threads").c_str());
           compareSolvers<BlockCholeskyLinearSystemSolver, DenseQrLinearSystemSolver>(D,E,useM, useDiag, nThreads);
       }

       {
           useDiag = true;
           SCOPED_TRACE(("With Diagonal and " + boost::lexical_cast<std::string>(nThreads) + " threads").c_str());
           compareSolvers<BlockCholeskyLinearSystemSolver, DenseQrLinearSystemSolver>(D,E,useM, useDiag, nThreads);
       }
   }

}
*/

TEST(LinearSolverTestSuite, testSparseCholesky)
{
  using namespace aslam::backend;
  std::vector<DesignVariable*> dvs;
  std::vector<ErrorTerm*> errs;
  const int D = 4;
  const int E = 20;
  const bool useM = false;
  bool useDiag = true;
  for (int nThreads = 0; nThreads < 4; ++nThreads) {
    {
      useDiag = false;
      SCOPED_TRACE(("No Diagonal and " + boost::lexical_cast<std::string>(nThreads) + " threads").c_str());
      compareSolvers<SparseCholeskyLinearSystemSolver, BlockCholeskyLinearSystemSolver>(D, E, useM, useDiag, nThreads);
    }
    {
      useDiag = true;
      SCOPED_TRACE(("With Diagonal and " + boost::lexical_cast<std::string>(nThreads) + " threads").c_str());
      compareSolvers<SparseCholeskyLinearSystemSolver, BlockCholeskyLinearSystemSolver>(D, E, useM, useDiag, nThreads);
    }
  }
}

TEST(LinearSolverTestSuite, testSparseQR)
{
  using namespace aslam::backend;
  std::vector<DesignVariable*> dvs;
  std::vector<ErrorTerm*> errs;
  const int D = 4;
  const int E = 20;
  const bool useM = false;
  bool useDiag = false;
  for (int nThreads = 0; nThreads < 4; ++nThreads) {
    {
      useDiag = false;
      SCOPED_TRACE(("No Diagonal and " + boost::lexical_cast<std::string>(nThreads) + " threads").c_str());
      compareSolvers<SparseCholeskyLinearSystemSolver, SparseQrLinearSystemSolver>(D, E, useM, useDiag, nThreads);
    }
    /*
            {
                useDiag = true;
                SCOPED_TRACE(("With Diagonal and " + boost::lexical_cast<std::string>(nThreads) + " threads").c_str());
                compareSolvers<SparseCholeskyLinearSystemSolver, SparseQrLinearSystemSolver>(D,E,useM, useDiag, nThreads);
            }*/
  }
}

class ConstZeroError : public ErrorTermFs<1> {
 protected:
  virtual double evaluateErrorImplementation() { return 0; }

  virtual void evaluateJacobiansImplementation(JacobianContainer &) {}
};

TEST(LinearSolverTestSuite, testSparseQRAcceptConstantErrorTerms)
{
  using namespace aslam::backend;
  std::vector<DesignVariable*> dvs;
  std::vector<ErrorTerm*> errs;

  buildSystem(1, 1, dvs, errs);
  errs.push_back(new ConstZeroError);

  SparseQrLinearSystemSolver solver;
  EXPECT_FALSE(solver.isAcceptConstantErrorTerms());
  solver.setAcceptConstantErrorTerms(true);
  EXPECT_NO_THROW(solver.initMatrixStructure(dvs, errs, false));
  EXPECT_NO_THROW(solver.buildSystem(1, false));

  solver.setAcceptConstantErrorTerms(false);
  EXPECT_ANY_THROW(solver.initMatrixStructure(dvs, errs, false));
  deleteSystem(dvs, errs);
}


#include "base/color.hpp"
#include "boost/tokenizer.hpp"
#include "core/plan/runner.hpp"

#include <boost/algorithm/string.hpp>
#include <cmath>
#include <regex>
#include <string>

#include "examples/lr/basic_lr.hpp"

// #define ENABLE_CP

DEFINE_int32(num_of_docs, 1, "# number of docs");
DEFINE_int32(num_doc_partition, 10, "");
DEFINE_int32(num_term_partition, 10, "");

DEFINE_bool(is_sgd, false, "Full gradient descent or mini-batch SGD");

using namespace xyz;

class Document {
public:
  using KeyT = std::string;
  Document() = default;
  explicit Document(const KeyT &t) : title(t) {}
  KeyT title;
  std::vector<double> tf;
  std::vector<double> tf_idf;
  std::vector<int> words;
  int label; // for training
  int total_words = 0;
  const KeyT &id() const { return title; }
  KeyT Key() const { return title; }
  friend SArrayBinStream &operator<<(SArrayBinStream &stream,
                                     const Document &doc) {
    stream << doc.title << doc.tf << doc.tf_idf << doc.words << doc.total_words
           << doc.label;
    return stream;
  }
  friend SArrayBinStream &operator>>(SArrayBinStream &stream, Document &doc) {
    stream >> doc.title >> doc.tf >> doc.tf_idf >> doc.words >>
        doc.total_words >> doc.label;
    return stream;
  }
};

class Term {
public:
  using KeyT = int;
  Term() = default;
  explicit Term(const KeyT &term) : termid(term) { idf = 0; }
  KeyT termid;
  int idf;
  const KeyT &id() const { return termid; }
  KeyT Key() const { return termid; }
  friend SArrayBinStream &operator<<(SArrayBinStream &stream, const Term &t) {
    stream << t.termid << t.idf;
    return stream;
  }
  friend SArrayBinStream &operator>>(SArrayBinStream &stream, Term &t) {
    stream >> t.termid >> t.idf;
    return stream;
  }
};

// lr
static std::pair<int, int> sgd_train(TypedPartition<Document> *p, bool is_sgd,
                                     std::vector<double> &old_params,
                                     std::vector<double> &step_sum, double alpha,
                                     int num_params) {
  auto data_iter = p->begin();
  int count = 0, correct_count = 0;
  int sgd_counter = -1;
  auto end_iter = p->end();
  while (data_iter != end_iter) {
    // sgd: pick 1 point out of 10
    if (is_sgd) {
      sgd_counter++;
      if (sgd_counter % 10 != 0)
        continue;
    }

    auto &words = data_iter->words;
    auto &tf_idf = data_iter->tf_idf;
    CHECK_EQ(words.size(), tf_idf.size());
    int y = data_iter->label;

    double pred_y = 0.0;
    for (int i = 0; i < tf_idf.size(); i++) {
      pred_y += old_params[words[i]] * tf_idf[i];
    }
    pred_y += old_params[num_params - 1]; // intercept
    pred_y = 1. / (1. + exp(-1 * pred_y));

    // Checked and found that half of y is 0 & half is 1, but why the
    // correct_count is always 0 ???
    if ((y == 0 && pred_y < 0.5) || (y == 1 && pred_y >= 0.5)) {
      correct_count++;
    }

    for (int i = 0; i < tf_idf.size(); i++) {
      step_sum[words[i]] += alpha * tf_idf[i] * (y - pred_y);
    }
    step_sum[num_params - 1] += alpha * (y - pred_y); // intercept
    count++;

    ++data_iter;
  }
  return std::make_pair(count, correct_count);
}

int main(int argc, char **argv) {
  Runner::Init(argc, argv);
  const int combine_timeout = ParseCombineTimeout(FLAGS_combine_type);
  if (FLAGS_node_id == 0) {
    LOG(INFO) << "combine_type: " << FLAGS_combine_type
              << ", timeout: " << combine_timeout;
  }

  auto loaded_docs =
      Context::load(FLAGS_url, [](std::string content) {
        // parse extract title
        Document doc("");
        std::vector<int> count;
        if (content.size() > 0) {
	  std::transform(content.begin(), content.end(), content.begin(), ::tolower);
          // boost::char_separator<char> sep(" \t\n.,()\'\":;!?<>");
          boost::char_separator<char> sep(" \t\n");
          boost::tokenizer<boost::char_separator<char>> tok(content, sep);
          for (auto &w : tok) {
            // doc.words.push_back(std::hash<std::string>{}(w)%1000);
            // doc.words.push_back(std::hash<std::string>{}(w)%(2<<18));
            doc.words.push_back(std::hash<std::string>{}(w)%FLAGS_num_params);
            // std::transform(doc.words.back().begin(), doc.words.back().end(),
            // doc.words.back().begin(), ::tolower);
          }
          doc.total_words = doc.words.size();
          std::sort(doc.words.begin(), doc.words.end());
          int n = 0;
          for (int i = 0, j = 0; i < doc.words.size(); i = j) {
            for (j = i + 1; j < doc.words.size(); j++) {
              if (doc.words.at(i) != doc.words.at(j))
                break;
            }
            count.push_back(j - i);
            doc.words[n++] = doc.words[i];
          }
          doc.words.resize(n);
          doc.words.shrink_to_fit();
          doc.tf.resize(doc.words.size());
          doc.tf_idf.resize(doc.words.size());
          for (int i = 0; i < doc.words.size(); i++) {
            doc.tf.at(i) = static_cast<double>(count.at(i)) / doc.total_words;
          }
        }

        return doc;
      });
  Context::count(loaded_docs);

  // no need indexed_docs using pull model.
  // auto indexed_docs =
  // Context::placeholder<Document>(FLAGS_num_doc_partition);
  auto terms_key_part_mapper =
      std::make_shared<HashKeyToPartMapper<int>>(FLAGS_num_term_partition);
  auto terms = Context::placeholder<Term>(FLAGS_num_term_partition,
                                          terms_key_part_mapper);
  /*
  Context::mappartupdate(
      loaded_docs, indexed_docs,
      [](TypedPartition<Document>* p) {
        std::vector<std::pair<std::string, Document>> ret;
        for (auto& doc : *p) {
          ret.push_back({doc.id(),doc});
        }
        return ret;
      },
      [](Document *p_doc, Document doc) {
        *p_doc = std::move(doc);
      })
      ->SetName("build indexed_docs from loaded_docs");
  */

  // Context::sort_each_partition(indexed_docs);
  // Context::sort_each_partition(terms);

  Context::mappartupdate(loaded_docs, terms,
                       [](TypedPartition<Document> *p, Output<int, int> *o) {
                         std::vector<std::pair<int, int>> ret;
                         for (auto &doc : *p) {
                           for (int i = 0; i < doc.words.size(); i++) {
                             o->Add(doc.words[i], 1);
                           }
                         }
                       },
                       [](Term *term, int n) {
                         term->idf += n;

                       })
      ->SetCombine([](int *a, int b) { *a += b; })
      ->SetName("Out all the doc");

  // Context::count(terms);
  auto dummy_collection = Context::placeholder<KVObjT<int, double>>(1);
  Context::mappartwithupdate(
      loaded_docs, terms, dummy_collection,
      [terms_key_part_mapper](TypedPartition<Document> *p,
                              TypedCache<Term> *typed_cache,
                              Output<int, double> *o) {

        std::vector<std::shared_ptr<IndexedSeqPartition<Term>>> with_parts(
            FLAGS_num_term_partition);
        std::map<std::string, int> terms_map;
        int start_idx = rand() % FLAGS_num_term_partition; // random start_idx
                                                           // to avoid overload
                                                           // on one point
        for (int i = 0; i < FLAGS_num_term_partition; i++) {
          int idx = (start_idx + i) % FLAGS_num_term_partition;
          auto start_time = std::chrono::system_clock::now();
          auto part = typed_cache->GetPartition(idx);
          auto end_time = std::chrono::system_clock::now();
          std::chrono::duration<double> duration = end_time - start_time;
          // if (FLAGS_node_id == 0) {
          //   LOG(INFO) << GREEN("fetch time for " + std::to_string(idx) << " :
          //   " + std::to_string(duration.count()));
          // }
          with_parts[idx] =
              std::dynamic_pointer_cast<IndexedSeqPartition<Term>>(part);
        }
        // LOG(INFO) << "fetch done";
        // method1: copy and find

        // method2
        for (auto &doc : *p) {
          for (int i = 0; i < doc.words.size(); i++) {
            int term = doc.words[i];
            auto &with_p = with_parts[terms_key_part_mapper->Get(term)];
            auto *t = with_p->Find(term);
            CHECK_NOTNULL(t);
            int count = t->idf;
            doc.tf_idf[i] =
                doc.tf[i] * std::log(FLAGS_num_of_docs / double(count));
          }
        }

        for (int i = 0; i < FLAGS_num_term_partition; i++) {
          typed_cache->ReleasePart(i);
        }
      },
      [](KVObjT<int, double> *t, double acc) { t->b += acc; })
      ->SetCombine([](double* a, double b) { *a += b; })
      ->SetName("Send idf back to doc");

  Context::foreach(dummy_collection, [](const KVObjT<int, double>& obj) {
    LOG(INFO) << "********** res: " << obj.b << " *********";
  });

  // Logistic regression
  int num_params = FLAGS_num_params + 2;
  double alpha = FLAGS_alpha;
  const int num_param_per_part = FLAGS_num_param_per_part;
  bool is_sgd = FLAGS_is_sgd;

  int num_param_parts = num_params / num_param_per_part;
  if (num_params % num_param_per_part != 0) {
    num_param_parts += 1;
  }
  // there are num_param_parts elements and num_param_parts partition.
  // the partition number does not need to be num_param_parts.
  auto dense_rows =
      create_dense_rows(num_param_parts, num_params, num_param_per_part);

  // auto params = Context::placeholder<Param>(num_param_parts);
  auto p1 =
      Context::mappartwithupdate(
          loaded_docs, dense_rows, dense_rows,
          [num_params, alpha, is_sgd, num_param_parts, num_param_per_part](
              TypedPartition<Document> *p, TypedCache<DenseRow> *typed_cache,
              Output<int, std::vector<double>> *o) {
            std::vector<double> step_sum(num_params, 0);

            // 1. prepare params
            auto begin_time = std::chrono::steady_clock::now();
            auto with_parts =
                prepare_lr_params<DenseRow>(num_param_parts, typed_cache);
            // TOOD:FT for fetching map is not supported yet!
            // so make sure to kill after fetching
            // LOG_IF(INFO, FLAGS_node_id == 0) << GREEN("sleeping");
            // std::this_thread::sleep_for(std::chrono::seconds(1));

            auto end_time = std::chrono::steady_clock::now();
            auto duration =
                std::chrono::duration_cast<std::chrono::milliseconds>(
                    end_time - begin_time);
            // LOG_IF(INFO, FLAGS_node_id == 0) << GREEN("Parameter prepare
            // time: " + std::to_string(duration.count()));
            LOG_IF(INFO, p->id == 0)
                << GREEN("Parameter prepare time: " +
                         std::to_string(duration.count()) + "ms on part 0");

            // 2. copy params
            begin_time = std::chrono::steady_clock::now();
            auto old_params = copy_lr_params<double>(with_parts, num_param_parts,
                                             num_params, num_param_per_part);

            end_time = std::chrono::steady_clock::now();
            duration = std::chrono::duration_cast<std::chrono::milliseconds>(
                end_time - begin_time);
            // LOG_IF(INFO, FLAGS_node_id == 0) << GREEN("Parameter copy time: "
            // + std::to_string(duration.count()));
            LOG_IF(INFO, p->id == 0)
                << GREEN("Parameter copy time: " +
                         std::to_string(duration.count()) + "ms on part 0");

            // 3. calculate
            begin_time = std::chrono::steady_clock::now();
            auto count_correct =
                sgd_train(p, is_sgd, old_params, step_sum, alpha, num_params);
            int count = count_correct.first;
            int correct_count = count_correct.second;

            // create the output
            auto kvs = create_lr_output<double>(num_param_parts, num_param_per_part,
                                        num_params, count, step_sum);
            for (auto &kv : kvs)
              o->Add(kv.first, std::move(kv.second));

            LOG_IF(INFO, p->id == 0) << RED(
                "Correct: " + std::to_string(correct_count) + ", Batch size: " +
                std::to_string(count) + ", Accuracy: " +
                std::to_string(correct_count / double(count)) + " on part 0");

            end_time = std::chrono::steady_clock::now();
            duration = std::chrono::duration_cast<std::chrono::milliseconds>(
                end_time - begin_time);
            LOG_IF(INFO, p->id == 0)
                << GREEN("Computation time: " +
                         std::to_string(duration.count()) + "ms on part 0");

            for (int i = 0; i < num_param_parts; i++) {
              typed_cache->ReleasePart(i);
            }

            return kvs;
          },
          [](DenseRow *row, std::vector<double> v) {
            CHECK_EQ(row->params.size(), v.size());
            for (int i = 0; i < v.size(); ++i) {
              row->params[i] += v[i];
            }
          })
          ->SetIter(FLAGS_num_iter)
          ->SetStaleness(FLAGS_staleness)
#ifdef ENABLE_CP
          ->SetCheckpointInterval(5, "/tmp/tmp/yz")
#endif
          ->SetCombine(
              [](std::vector<double> *v, const std::vector<double> &o) {
                CHECK_EQ(v->size(), o.size());
                for (int i = 0; i < v->size(); ++i) {
                  (*v)[i] += o[i];
                }
              },
              combine_timeout);

  // lr2
  /*
  auto dense_rows2 =
      create_dense_rows(num_param_parts, num_params, num_param_per_part);
  auto p2 =
      Context::mappartwithupdate(
          loaded_docs, dense_rows2, dense_rows2,
          [num_params, alpha, is_sgd, num_param_parts, num_param_per_part](
              TypedPartition<Document> *p, TypedCache<DenseRow> *typed_cache,
              Output<int, std::vector<double>> *o) {
            std::vector<double> step_sum(num_params, 0);

            // 1. prepare params
            auto begin_time = std::chrono::steady_clock::now();
            auto with_parts =
                prepare_lr_params<DenseRow>(num_param_parts, typed_cache);
            // TOOD:FT for fetching map is not supported yet!
            // so make sure to kill after fetching
            // LOG_IF(INFO, FLAGS_node_id == 0) << GREEN("sleeping");
            // std::this_thread::sleep_for(std::chrono::seconds(1));

            auto end_time = std::chrono::steady_clock::now();
            auto duration =
                std::chrono::duration_cast<std::chrono::milliseconds>(
                    end_time - begin_time);
            // LOG_IF(INFO, FLAGS_node_id == 0) << GREEN("Parameter prepare
            // time: " + std::to_string(duration.count()));
            LOG_IF(INFO, p->id == 0)
                << GREEN("Parameter prepare time: " +
                         std::to_string(duration.count()) + "ms on part 0");

            // 2. copy params
            begin_time = std::chrono::steady_clock::now();
            auto old_params = copy_lr_params<double>(with_parts, num_param_parts,
                                             num_params, num_param_per_part);

            end_time = std::chrono::steady_clock::now();
            duration = std::chrono::duration_cast<std::chrono::milliseconds>(
                end_time - begin_time);
            // LOG_IF(INFO, FLAGS_node_id == 0) << GREEN("Parameter copy time: "
            // + std::to_string(duration.count()));
            LOG_IF(INFO, p->id == 0)
                << GREEN("Parameter copy time: " +
                         std::to_string(duration.count()) + "ms on part 0");

            // 3. calculate
            begin_time = std::chrono::steady_clock::now();
            auto count_correct =
                sgd_train(p, is_sgd, old_params, step_sum, alpha, num_params);
            int count = count_correct.first;
            int correct_count = count_correct.second;

            // create the output
            auto kvs = create_lr_output<double>(num_param_parts, num_param_per_part,
                                        num_params, count, step_sum);
            for (auto &kv : kvs)
              o->Add(kv.first, std::move(kv.second));

            LOG_IF(INFO, p->id == 0) << RED(
                "Correct: " + std::to_string(correct_count) + ", Batch size: " +
                std::to_string(count) + ", Accuracy: " +
                std::to_string(correct_count / double(count)) + " on part 0");

            end_time = std::chrono::steady_clock::now();
            duration = std::chrono::duration_cast<std::chrono::milliseconds>(
                end_time - begin_time);
            LOG_IF(INFO, p->id == 0)
                << GREEN("Computation time: " +
                         std::to_string(duration.count()) + "ms on part 0");

            for (int i = 0; i < num_param_parts; i++) {
              typed_cache->ReleasePart(i);
            }

          },
          [](DenseRow *row, std::vector<double> v) {
            CHECK_EQ(row->params.size(), v.size());
            for (int i = 0; i < v.size(); ++i) {
              row->params[i] += v[i];
            }
          })
          ->SetIter(FLAGS_num_iter)
          ->SetStaleness(FLAGS_staleness)
#ifdef ENABLE_CP
          ->SetCheckpointInterval(5, "/tmp/tmp/yz")
#endif
          ->SetCombine(
              [](std::vector<double> *v, const std::vector<double> &o) {
                CHECK_EQ(v->size(), o.size());
                for (int i = 0; i < v->size(); ++i) {
                  (*v)[i] += o[i];
                }
              },
              combine_timeout);
  */

  Runner::Run();
}


#include <fastad_bits/reverse/core/var.hpp>
#include <fastad_bits/reverse/core/eq.hpp>
#include <fastad_bits/reverse/core/unary.hpp>
#include <fastad_bits/reverse/core/binary.hpp>
#include <fastad_bits/reverse/core/eval.hpp>
#include <fastad_bits/reverse/core/pow.hpp>
#include <fastad_bits/reverse/core/prod.hpp>
#include <benchmark/benchmark.h>
#ifdef USE_ADEPT
#include <adept_arrays.h>
#endif

// Finite-difference

static inline double f_test(const std::vector<double>& x)
{
    double prod = 1.;
    for (const auto& xi : x) {
        prod *= xi * xi;
    }
    return prod * prod + std::cos(prod);
}

static void BM_prod_fd(benchmark::State& state)
{
    constexpr double h = 1e-10;
    std::vector<double> x(10);
    for (size_t i = 0; i < x.size(); ++i) {
        x[i] = i;
    }
    for (auto _ : state) {
        double f = f_test(x);
        for (size_t i = 0; i < x.size(); ++i) {
            x[i] += h;
            double f_h = f_test(x);
            double dfdx_i = (f_h - f)/h;
            benchmark::DoNotOptimize(dfdx_i);
            x[i] -= h;
        }
    }
}

BENCHMARK(BM_prod_fd);

// FastAD
static void BM_prod_fastad(benchmark::State& state)
{
    using namespace ad;
    std::vector<Var<double>> vec;
    for (size_t i = 0; i < 1e1; ++i) {
        vec.emplace_back(i);
    }
    Var<double> w4, w5;
    auto prod_expr = ad::prod(vec.begin(), vec.end()
        , [](const auto& x) {
            return x * x;
        });
    auto expr = ad::bind((w4 = prod_expr, w5 = w4 * w4 + ad::cos(w4)));

    for (auto _ : state) {
        autodiff(expr);
        benchmark::ClobberMemory();
    }
}

BENCHMARK(BM_prod_fastad);

#ifdef USE_ADEPT

// Adept
static void BM_prod_adept(benchmark::State& state)
{
    using namespace adept;
    for (auto _ : state) {
        Stack stack;
        aVector x = {0.,1.,2.,3.,4.,5.,6.,7.,8.,9.};
        stack.new_recording();
        aReal y = product(x * x);
        aReal J = pow(y, 2) + cos(y);
        J.set_gradient(1.);
        stack.reverse();
        benchmark::ClobberMemory();
    }
}

BENCHMARK(BM_prod_adept);

#endif


/*
 * @file seq_vector.cc
 * A read-only, monotonic sequence of values.
 */
#include <usml/types/seq_vector.h>

#include <boost/foreach.hpp>
#include <usml/types/seq_data.h>
#include <usml/types/seq_linear.h>
#include <usml/types/seq_log.h>

using namespace usml::types;

/**
 * Clips the current seq_vector based on the intersection of the min
 * and max values provided. Creates and returns new seq_vector using
 * the build_best method. Caller is responsible to delete the returned
 * pointer.
 */
seq_vector::self_type* seq_vector::clip(double min, double max) const {

    size_t index = 0;
    double* data = new double[size()];
    BOOST_FOREACH(double value, *this) {
        if ( ( value >= min ) && ( value <= max ) ) {
            data[index] = value;
             ++index;
        }
    }
    self_type* seq = build_best(data, index);
    delete[] data;
    return seq;
}

/**
 * Builds the best seq_vector pointer from seq_data, seq_linear or seq_log
 * based on the contents of the array.
 */
seq_vector::self_type* seq_vector::build_best(double* data, size_t count) {

    bool isLinear = true;
    bool isLog = true;

    const int N = ( int ) count;
    double p1 = data[0];
    double minValue = p1;
    double maxValue = p1;

    if ( N > 1 ) {
        double p2 = data[1];
        maxValue = p2;
        for ( int n = 2; n < N; ++n ) {
            double p3 = data[n];
            maxValue = p3;
            if ( abs(( ( p3 - p2 ) - ( p2 - p1 ) ) / p2) > 1E-4 ) {
                isLinear = false;
            }
            if ( p1 == 0.0 || p2 == 0.0 ||
                abs(( p3 / p2 ) - ( p2 / p1 )) > 1E-5 ) {
                isLog = false;
            }
            if ( !( isLinear || isLog ) ) break;
            p1 = p2;
            p2 = p3;
        }
    }

    // build a new seq_vector sub type
    if ( isLinear ) {
        return new seq_linear(minValue, ( maxValue - minValue ) / ( N - 1 ), N);
    }
    else if ( isLog ) {
        return new seq_log(minValue, ( maxValue / minValue ) / ( N - 1 ), N);
    }
    return new seq_data(data, count);
}

/**
 * Builds the best seq_vector pointer from seq_data, seq_linear or seq_log
 * based on the contents of the ublas vector.
 */
seq_vector::self_type* seq_vector::build_best(boost::numeric::ublas::vector<double> data) {

    double* data_array = new double[data.size()-1];
    size_t index = 0;
    BOOST_FOREACH(double value, data) {
        data_array[index] = value;
         ++index;
    }
    self_type* seq = build_best(data_array, index);
    delete[] data_array;
    return seq;
}

/**
 * Builds the best seq_vector pointer from seq_data, seq_linear or seq_log
 * based on the contents of the std::vector.
 */
seq_vector::self_type* seq_vector::build_best(std::vector<double> data) {

    double* data_array = new double[data.size()-1];
    size_t index = 0;
    BOOST_FOREACH(double value, data) {
        data_array[index] = value;
        ++index;
    }
    self_type* seq = build_best(data_array, index);
    delete[] data_array;
    return seq;
}


/**
 * @file   test_lpmcf.cpp
 * @brief  test @ref limbo::solvers::lpmcf::Lgf and @ref limbo::solvers::lpmcf::LpDualMcf
 * @author Yibo Lin
 * @date   Oct 2014
 */

#include <iostream>
#include <boost/algorithm/string/predicate.hpp>
#include <limbo/solvers/lpmcf/Lgf.h>
#include <limbo/solvers/lpmcf/LpDualMcf.h>

using std::cout;
using std::endl;

/// @brief main function
///
/// If given .lgf file, solve min-cost flow of the graph by @ref limbo::solvers::lpmcf::Lgf;
/// If given .lp file, solver linear programming problem with dual min-cost flow by @ref limbo::solvers::lpmcf::LpDualMcf.
/// Here we assume .lgf and .lp inputs describe different sets of problems that can be solved by the mathematical solver.
/// .Lgf format is defined by Lemon to describe a network flow graph
///
/// @param argc number of arguments
/// @param argv values of arguments
/// @return 0
int main(int argc, char** argv)
{
	if (argc > 1)
	{
		limbo::solvers::lpmcf::Lgf<long> lpsolver1;
		limbo::solvers::lpmcf::LpDualMcf<long> lpsolver2;

		string filename = argv[1];
		if (boost::ends_with(filename, ".lgf"))
		{
			lpsolver1(filename);
			lpsolver1.print_graph("graph1");
		}
		else if (boost::ends_with(filename, ".lp"))
		{
			lpsolver2(filename);
			lpsolver2.print_graph("graph2");
		}
		else
			cout << "only support .lgf and .lp file formats" << endl;

		//lpsolver();
	}
	else
		cout << "at least 1 argument required\n";

	return 0;
}


/*
    Copyright 2016 Emanuele Vespa, Imperial College London
    Redistribution and use in source and binary forms, with or without
    modification, are permitted provided that the following conditions are met:

    1. Redistributions of source code must retain the above copyright notice, this
    list of conditions and the following disclaimer.

    2. Redistributions in binary form must reproduce the above copyright notice,
    this list of conditions and the following disclaimer in the documentation
    and/or other materials provided with the distribution.

    3. Neither the name of the copyright holder nor the names of its contributors
    may be used to endorse or promote products derived from this software without
    specific prior written permission.

    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
    ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
    WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
    DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
    FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
    DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
    SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
    OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
    OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

*/
#ifndef OCTANT_OPS_HPP
#define OCTANT_OPS_HPP
#include "utils/morton_utils.hpp"
#include "utils/math_utils.h"
#include "octree_defines.h"
#include <iostream>
#include <bitset>
#include <Eigen/Dense>

namespace se {
  namespace keyops {

    inline se::key_t code(const se::key_t key) {
      return key & ~SCALE_MASK;
    }

    inline int level(const se::key_t key) {
      return key & SCALE_MASK;
}

    inline se::key_t encode(const int x, const int y, const int z,
        const int level, const int max_depth) {
      const int offset = MAX_BITS - max_depth + level - 1;
      return (compute_morton(x, y, z) & MASK[offset]) | level;
    }

    inline Eigen::Vector3i decode(const se::key_t key) {
      return unpack_morton(key & ~SCALE_MASK);
    }
  }
}

/*
 * Algorithm 5 of p4est paper: https://epubs.siam.org/doi/abs/10.1137/100791634
 */
inline Eigen::Vector3i face_neighbour(const se::key_t o,
    const unsigned int face, const unsigned int l,
    const unsigned int max_depth) {
  Eigen::Vector3i coords = se::keyops::decode(o);
  const unsigned int side = 1 << (max_depth - l);
  coords(0) = coords(0) + ((face == 0) ? -side : (face == 1) ? side : 0);
  coords(1) = coords(1) + ((face == 2) ? -side : (face == 3) ? side : 0);
  coords(2) = coords(2) + ((face == 4) ? -side : (face == 5) ? side : 0);
  return {coords(0), coords(1), coords(2)};
}

/*
 * \brief Return true if octant is a descendant of ancestor
 * \param octant
 * \param ancestor
 * \param max_depth max depth of the tree on which the octant lives
 */
inline bool descendant(se::key_t octant, se::key_t ancestor,
    const int max_depth) {
  const int level = se::keyops::level(ancestor);
  const int idx = MAX_BITS - max_depth + level - 1;
  ancestor = se::keyops::code(ancestor);
  octant = se::keyops::code(octant) & MASK[idx];
  return (ancestor ^ octant) == 0;
}

/*
 * \brief Computes the parent's morton code of a given octant
 * \param octant
 * \param max_depth max depth of the tree on which the octant lives
 */
inline se::key_t parent(const se::key_t& octant, const int max_depth) {
  const int level = se::keyops::level(octant) - 1;
  const int idx = MAX_BITS - max_depth + level - 1;
  return (octant & MASK[idx]) | level;
}

/*
 * \brief Computes the octants's id in its local brotherhood
 * \param octant
 * \param level of octant
 * \param max_depth max depth of the tree on which the octant lives
 */
inline int child_id(se::key_t octant, const int level,
    const int max_depth) {
  int shift = max_depth - level;
  octant = se::keyops::code(octant) >> shift*3;
  int idx = (octant & 0x01) | (octant & 0x02) | (octant & 0x04);
  return idx;
}

/*
 * \brief Computes the octants's corner which is not shared with its siblings
 * \param octant
 * \param level of octant
 * \param max_depth max depth of the tree on which the octant lives
 */
inline Eigen::Vector3i far_corner(const se::key_t octant, const int level,
    const int max_depth) {
  const unsigned int side = 1 << (max_depth - level);
  const int idx = child_id(octant, level, max_depth);
  const Eigen::Vector3i coordinates = se::keyops::decode(octant);
  return Eigen::Vector3i(coordinates(0) + (idx & 1) * side,
                   coordinates(1) + ((idx & 2) >> 1) * side,
                   coordinates(2) + ((idx & 4) >> 2) * side);
}

/*
 * \brief Computes the non-sibling neighbourhood around an octants. In the
 * special case in which the octant lies on an edge, neighbour are duplicated
 * as movement outside the enclosing cube is forbidden.
 * \param result 7-vector containing the neighbours
 * \param octant
 * \param level of octant
 * \param max_depth max depth of the tree on which the octant lives
 */
inline void exterior_neighbours(se::key_t result[7],
    const se::key_t octant, const int level, const int max_depth) {

  const int idx = child_id(octant, level, max_depth);
  Eigen::Vector3i dir = Eigen::Vector3i((idx & 1) ? 1 : -1,
                       (idx & 2) ? 1 : -1,
                       (idx & 4) ? 1 : -1);
  Eigen::Vector3i base = far_corner(octant, level, max_depth);
  dir(0) = se::math::in(base(0) + dir(0) , 0, (1 << max_depth) - 1) ? dir(0) : 0;
  dir(1) = se::math::in(base(1) + dir(1) , 0, (1 << max_depth) - 1) ? dir(1) : 0;
  dir(2) = se::math::in(base(2) + dir(2) , 0, (1 << max_depth) - 1) ? dir(2) : 0;

 result[0] = se::keyops::encode(base(0) + dir(0), base(1) + 0, base(2) + 0,
     level, max_depth);
 result[1] = se::keyops::encode(base(0) + 0, base(1) + dir(1), base(2) + 0,
     level, max_depth);
 result[2] = se::keyops::encode(base(0) + dir(0), base(1) + dir(1), base(2) + 0,
     level, max_depth);
 result[3] = se::keyops::encode(base(0) + 0, base(1) + 0, base(2) + dir(2),
     level, max_depth);
 result[4] = se::keyops::encode(base(0) + dir(0), base(1) + 0, base(2) + dir(2),
     level, max_depth);
 result[5] = se::keyops::encode(base(0) + 0, base(1) + dir(1), base(2) + dir(2),
     level, max_depth);
 result[6] = se::keyops::encode(base(0) + dir(0), base(1) + dir(1),
     base(2) + dir(2), level, max_depth);
}

/*
 * \brief Computes the morton number of all siblings around an octant,
 * including itself.
 * \param result 8-vector containing the neighbours
 * \param octant
 * \param max_depth max depth of the tree on which the octant lives
 */
inline void siblings(se::key_t result[8],
    const se::key_t octant, const int max_depth) {
  const int level = (octant & SCALE_MASK);
  const int shift = 3*(max_depth - level);
  const se::key_t p = parent(octant, max_depth) + 1; // set-up next level
  for(int i = 0; i < 8; ++i) {
    result[i] = p | (i << shift);
  }
}
#endif


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2010 Roland Lichters

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program 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 license for more details.
*/

#include <qlo/qladdindefines.hpp>
#include <qlo/credit.hpp>
#include <qlo/enumerations/factories/termstructuresfactory.hpp>

#include <ql/instruments/stock.hpp>
#include <ql/quote.hpp>
#include <ql/time/daycounter.hpp>
#include <ql/termstructures/credit/interpolatedhazardratecurve.hpp>
#include <ql/termstructures/yield/piecewiseyieldcurve.hpp>
#include <ql/termstructures/credit/piecewisedefaultcurve.hpp>
#include <ql/math/interpolations/backwardflatinterpolation.hpp>
#include <ql/math/interpolations/loginterpolation.hpp>
#include <ql/pricingengines/credit/midpointcdsengine.hpp>

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

#include <ql/settings.hpp>

using boost::algorithm::to_upper_copy;

namespace QuantLibAddin {

    CreditDefaultSwap::CreditDefaultSwap(
              const boost::shared_ptr<ObjectHandler::ValueObject>& properties,
              QuantLib::Protection::Side side,
              QuantLib::Real notional,
              QuantLib::Rate upfront,
              QuantLib::Rate spread,
              const boost::shared_ptr<QuantLib::Schedule>& schedule,
              QuantLib::BusinessDayConvention paymentConvention,
              const QuantLib::DayCounter& dayCounter,
              bool settlesAccrual,
              bool paysAtDefaultTime,
              const QuantLib::Date& protectionStart,
              const QuantLib::Date& upfrontDate,
              bool permanent)
        : Instrument(properties, permanent) {
        libraryObject_ = boost::shared_ptr<QuantLib::CreditDefaultSwap>(
                    new QuantLib::CreditDefaultSwap(side,
                                                    notional,
                                                    upfront,
                                                    spread,
                                                    *schedule,
                                                    paymentConvention,
                                                    dayCounter,
                                                    settlesAccrual,
                                                    paysAtDefaultTime,
                                                    protectionStart,
                                                    upfrontDate));
    }

    MidPointCdsEngine::MidPointCdsEngine(
            const boost::shared_ptr<ObjectHandler::ValueObject>& properties,
            const QuantLib::Handle<QuantLib::DefaultProbabilityTermStructure>& defaultTS,
            QuantLib::Real recoveryRate,
            const QuantLib::Handle<QuantLib::YieldTermStructure>& yieldTS,
            bool permanent)
        : PricingEngine(properties, permanent) {
        libraryObject_ = boost::shared_ptr<QuantLib::PricingEngine>(new
              QuantLib::MidPointCdsEngine(defaultTS, recoveryRate, yieldTS));
    }

    SpreadCdsHelper::SpreadCdsHelper(
            const boost::shared_ptr<ObjectHandler::ValueObject>& properties,
            const QuantLib::Handle<QuantLib::Quote>& quote,
            const QuantLib::Period& period,
            QuantLib::Natural settlementDays,
            const QuantLib::Calendar& calendar,
            QuantLib::Frequency frequency,
            QuantLib::BusinessDayConvention paymentConvention,
            QuantLib::DateGeneration::Rule rule,
            const QuantLib::DayCounter& dayCounter,
            QuantLib::Real recoveryRate,
            const QuantLib::Handle<QuantLib::YieldTermStructure>& yieldTS,
            bool settlesAccrual,
            bool paysAtDefaultTime,
            bool permanent) : DefaultProbabilityHelper(properties, permanent) {

        libraryObject_ = boost::shared_ptr<QuantLib::DefaultProbabilityHelper>(new
		       QuantLib::SpreadCdsHelper(quote,
						 period,
						 settlementDays,
						 calendar,
						 frequency,
						 paymentConvention,
						 rule,
						 dayCounter,
						 recoveryRate,
						 yieldTS,
						 settlesAccrual,
						 paysAtDefaultTime));
    }

    UpfrontCdsHelper::UpfrontCdsHelper(
            const boost::shared_ptr<ObjectHandler::ValueObject>& properties,
            const QuantLib::Handle<QuantLib::Quote>& quote,
            QuantLib::Rate runningSpread,
            const QuantLib::Period& period,
            QuantLib::Natural settlementDays,
            const QuantLib::Calendar& calendar,
            QuantLib::Frequency frequency,
            QuantLib::BusinessDayConvention paymentConvention,
            QuantLib::DateGeneration::Rule rule,
            const QuantLib::DayCounter& dayCounter,
            QuantLib::Real recoveryRate,
            const QuantLib::Handle<QuantLib::YieldTermStructure>& yieldTS,
            QuantLib::Natural upfrontSettlementDays,
            bool settlesAccrual,
            bool paysAtDefaultTime,
            bool permanent) : DefaultProbabilityHelper(properties, permanent) {
        libraryObject_ = boost::shared_ptr<QuantLib::DefaultProbabilityHelper>(new
		       QuantLib::UpfrontCdsHelper(quote,
                                          runningSpread,
                                          period,
                                          settlementDays,
                                          calendar,
                                          frequency,
                                          paymentConvention,
                                          rule,
                                          dayCounter,
                                          recoveryRate,
                                          yieldTS,
                                          upfrontSettlementDays,
                                          settlesAccrual,
                                          paysAtDefaultTime));
    }

    HazardRateCurve::HazardRateCurve(
            const boost::shared_ptr<ObjectHandler::ValueObject>& properties,
            const std::vector<QuantLib::Date>& dates,
            const std::vector<QuantLib::Rate>& hazardRates,
            const QuantLib::DayCounter& dayCounter,
            bool permanent)
        : DefaultProbabilityTermStructure(properties, permanent) {
        QL_REQUIRE(!dates.empty(), "no input dates given");
        QL_REQUIRE(dates.size() == hazardRates.size(),
                   "vector sizes differ");
        libraryObject_ = boost::shared_ptr<QuantLib::Extrapolator>(
        new QuantLib::InterpolatedHazardRateCurve<QuantLib::BackwardFlat>(
 				 dates, hazardRates, dayCounter));
    }

    PiecewiseFlatHazardRateCurve::PiecewiseFlatHazardRateCurve(
            const boost::shared_ptr<ObjectHandler::ValueObject>& properties,
            const QuantLib::Date& referenceDate,
            const std::vector<boost::shared_ptr<QuantLib::DefaultProbabilityHelper> >& helpers,
            const QuantLib::DayCounter& dayCounter,
            QuantLib::Real accuracy,
            bool permanent)
        : DefaultProbabilityTermStructure(properties, permanent) {
        libraryObject_ = boost::shared_ptr<QuantLib::Extrapolator>(new
               QuantLib::PiecewiseDefaultCurve<QuantLib::HazardRate,QuantLib::BackwardFlat>(referenceDate, helpers, dayCounter));
    }

    PiecewiseFlatForwardCurve::PiecewiseFlatForwardCurve(
            const boost::shared_ptr<ObjectHandler::ValueObject>& properties,
            const QuantLib::Date& referenceDate,
            const std::vector<boost::shared_ptr<QuantLib::RateHelper> >& helpers,
            const QuantLib::DayCounter& dayCounter,
            QuantLib::Real accuracy,
            bool permanent)
        : YieldTermStructure(properties, permanent) {
        libraryObject_ = boost::shared_ptr<QuantLib::Extrapolator>(new
               QuantLib::PiecewiseYieldCurve<QuantLib::Discount,QuantLib::LogLinear>(referenceDate, helpers, dayCounter));
    }

}


#include "led.h"
#include "show.hpp"
#include <opencv2/opencv.hpp>
#include <boost/asio.hpp>
#include <thread>
#include <mutex>
#include <chrono>
#include <random>

namespace
{
    std::mutex sMutex;
    int m[LED_WIDTH][LED_HEIGHT][LED_DEPTH];
    std::string sUrl;
    uint16_t sPort = 9001;
    bool sIsEnableSimulator = true;

    class MutexLocker
    {
        std::mutex & m;
        MutexLocker & operator = (MutexLocker &) = delete;
    public:
        MutexLocker(std::mutex & mutex) : m(mutex){ m.lock(); }
        ~MutexLocker(){ m.unlock(); }
    };

    struct xyz_t{
        double x, y, z;
        double len() const { return std::sqrt(x*x + y*y + z*z); }
    };

    xyz_t xyz(double x, double y, double z)
    {
        return{ x, y, z };
    }

    struct xyzc_t{
        xyz_t p;
        int color;
    };

    xyz_t operator*(xyz_t const & a, double t)
    {
        return{ a.x*t, a.y*t, a.z*t };
    }

    xyz_t operator+(xyz_t const & a, xyz_t const & b)
    {
        return{ a.x + b.x, a.y + b.y, a.z + b.z };
    }

    xyz_t operator-(xyz_t const & a, xyz_t const & b)
    {
        return{ a.x - b.x, a.y - b.y, a.z - b.z };
    }

    xyz_t & operator+=(xyz_t & a, xyz_t const & b)
    {
        a = a + b;
        return a;
    }

    xyz_t & operator-=(xyz_t & a, xyz_t const & b)
    {
        a = a - b;
        return a;
    }

    double ip(xyz_t const & a, xyz_t const & b)
    {
        return a.x*b.x + a.y*b.y + a.z*b.z;
    }

    xyz_t op(xyz_t const & a, xyz_t const & b)
    {
        return{
            a.y*b.z - a.z*b.y,
            a.z*b.x - a.x*b.z,
            a.x*b.y - a.y*b.x
        };
    }

    xyz_t sphere_face(std::mt19937 & rng)
    {
        std::uniform_real_distribution<double> dist(-1, 1);
        for (;;) {
            xyz_t pos = { dist(rng), dist(rng), dist(rng) };
            double d2 = pos.x*pos.x + pos.y*pos.y + pos.z*pos.z;
            if (0.1<d2 && d2 < 1) {
                double d = 0.5;
                return{ pos.x * d, pos.y * d + 0.1, pos.z * d };
            }
        }
    }

    bool can_show(xyz_t const & p)
    {
        return 0 <= p.x && p.x < LED_WIDTH
        && 0 <= p.y && p.y < LED_HEIGHT
        && 0 <= p.z && p.z < LED_DEPTH;
    }
}

void SetUrl(char const * url)
{
    sUrl = url;
}

void SetPort(unsigned short port)
{
    sPort = port;
}

void SetLed(int x, int y, int z, int rgb)
{
    MutexLocker locker(sMutex);
    if (x < 0 || LED_WIDTH <= x){
        return;
    }
    if (y < 0 || LED_HEIGHT <= y){
        return;
    }
    if (z < 0 || LED_DEPTH <= z){
        return;
    }
    m[x][y][z] = rgb;
}

void Clear()
{
    MutexLocker locker(sMutex);
    for (int x = 0; x < LED_WIDTH; ++x){
        for (int y = 0; y < LED_HEIGHT; ++y){
            for (int z = 0; z < LED_DEPTH; ++z){
                m[x][y][z] = 0;
            }
        }
    }
}

namespace {
    void send(std::string url, unsigned short port, int(&m)[LED_WIDTH][LED_HEIGHT][LED_DEPTH])
    {
        std::stringstream src;
        for (int x = 0; x < LED_WIDTH; ++x){
            for (int y = 0; y < LED_HEIGHT; ++y){
                for (int z = 0; z < LED_DEPTH; ++z){
                    int v = m[x][y][z];
                    char c[2] = { 0 };
                    c[0] = static_cast<char>((v & 0xF80000) >> 16) + ((v & 0xE000) >> 13);
                    c[1] = static_cast<char>((v & 0x1C00) >> 5) + ((v & 0xF8) >> 3);
                    src << c[0] << c[1];
                }
            }
        }
        namespace asio = boost::asio;
        namespace ip = asio::ip;
        boost::asio::io_service io_service;
        ip::udp::socket socket(io_service);
        socket.open(ip::udp::v4());
        ip::udp::endpoint ep(ip::udp::endpoint(ip::address::from_string(url), port));
        socket.send_to(boost::asio::buffer(src.str()), ep);
    }
}

void Show()
{
    MutexLocker locker(sMutex);
    try{
        if (!sUrl.empty()){
            send(sUrl, sPort, m);
        }
        if (sIsEnableSimulator){
            ShowWindow("Sender", m);
        }
    }
    catch (std::exception const & e){
        std::cerr << e.what() << std::endl;
    }
}

void Wait(int ms)
{
    static auto sLastWaitTime = std::chrono::system_clock::now();
    auto now = std::chrono::system_clock::now();
    int diff = static_cast<int>(std::chrono::duration_cast<std::chrono::milliseconds>(now - sLastWaitTime).count());
    ms = std::max(1, ms - diff);
    std::this_thread::sleep_for(std::chrono::milliseconds(ms));
    sLastWaitTime = std::chrono::system_clock::now();
}

void EnableSimulator(bool isEnable)
{
    sIsEnableSimulator = isEnable;
}

namespace
{
    std::string toText(cv::Mat const & src)
    {
        std::string dst;
        for (int y = 0; y < src.rows; ++y){
            for (int x = 0; x < src.cols; ++x){
                unsigned char b = src.at<unsigned char>(y, x);
                char a = 'A' + b / 16;
                dst += a;
            }
        }
        return dst;
    }

    cv::Mat makeMat(char c)
    {
        char chr[2] = { c };
        int thickness = 2;
        int baseline = 0;
        auto const s = cv::getTextSize(chr, cv::FONT_HERSHEY_DUPLEX, 1, thickness, &baseline);
        cv::Mat m = cv::Mat::zeros({ s.width, s.height + baseline }, CV_8UC1);
        cv::putText(m, chr, { (m.cols - s.width) / 2, (m.rows + s.height) / 2 }, cv::FONT_HERSHEY_DUPLEX, 1, 255, thickness, CV_AA, false);
        cv::resize(m, m, { LED_WIDTH, LED_HEIGHT });
        return m;
    }

    std::vector<std::string> makeLetters(std::string str)
    {
        std::vector<std::string> dst;
        for (auto it = str.begin(); it != str.end(); ++it){
            dst.push_back(toText(makeMat(*it)));
        }
        return dst;
    }
}

void ShowMotioningText1(const char * text)
{
    const int N0 = 4 * 2;
    const int N1 = 4 * 2;
    auto color = [&](std::string const & a, xyz_t p, int ix)->int {
        int colbase;
        int xx = static_cast<int>(p.x * 15 / LED_WIDTH);
        int yy = static_cast<int>(p.y * 15 / LED_HEIGHT);
        auto cb = [](int r, int g, int b)->int
        {
            return 0x1 * b + 0x100 * g + 0x10000 * r;
        };
        switch (ix % 3) {
            case 0:  colbase = cb(xx, yy, 15); break;
            case 1:  colbase = cb(15, 15 - xx, yy); break;
            case 2:  colbase = cb(15 - yy, 15, xx); break;
            default:
                throw "DEATH!";
        }
        return (a[static_cast<int>(p.x + p.y*LED_WIDTH)] - 'A') * colbase;
    };
    auto show = [&](std::string const & a, std::string const & b, double move, double light, int ix) {
        struct pp_t{
            xyz_t p[2];
        };
        std::mt19937 rng(1);
        std::vector<pp_t> pps;
        while (pps.size()<2000) {
            xyz_t p0{
                static_cast<double>(rng() % LED_WIDTH),
                static_cast<double>(rng() % LED_HEIGHT),
                static_cast<double>(rng() % LED_DEPTH) };
            xyz_t p1{
                static_cast<double>(rng() % LED_WIDTH),
                static_cast<double>(rng() % LED_HEIGHT),
                static_cast<double>(rng() % LED_DEPTH) };
            if (color(a, p0, ix) && color(b, p1, ix + 1)) {
                pps.push_back({ { p0, p1 } });
            }
        }
        Clear();
        for (auto const & pp : pps) {
            xyz_t p = pp.p[0] * (1 - move) + pp.p[1] * move;
            int c0 = color(a, pp.p[0], ix);
            int c1 = color(b, pp.p[1], ix + 1);
            auto mean = [&](int mask, double d) {
                int v = static_cast<int>(((c0 & mask)*(1 - move) + (c1 & mask)*move)*d);
                return v & mask;
            };
            int col = mean(0xff0000, light) + mean(0xff00, light) + mean(0xff, light);
            if (can_show(p)) {
                SetLed(static_cast<int>(p.x), static_cast<int>(p.y), static_cast<int>(p.z), col);
            }
        }
        Show();
        Wait(50);
    };
    auto fade_in = [&](std::string const & c, int letter_index) {
        for (int ix = 0; ix < N0; ++ix) {
            show(c, c, 0, ix*1.0 / N0, letter_index);
        }
    };
    auto fade_out = [&](std::string const & c, int letter_index) {
        for (int ix = 0; ix < N0; ++ix) {
            show(c, c, 0, 1 - ix*1.0 / N0, letter_index);
        }
    };
    auto stop = [&](std::string const &  c, int letter_index) {
        for (int ix = 0; ix < N0; ++ix) {
            show(c, c, 0, 1, letter_index);
        }
    };
    auto move = [&](std::string const & a, std::string const & b, int letter_index) {
        for (int ix = 0; ix < N1; ++ix) {
            show(a, b, ix*1.0 / N1, 1, letter_index);
        }
    };

    auto const letters = makeLetters(text);
    for (size_t ix = 0; ix < letters.size(); ++ix) {
        if (ix == 0) {
            fade_in(letters[ix], ix);
            stop(letters[ix], ix);
        }
        else if (ix == letters.size() - 1) {
            stop(letters[ix], ix);
            fade_out(letters[ix], ix);
        }
        else {
            stop(letters[ix], ix);
        }
        if (ix < letters.size() - 1) {
            move(letters[ix], letters[ix + 1], ix);
        }
    }
}

void SetChar(int x, int y, int z, char c, int rgb)
{
    if (x < -LED_WIDTH || LED_WIDTH <= x){
        return;
    }
    if (y < -LED_HEIGHT || LED_HEIGHT <= y){
        return;
    }
    if (z < -LED_DEPTH || LED_DEPTH <= z){
        return;
    }
    auto u = [](int rgb, int shift){
        return static_cast<unsigned char>((rgb >> shift) & 0xFF);
    };
    auto src = makeMat(c);
    for (int y2 = 0; y2 < src.rows; ++y2){
        for (int x2 = 0; x2 < src.cols; ++x2){
            unsigned char c = src.at<unsigned char>(y2, x2);
            int r = u(rgb, 16) * c / 255;
            int g = u(rgb, 8) * c / 255;
            int b = u(rgb, 0) * c / 255;
            int rgb2 = 0x1 * b + 0x100 * g + 0x10000 * r;
            int xx = x + x2;
            int yy = y + y2;
            SetLed(xx, yy, z, rgb2);
        }
    }
}

void ShowFirework(int x, int y, int z)
{
    const int N = 39;
    std::vector<xyzc_t> poss;
    std::vector<xyz_t> vs(poss.size());
    std::mt19937 rng;
    const int PS = 1000;
    auto rgb = [](double x)->int {
        auto red = [](int ix)->int {
            int i = ix % 90;
            if (i < 30) {
                return i * 255 / 30;
            }
            if (i < 60) {
                return (60 - i) * 255 / 30;
            }
            return 0;
        };
        int n = static_cast<int>(std::floor(x * 1 * 90));
        return red(n) * 0x10000 + red(n+30) * 0x100 + red(n+60) * 0x1;
    };
    auto darken = [](int x)->int
    {
        int r = ((x & 0xff0000) * 49 / 50) & 0xff0000;
        int g = ((x & 0xff00) * 49 / 50) & 0xff00;
        int b = ((x & 0xff) * 49 / 50) & 0xff;
        return r + g + b;
    };
    std::uniform_real_distribution<double> dist(0, 1);
    for (int ix = 0; ix < N; ++ix) {
        Clear();
        if (ix % 20 == 0 && 20 < N - ix) {
            double cx = x;
            double cy = y;
            double cz = z;
            for (int i = 0; i < PS; ++i) {
                vs.push_back(sphere_face(rng));
                poss.push_back({{ cx, cy, cz }, rgb(vs.back().len())});
            }
        }
        for (size_t i = 0; i < poss.size(); ++i) {
            auto & p = poss[i];
            auto & v = vs[i];
            if (can_show(p.p)) {
                SetLed(static_cast<int>(p.p.x), static_cast<int>(p.p.y), static_cast<int>(p.p.z), p.color);
            }
            p.p += v;
            p.color = darken(p.color);
        }
        Show();
        Wait(1);
    }
}