#include "kitti_helpers.hpp"

#include <cstdlib>
#include <fstream>
#include <iostream>
#include <string>
#include <vector>

#include <boost/algorithm/string.hpp>

struct PathCompare
{
    bool operator()(const boost::filesystem::path& a, const boost::filesystem::path& b)
    {
        return a.string() < b.string();
    }
};

Eigen::Matrix4f computePoseFromOxts(const KittiOxts_t& oxts, const float scale)
{
    // Earth radius (approx.) in meters.
    static const float earthRadius = 6378137.0f;

    // Compute the rotation transform.
    auto rz = Eigen::Affine3f(Eigen::AngleAxisf(oxts.yaw, Eigen::Vector3f::UnitZ()));
    auto h = rz.matrix();

    // Add in the translation.
    h(0,3) = scale * oxts.lon * kPi * earthRadius / 180.0f;
    h(1,3) = scale * earthRadius * std::log(std::tan(((90.0f + oxts.lat) * kPi) / 360.0f));

    return h;
}

std::vector<boost::filesystem::path> getDirectoryContents(
    const boost::filesystem::path& kPath,
    bool findDirs,
    const std::string& fileExt)
{
    std::vector<boost::filesystem::path> dirContents;
    for (auto& item : boost::filesystem::directory_iterator(kPath))
    {
        const boost::filesystem::path& p = item.path();
        if (findDirs && boost::filesystem::is_directory(p))
        {
            dirContents.push_back(p);
        }
        else if (!findDirs && boost::filesystem::is_regular_file(p)
            && p.string().find(fileExt) != std::string::npos)
        {
            dirContents.push_back(p);
        }
    }
    std::sort(dirContents.begin(), dirContents.end(), PathCompare());
    return dirContents;
}

KittiOxts_t readOxtsData(const std::string& kOxtsPath)
{
    // Read the OXTS data from disk.
    std::fstream oxtsfile(kOxtsPath.c_str(), std::ios::in);
    if (!oxtsfile.good())
    {
        std::cerr << "Cannot open OXTS file: " << kOxtsPath << std::endl;
        exit(EXIT_FAILURE);
    }
    oxtsfile.seekg(0, std::ios::beg);
    std::string line;
    std::getline(oxtsfile, line);

    // Create the Kitti OXTS struct.
    std::vector<std::string> tokens;
    boost::split(tokens, line, boost::is_any_of(" "));

    // return oxts;
    if (tokens.size() != 30)
    {
        std::cerr << "There should be 30 numbers from file '" << kOxtsPath << "'. Found ";
        std::cerr << tokens.size() << ".\n";
        exit(EXIT_FAILURE);
    }
    return KittiOxts_t(tokens);
}

void readVelodyneBin(
    const std::string& kBinPath,
    pcl::PointCloud<pcl::PointXYZI>& cloud)
{
    // Load the point cloud binary file.
    std::fstream binfile(kBinPath.c_str(), std::ios::in | std::ios::binary);
    if (!binfile.good())
    {
        std::cerr << "Cannot load binary file: " << kBinPath << std::endl;
        exit(EXIT_FAILURE);
    }

    // Iteratively load all the points from the binary file.
    binfile.seekg(0, std::ios::beg);
    for (std::size_t i = 0; binfile.good() && !binfile.eof(); i++)
    {
        pcl::PointXYZI pt;
        binfile.read((char*) &pt.x, 3 * sizeof(float));
        binfile.read((char*) &pt.intensity, sizeof(float));
        cloud.push_back(pt);
    }
    binfile.close();
}


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

/*
 Copyright (C) 2008 Ferdinando Ametrano
 Copyright (C) 2007 Chris Kenyon
 Copyright (C) 2007 StatPro Italia srl

 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.
*/

/*! \file bootstraperror.hpp
    \brief boostrap error.
*/

#ifndef quantlib_bootstrap_error_hpp
#define quantlib_bootstrap_error_hpp

#include <ql/types.hpp>
#include <boost/shared_ptr.hpp>

namespace QuantLib {

    //! bootstrap error
    template <class Curve>
    class BootstrapError {
        typedef typename Curve::traits_type Traits;
      public:
        BootstrapError(
                 const Curve* curve,
                 const boost::shared_ptr<typename Traits::helper>& instrument,
                 Size segment);
        Real operator()(Rate guess) const;
        const boost::shared_ptr<typename Traits::helper>& helper() {
            return helper_;
        }
      private:
        const Curve* curve_;
        const boost::shared_ptr<typename Traits::helper> helper_;
        const Size segment_;
    };

    // template definitions

    template <class Curve>
    BootstrapError<Curve>::BootstrapError(
                     const Curve* curve,
                     const boost::shared_ptr<typename Traits::helper>& helper,
                     Size segment)
    : curve_(curve), helper_(helper), segment_(segment) {}

    #ifndef __DOXYGEN__
    template <class Curve>
    Real BootstrapError<Curve>::operator()(Real guess) const {
        Traits::updateGuess(curve_->data_, guess, segment_);
        curve_->interpolation_.update();
        return helper_->quoteError();
    }
    #endif

}

#endif


//          Copyright Oliver Kowalke 2009.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <iostream>
#include <cstdlib>
#include <stdexcept>
#include <vector>

#include <boost/assert.hpp>
#include <boost/bind.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>

#include "boost/task.hpp"

namespace pt = boost::posix_time;
namespace tsk = boost::tasks;

typedef tsk::static_pool< tsk::unbounded_fifo > pool_type;

int serial_fib( int n)
{
	if( n < 2)
		return n;
	else
		return serial_fib( n - 1) + serial_fib( n - 2);
}

int parallel_fib( int n, int cutof)
{
	if ( n < cutof)
	{
		return serial_fib( n);
	}
	else
	{
		BOOST_ASSERT( boost::this_task::runs_in_pool() );
		tsk::handle< int > h1(
			tsk::async(
				tsk::make_task(
					parallel_fib,
					n - 1,
					cutof),
				tsk::as_sub_task() ) );
		tsk::handle< int > h2(
			tsk::async(
				tsk::make_task(
					parallel_fib,
					n - 2,
					cutof),
				tsk::as_sub_task() ) );
		return h1.get() + h2.get();
	}
}

int main( int argc, char *argv[])
{
# if defined(BOOST_HAS_PROCESSOR_BINDINGS)
	try
	{
		pool_type pool( pool_type::bind_to_processors() );

		std::vector< tsk::handle< int > > results;
		results.reserve( 10);

		pt::ptime start = pt::microsec_clock::universal_time();

//	for ( int i = 0; i < 15; ++i)
//		results.push_back(
//			tsk::async(
//				tsk::make_task(
//					parallel_fib,
//					10,
//					5),
//				pool) );
//

		results.push_back(
			tsk::async(
				tsk::make_task(
					parallel_fib,
					25,
					5),
				pool) );

		tsk::waitfor_all( results.begin(), results.end() );

		pt::ptime stop = pt::microsec_clock::universal_time();

		pt::time_duration elapsed = stop - start;
		std::cout << "total microseconds == " << elapsed.total_microseconds() << std::endl;
		std::cout << "total milliseconds == " << elapsed.total_milliseconds() << std::endl;

		int k = 0;
		std::vector< tsk::handle< int > >::iterator e( results.end() );
		for (
			  std::vector< tsk::handle< int > >::iterator i( results.begin() );
			  i != e;
			  ++i)
			std::cout << "fibonacci(" << k++ << ") == " << i->get() << std::endl;

		return EXIT_SUCCESS;
	}
	catch ( std::exception const& e)
	{ std::cerr << "exception: " << e.what() << std::endl; }
	catch ( ... )
	{ std::cerr << "unhandled" << std::endl; }

# endif

	return EXIT_FAILURE;
}


#include <boost/date_time/gregorian/gregorian.hpp>
#include <iostream>

using namespace boost;

int main()
{
  gregorian::date d{2014, 5, 12};
  gregorian::day_iterator it{d};
  std::cout << *++it << '\n';
  std::cout << date_time::next_weekday(*it,
    gregorian::greg_weekday(date_time::Friday)) << '\n';
}


#ifndef LATTICE_SQUARELATTICE_HPP
#define LATTICE_SQUARELATTICE_HPP

#include <string>
#include <vector>
#include <boost/lexical_cast.hpp>

namespace lattice {

class SquareLattice {
public:
  static std::string name() { return "SquareLattice";}
  static int set_num_particles(int Ns){return Ns*Ns;}
  SquareLattice(int LatticeSize) : dim_(LatticeSize) {}
  void create_table(std::vector<std::vector < int > >& table){
  //for bulk
    for (int i = 0; i < dim_; ++i){
      for (int j = 0; j < dim_; ++j){
          int l = numberize(i,j);
          int temp = 0;
          for(int d = -1; d < 2; d =d+2){
            table[l][temp] = numberize(i+d,j);
            temp += 1;
            table[l][temp] = numberize(i,j+d);
            temp += 1;
          }
        }
      }
    }

  int numberize(const int i, const int j){
    int i_t = (i + dim_);
    int j_t = (j + dim_);
    i_t %= dim_ ;
    j_t %= dim_ ;
    return i_t + j_t * dim_;
  }

  int latticize(int d, const int l){ //d is direction(0 ~ ) ex. 0 denotes x, 1 denotes y,~
    int j = l/dim_ ;
    int i = (l - dim_ *j);
    if(d==0) return i;
    else return j;
  }

  int target_number(const int i, const int observe){
    int sight_x = 0;
    int sight_y = 0;
    sight_x = latticize(0,observe);
    sight_y = latticize(1,observe);
    return  numberize(sight_x + i, sight_y);
  }

  int number_adjacent() {return 4;}

private:
  int dim_;
};//Regular cube end

} //end namespace

#endif //LATTICE_SQUARELATTICE_HPP


#ifndef stddecimal_general_decimal_arithmetic_test_runner_test_file_hpp
#define stddecimal_general_decimal_arithmetic_test_runner_test_file_hpp

#include <string>
#include <vector>
#include <fstream>
#include "test_context.hpp"
#include "test_line.hpp"
#include "test_results.hpp"
#include "tests.hpp"
#include <iostream>
#include <boost/algorithm/string/trim_all.hpp>
#include <boost/algorithm/string/split.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/regex.hpp>
#include <decimal_numeric_limits.hpp>

namespace gda
{

template<int Bits>
class test_file
{
public:

    test_file(const std::string& filename, test_results& results)
    :   m_is(filename),
        m_results(results)
    {
        if (!m_is) {
            throw std::runtime_error("Unable to open test file '" + filename + "'");
        }
    }

    void process()
    {
        test_context context;

        while (m_is)
        {
            std::string text;

            if (!std::getline(m_is, text)) {
                break;
            }

            try
            {
                test_line line(text);

                if (line.is_blank()) {
                    continue;
                }

                if (line.is_comment()) {
                    continue;
                }

                if (line.is_directive()) {
                    context.apply_directive(line.keyword(), line.value());
                    continue;
                }

                if (line.is_test()) {

                    if (context.clamp()) {
                        m_results.record(result::skip);
                        continue;
                    }

                    test test;
                    test.id = line.id();
                    test.operation = line.operation();
                    test.operands = line.operands();
                    test.expected_result = line.expected_result();
                    test.expected_conditions = line.expected_conditions();

                    using traits = operation_traits<Bits>;
                    using limits = std::decimal::numeric_limits<typename traits::decimal_type>;

                    // if (context.precision() > limits::digits) {
                    //     std::cerr << "skipping due to precision " << *context.precision() << " v " << limits::digits << "\n";
                    //     m_results.record(result::skip);
                    //     continue;
                    // }

                    try {
                        context.apply_rounding();
                        // std::decimal::clear_exceptions(std::decimal::FE_DEC_ALL_EXCEPT);
                        std::decimal::set_exceptions(std::decimal::FE_DEC_DIVBYZERO |
                                                    std::decimal::FE_DEC_INEXACT |
                                                    std::decimal::FE_DEC_INVALID |
                                                    std::decimal::FE_DEC_OVERFLOW |
                                                    std::decimal::FE_DEC_UNDERFLOW);
                        m_results.record(process_test(test));
                    }
                    catch (std::decimal::exception& ex) {
                        if (ex.flags() != test.expected_conditions) {
                            report_failure(test, ex.flags());
                            m_results.record(result::fail);
                        }
                        else {
                            m_results.record(result::pass);
                        }
                    }
                    catch (std::exception& ex) {
                        std::cerr << "GENERIC EXCEPTION " << test.id << " " << ex.what() << std::endl;
                        m_results.record(result::skip);
                    }

                    continue;
                }

                std::cerr << "failed to process line: " << text << '\n';
            }
            catch (std::exception& ex)
            {
                std::cerr << "failed to parse line: " + text << '\n';
            }
        }
    }

private:

    result process_test(const test& test)
    {
        using traits = operation_traits<Bits>;

        if (test.operation == "add") {
            return add_test<typename traits::decimal_type>::run(test);
        }

        if (test.operation == "subtract") {
            return subtract_test<typename traits::decimal_type>::run(test);
        }

        if (test.operation == "divide") {
            return divide_test<typename traits::decimal_type>::run(test);
        }

        if (test.operation == "multiply") {
            return multiply_test<typename traits::decimal_type>::run(test);
        }

        if (test.operation == "squareroot") {
            return square_root_test<typename traits::decimal_type>::run(test);
        }

        if (test.operation == "abs") {
            return abs_test<typename traits::decimal_type>::run(test);
        }

        if (test.operation == "plus") {
            return plus_test<typename traits::decimal_type>::run(test);
        }

        if (test.operation == "minus") {
            return minus_test<typename traits::decimal_type>::run(test);
        }

        if (test.operation == "power") {
            return power_test<typename traits::decimal_type>::run(test);
        }

        if (test.operation == "quantize") {
            return quantize_test<typename traits::decimal_type>::run(test);
        }

        if (test.operation == "log10") {
            return log10_test<typename traits::decimal_type>::run(test);
        }

        if (test.operation == "fma") {
            return fma_test<typename traits::decimal_type>::run(test);
        }

        if (test.operation == "min") {
            return min_test<typename traits::decimal_type>::run(test);
        }

        if (test.operation == "max") {
            return max_test<typename traits::decimal_type>::run(test);
        }

        if (test.operation == "remainder") {
            return remainder_test<typename traits::decimal_type>::run(test);
        }

        if (test.operation == "exp") {
            return exp_test<typename traits::decimal_type>::run(test);
        }

        if (test.operation == "ln") {
            return log_test<typename traits::decimal_type>::run(test);
        }

        if (test.operation == "samequantum") {
            return samequantum_test<typename traits::decimal_type>::run(test);
        }

        if (test.operation == "compare") {
            return compare_test<typename traits::decimal_type>::run(test);
        }

        // comparetotal0.decTest
        // randoms0.decTest
        // rescale0.decTest
        // inexact0.decTest
        // reduce0.decTest
        // testall0.decTest
        // base0.decTest
        // divideint0.decTest
        // tointegral0.decTest
        // remaindernear0.decTest
        // trim0.decTest

        std::cerr << "skipping: " << test.id << '\n';

        return result::skip;
    }

    std::ifstream m_is;
    test_results& m_results;

};

} // namespace gda

#endif


#include <stdio.h>
#include <iostream>
#include <fstream>
#include <string>
#include <boost/tokenizer.hpp>
#include <algorithm>

using std::vector;
using std::string;
using std::cout;

boost::char_separator<char> sep(" ");

vector<string> Tokenize(string game_line) {
  boost::tokenizer<boost::char_separator<char> > tokens(game_line, sep);
  vector<string> string_tokens;
  for (auto single_word : tokens) {
    // std::cout << single_word << ",";
    string_tokens.push_back(single_word);
  }
  // std::cout << "\n";
  return string_tokens;
}

void SplitPlayers(const vector<string> &card_vec, vector<string> *player_1,
    vector<string> *player_2) {
  assert(card_vec.size() == 10);
  for (size_t i = 0; i < 5; ++i) {
    player_1->push_back(card_vec[i]);
  }

  for (size_t i = 5; i < 10; ++i) {
    player_2->push_back(card_vec[i]);
  }
}

enum Suits {
  CLUBS    = 0,
  DIAMONDS,
  HEARTS,
  SPADES
};

enum HandRank {
  HIGH_CARD = 0,
  ONE_PAIR = 1,
  TWO_PAIRS = 2,
  THREE_KIND = 3,
  STRAIGHT = 4,
  FLUSH = 5,
  FULL_HOUSE = 6,
  FOUR_KIND = 7,
  STRAIGHT_FLUSH = 8,
  ROYAL_FLUSH = 9
};

Suits ConvertLetterToSuit(char suit_letter) {
  switch (suit_letter) {
    case 'C':
      return CLUBS;
    case 'D':
      return DIAMONDS;
    case 'H':
      return HEARTS;
    case 'S':
      return SPADES;
    default:
      std::cout << "error: unknown letter indicating suit\n";
  }
}

HandRank CheckKind(const vector<string> &player_cards) {
  // 2, 3, 4, 5, 6, 7, 8, 9, 10, Jack, Queen, King, Ace
  vector<int> card_count(14, 0);
  for (int i = 0; i < 5; i++) {
    int val = 0;
    if (player_cards[i][0] >= '2' && player_cards[i][0] <= '9') {
      val = player_cards[i][0] - '0';
    } else {
#if 1
      switch (player_cards[i][0]) {
        case 'T': {
          val = 10;
          break;
        }
        case 'J': {
          val = 11;
          break;
        }
        case 'Q': {
          val = 12;
          break;
        }
        case 'K': {
          val = 13;
          break;
        }
        case 'A': {
          val = 1;
          break;
        }
        default: {
          std::cout << "Error: invalid card " << player_cards[i] << "\n";
          val = 0;
        }
      }
#endif
    }

    card_count[val]++;
  }

  int pair_count = 0;
  int three_count = 0;
  for (int i = 1; i < 14; i++) {
    if (card_count[i] == 2) {
      pair_count++;
    } else if (card_count[i] == 3) {
      // 3 of a kind
      three_count++;
      // the three would have also counted as a pair
      pair_count--;
    } else if (card_count[i] == 4) {
      // 4 of a kind
      return FOUR_KIND;
    }
  }
  if (pair_count == 1) {
    if (three_count == 1) {
      return FULL_HOUSE;
    } else {
      return ONE_PAIR;
    }
  } else if (pair_count == 2) {
     return TWO_PAIRS;
  } else if (three_count == 1) {
    return THREE_KIND;
  }
  return HIGH_CARD;
}

vector<int> GetValues(const vector<string> &player_cards) {
  vector<int> values;
  assert(player_cards.size() == 5);

  int val = 0;
  for (int i = 0; i < 5; ++i) {
    if (player_cards[i][0] >= '2' && player_cards[i][0] <= '9') {
      val = player_cards[i][0] - '0';
    } else {
#if 1
      switch (player_cards[i][0]) {
        case 'T': {
          val = 10;
          break;
        }
        case 'J': {
          val = 11;
          break;
        }
        case 'Q': {
          val = 12;
          break;
        }
        case 'K': {
          val = 13;
          break;
        }
        case 'A': {
          val = 1;
          break;
        }
        default: {
          std::cout << "Error: invalid card " << player_cards[i] << "\n";
          val = 0;
        }
      }
#endif
    }
    values.push_back(val);
  }
  return values;
}

vector<int> GetValuesHighAce(const vector<string> &player_cards) {
  vector<int> values;
  assert(player_cards.size() == 5);

  int val = 0;
  for (int i = 0; i < 5; ++i) {
    if (player_cards[i][0] >= '2' && player_cards[i][0] <= '9') {
      val = player_cards[i][0] - '0';
    } else {
#if 1
      switch (player_cards[i][0]) {
        case 'T': {
          val = 10;
          break;
        }
        case 'J': {
          val = 11;
          break;
        }
        case 'Q': {
          val = 12;
          break;
        }
        case 'K': {
          val = 13;
          break;
        }
        case 'A': {
          val = 14;
          break;
        }
        default: {
          std::cout << "Error: invalid card " << player_cards[i] << "\n";
          val = 0;
        }
      }
#endif
    }
    values.push_back(val);
  }
  return values;
}
HandRank CheckStraight(const vector<string> &player_cards) {

  vector<int> values = GetValues(player_cards);
#if 0
  for (auto single_card : player_cards) {
  int val1 = 0;
#if 1
    if (single_card[0] >= '2' && single_card[0] <= '9') {
      // val = single_card[0] - '0';
      val1 = 0;
      std::cout << "numbers\n";
    } else {
      val1 = 0;
    }
#endif
    values.push_back(val1+1);
  }
#endif

  assert(values.size() == 5);
  std::sort(values.begin(), values.end());

  bool is_consecutive = true;
  for (int i = 0; i < values.size() - 1; ++i) {
    int diff = values[i+1] - values[i];
    if (diff != 1) {
      is_consecutive = false;
      break;
    }
  }
  if (is_consecutive == true) {
    return STRAIGHT;
  }
  if (values[0] == 1) {
    // need to check for high straight for ace
    values[0] = 14;
    std::sort(values.begin(), values.end());
    for (int i = 0; i < values.size() - 1; ++i) {
      int diff = values[i+1] - values[i];
      if (diff != 1) {
        return HIGH_CARD;
      }
    }
    return STRAIGHT;
  }
  return HIGH_CARD;
}

HandRank CheckFlush(const vector<string> &player_cards) {
  int suit_count[4] = {0, 0, 0, 0};
  for (auto single_card : player_cards) {
    int suit_enum = ConvertLetterToSuit(single_card[1]);
    suit_count[suit_enum]++;
  }

  for (int i = 0; i < 4; ++i) {
    if (suit_count[i] == 5) {
      return FLUSH;
    }
  }
  return HIGH_CARD;
}

HandRank Process(vector<string> player) {
  HandRank flush_out = CheckFlush(player);
  HandRank kind_out = CheckKind(player);
  HandRank straight_out = CheckStraight(player);

  if (flush_out == HIGH_CARD && kind_out == HIGH_CARD && straight_out == HIGH_CARD) {
    return HIGH_CARD;
  } else if (flush_out == FLUSH && straight_out == STRAIGHT) {
    std::vector<int> values = GetValues(player);
    std::sort(values.begin(), values.end());
    if (values[0] == 1 && values[1] == 10 && values[2] == 11 &&
        values[3] == 12 && values[4] == 13) {
      return ROYAL_FLUSH;
    }
    return STRAIGHT_FLUSH;
  } else if (flush_out == FLUSH) {
    return FLUSH;
  } else if (straight_out == STRAIGHT) {
    return STRAIGHT;
  } else {
    return kind_out;
  }
}

int DecideWinner(vector<string> card_vec) {
  // First five is player 1
  //
  //
  vector<string> player_1, player_2;
  SplitPlayers(card_vec, &player_1, &player_2);
  assert(player_1.size() == 5);
  assert(player_2.size() == 5);
  HandRank score_1 = Process(player_1);
  HandRank score_2 = Process(player_2);
  if (score_1 > score_2) {
    return 1;
  } else if (score_1 < score_2) {
    return 2;
  } else {
    vector<int> values_1 = GetValuesHighAce(player_1);
    vector<int> values_2 = GetValuesHighAce(player_2);
    std::sort(values_1.begin(), values_1.end());
    std::sort(values_2.begin(), values_2.end());
    // rank with the highest card wins
    switch (score_1) {
      case STRAIGHT_FLUSH: {
        if (values_1[0] > values_2[0]) {
          return 1;
        } else if (values_1[0] < values_2[0]) {
          return 2;
        } else {
          cout << "confused -- straight_flush\n";
        }
        break;
      }
      case FLUSH: {
        for (int i = 4; i >= 0; --i) {
          if (values_1[i] > values_2[i]) {
            return 1;
          } else if (values_1[i] < values_2[i]) {
            return 2;
          }
        }
        break;
      }
      case STRAIGHT: {
        if (values_1[0] > values_2[0]) {
          return 1;
        } else if (values_1[0] < values_2[0]) {
          return 2;
        } else {
          cout << "confused -- straight\n";
        }
        break;
      }
      case FULL_HOUSE: {
        if (values_1[2] > values_2[2]) {
          return 1;
        } else if (values_1[2] < values_2[2]) {
          return 2;
        } else {
          cout << "confused -- full_house\n";
        }
        break;
      }
      case FOUR_KIND: {
        // when sorted the index 2 has to be definitely in the 4 of a kind
        if (values_1[2] > values_2[2]) {
          return 1;
        } else if (values_1[2] < values_2[2]) {
          return 2;
        } else {
          cout << "confused -- four_kind\n";
        }
        break;
      }
      case THREE_KIND: {
        // when sorted the index 2 has to be definitely in the 4 of a kind
        if (values_1[2] > values_2[2]) {
          return 1;
        } else if (values_1[2] < values_2[2]) {
          return 2;
        } else {
          cout << "confused -- three kind\n";
        }
        break;
      }
      case HIGH_CARD: {
        for (int i = 4; i >= 0; --i) {
          if (values_1[i] > values_2[i]) {
            return 1;
          } else if (values_1[i] < values_2[i]) {
            return 2;
          }
        }
        cout << "confused -- high card\n";
        break;
      }
      case ONE_PAIR: {
 #if 1
        // find pair 1
        int val_1 = 0;
        int val_2 = 0;
        for (int i = 0; i < 4; ++i) {
          if (values_1[i+1] == values_1[i]) {
            val_1 = values_1[i+1];
          }
          if (values_2[i+1] == values_2[i]) {
            val_2 = values_2[i+1];
          }
        }
        if (val_1 > val_2) {
          return 1;
        } else if (val_1 < val_2) {
          return 2;
        } else {
          int i = 4, j = 4;
          while(i>=0 && j >= 0) {
            if (values_1[i] == val_1) {
              i -= 2;
            }
            if (values_2[j] == val_2) {
              j-=2;
            }
            if (j < 0 || i < 0) {
              cout << "confused -- one pair\n";
              break;
            }
            if (values_1[i] > values_2[j]) {
              return 1;
            } else if (values_1[i] < values_2[j]) {
              return 2;
            }
          }
        }
#endif
        break;
      }
      case TWO_PAIRS: {
        cout << "confused -- two pairs\n";
        break;
      }
      case ROYAL_FLUSH: {
        cout << "confused -- royal flush\n";
        break;
      }
      default:
        cout << "confused\n";
        return 2;
    }  // end of switch
  }
  std::cout << "\n------------------\n";
  return 2;
}

int main() {
  std::ifstream is("p054_poker.txt");
  // std::ifstream is("poker.txt");
  if (!is) {
    std::cout << "error opening file \n";
  }
  std::string game_line;
  int player1_win_counter = 0;

  while (std::getline(std::cin, game_line)) {
    // std::cout << game_line << std::endl;
    vector<string> card_vec = Tokenize(game_line);
    int player_num = DecideWinner(card_vec);
    if (player_num == 1) {
      player1_win_counter++;
    }
  }

  std::cout << "Number of wins by player1 is " << player1_win_counter << "\n";

  return 0;
}


/**
 * \ file RLSFilter.cpp
 */

#include <ATK/Adaptive/RLSFilter.h>

#include <ATK/Core/InPointerFilter.h>
#include <ATK/Core/OutPointerFilter.h>
#include <ATK/Core/Utilities.h>

#include <ATK/Mock/SimpleSinusGeneratorFilter.h>
#include <ATK/Mock/TriangleCheckerFilter.h>

#include <ATK/Tools/SumFilter.h>

#include <Eigen/Core>

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_NO_MAIN
#include <boost/test/unit_test.hpp>

#include <boost/math/constants/constants.hpp>

constexpr gsl::index PROCESSSIZE = 1024 * 64;

BOOST_AUTO_TEST_CASE(RLSFilter_size_negative_test)
{
  ATK::RLSFilter<double> filter(100);
  BOOST_CHECK_THROW(filter.set_size(0), ATK::RuntimeError);
}

BOOST_AUTO_TEST_CASE(RLSFilter_size_set_test)
{
  ATK::RLSFilter<double> filter(100);
  filter.set_size(10);
  BOOST_CHECK_EQUAL(filter.get_size(), 10);
}

BOOST_AUTO_TEST_CASE(RLSFilter_memory_negative_test)
{
  ATK::RLSFilter<double> filter(100);
  BOOST_CHECK_THROW(filter.set_memory(0), ATK::RuntimeError);
}

BOOST_AUTO_TEST_CASE(RLSFilter_memory_test)
{
  ATK::RLSFilter<double> filter(100);
  filter.set_memory(0.5);
  BOOST_CHECK_EQUAL(filter.get_memory(), 0.5);
}

BOOST_AUTO_TEST_CASE( RLSFilter_memory_positive1_test )
{
  ATK::RLSFilter<double> filter(100);
  BOOST_CHECK_THROW(filter.set_memory(1), ATK::RuntimeError);
}

BOOST_AUTO_TEST_CASE(RLSFilter_learning_set_test)
{
  ATK::RLSFilter<double> filter(100);
  BOOST_CHECK_EQUAL(filter.get_learning(), true);
  filter.set_learning(false);
  BOOST_CHECK_EQUAL(filter.get_learning(), false);
}

BOOST_AUTO_TEST_CASE( RLSFilter_memory_99_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(48000);
  generator.set_amplitude(1);
  generator.set_frequency(1000);

  ATK::RLSFilter<double> filter(10);
  filter.set_input_sampling_rate(48000);
  filter.set_output_sampling_rate(48000);
  filter.set_memory(.99);
  filter.set_learning(false);

  ATK::TriangleCheckerFilter<double> checker;
  checker.set_input_sampling_rate(48000);
  checker.set_amplitude(0);
  checker.set_frequency(1000);

  filter.set_input_port(0, &generator, 0);
  checker.set_input_port(0, &filter, 0);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( RLSFilter_constant_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(48000);
  generator.set_amplitude(1);
  generator.set_frequency(1000);

  ATK::SimpleSinusGeneratorFilter<double> reference;
  reference.set_output_sampling_rate(48000);
  reference.set_amplitude(-1);
  reference.set_frequency(1000);

  ATK::RLSFilter<double> filter(1);
  filter.set_input_sampling_rate(48000);
  filter.set_output_sampling_rate(48000);
  filter.set_memory(.99);
  filter.set_learning(false);
  Eigen::Matrix<double, Eigen::Dynamic, 1> w(1);
  w << 1;
  filter.set_w(w.data());

  ATK::SumFilter<double> sum;
  sum.set_input_sampling_rate(48000);
  sum.set_output_sampling_rate(48000);

  ATK::TriangleCheckerFilter<double> checker;
  checker.set_input_sampling_rate(48000);
  checker.set_amplitude(0);
  checker.set_frequency(1000);

  filter.set_input_port(0, &generator, 0);
  sum.set_input_port(0, &reference, 0);
  sum.set_input_port(1, &filter, 0);
  checker.set_input_port(0, &sum, 0);

  filter.process(1); // to start everything, the filter is supposed to be one sample late
  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( RLSFilter_learning_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(48000);
  generator.set_amplitude(1);
  generator.set_frequency(1000);

  ATK::SimpleSinusGeneratorFilter<double> reference;
  reference.set_output_sampling_rate(48000);
  reference.set_amplitude(-1);
  reference.set_frequency(1000);

  ATK::RLSFilter<double> filter(10);
  filter.set_input_sampling_rate(48000);
  filter.set_output_sampling_rate(48000);
  filter.set_memory(0.9999);
  filter.set_learning(true);

  ATK::SumFilter<double> sum;
  sum.set_input_sampling_rate(48000);
  sum.set_output_sampling_rate(48000);

  std::array<double, PROCESSSIZE> outdata;
  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);

  filter.set_input_port(0, &generator, 0);
  sum.set_input_port(0, &reference, 0);
  sum.set_input_port(1, &filter, 0);
  output.set_input_port(0, &sum, 0);

  output.process(PROCESSSIZE);

  for(std::int64_t i = 100; i < PROCESSSIZE; ++i) // let the RLS filter start learning first
  {
    BOOST_REQUIRE_SMALL(std::abs(outdata[i]), 0.1);
  }
}

BOOST_AUTO_TEST_CASE(RLSFilter_learning_training_test)
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(48000);
  generator.set_amplitude(1);
  generator.set_frequency(1000);

  ATK::SimpleSinusGeneratorFilter<double> reference;
  reference.set_output_sampling_rate(48000);
  reference.set_amplitude(-1);
  reference.set_frequency(1000);

  ATK::RLSFilter<double> filter(10);
  filter.set_input_sampling_rate(48000);
  filter.set_output_sampling_rate(48000);
  filter.set_memory(0.9999);
  filter.set_learning(true);

  ATK::SumFilter<double> sum;
  sum.set_input_sampling_rate(48000);
  sum.set_output_sampling_rate(48000);

  std::array<double, PROCESSSIZE> outdata;
  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);

  filter.set_input_port(0, &generator, 0);
  sum.set_input_port(0, &reference, 0);
  sum.set_input_port(1, &filter, 0);
  output.set_input_port(0, &sum, 0);

  output.process(1000);
  filter.set_learning(false);
  output.process(PROCESSSIZE - 1000);

  for (std::int64_t i = 100; i < PROCESSSIZE; ++i) // let the RLS filter start learning first
  {
    BOOST_REQUIRE_SMALL(std::abs(outdata[i]), 0.1);
  }

}


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

#include <type_traits>
#include "test.hpp"
#include <nil/crypto3/multiprecision/mpc.hpp>
#include <boost/math/constants/constants.hpp>

using nil::crypto3::multiprecision::mpc_complex_100;

template<class Complex>
void test_overloads() {
    typedef typename Complex::value_type Real;
    Complex ya = {5.2, 7.4};
    Complex yb = {8.2, 7.3};
    Real h = 0.0001;
    auto I0 = (ya + yb) * h;
    Complex I1 = I0 / 2 + yb * h;

    // I1 = I0;  // not supposed to work.

    Complex z {2, 3};
    typename Complex::value_type theta = 0.2;
    int n = 2;
    using std::sin;
    Complex arg = z * sin(theta) - n * theta;

    using std::exp;
    Real v = 0.2;
    Real cotv = 7.8;
    Real cscv = 8.2;
    Complex den = z + v * cscv * exp(-v * cotv);

    nil::crypto3::multiprecision::number<nil::crypto3::multiprecision::backends::mpc_complex_backend<100>> a = 2;
    nil::crypto3::multiprecision::number<nil::crypto3::multiprecision::backends::mpc_complex_backend<100>> b = 3;
    /*
   if (a <= b) {
     b = a;
   }*/
}

template<class F, class Real>
typename std::result_of_t<F(Real)> some_functional(F f, Real a, Real b) {
    if (a <= -boost::math::tools::max_value<Real>()) {
        return f(a);
    }

    return f(b);
}

template<class Complex>
void test_functional() {
    typedef typename Complex::value_type Real;
    auto f = [](Real x) -> Complex {
        Complex z(x, 3);
        return z;
    };
    Real a = 0;
    Real b = 1;
    Complex result = some_functional(f, a, b);
}

int main() {
    test_overloads<mpc_complex_100>();
    test_functional<mpc_complex_100>();
}


// ----------------------------------------------------------------------------
// -                        Open3D: www.open3d.org                            -
// ----------------------------------------------------------------------------
// The MIT License (MIT)
//
// Copyright (c) 2018 www.open3d.org
//
// 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 "CorrespondenceChecker.h"

#include <Eigen/Dense>
#include <Open3D/Utility/Console.h>
#include <Open3D/Geometry/PointCloud.h>

namespace open3d {
namespace registration {

bool CorrespondenceCheckerBasedOnEdgeLength::Check(
        const geometry::PointCloud &source,
        const geometry::PointCloud &target,
        const CorrespondenceSet &corres,
        const Eigen::Matrix4d & /*transformation*/) const {
    for (auto i = 0; i < corres.size(); i++) {
        for (auto j = i + 1; j < corres.size(); j++) {
            // check edge ij
            double dis_source = (source.points_[corres[i](0)] -
                                 source.points_[corres[j](0)])
                                        .norm();
            double dis_target = (target.points_[corres[i](1)] -
                                 target.points_[corres[j](1)])
                                        .norm();
            if (dis_source < dis_target * similarity_threshold_ ||
                dis_target < dis_source * similarity_threshold_) {
                return false;
            }
        }
    }
    return true;
}

bool CorrespondenceCheckerBasedOnDistance::Check(
        const geometry::PointCloud &source,
        const geometry::PointCloud &target,
        const CorrespondenceSet &corres,
        const Eigen::Matrix4d &transformation) const {
    for (const auto &c : corres) {
        const auto &pt = source.points_[c(0)];
        Eigen::Vector3d pt_trans =
                (transformation * Eigen::Vector4d(pt(0), pt(1), pt(2), 1.0))
                        .block<3, 1>(0, 0);
        if ((target.points_[c(1)] - pt_trans).norm() > distance_threshold_) {
            return false;
        }
    }
    return true;
}

bool CorrespondenceCheckerBasedOnNormal::Check(
        const geometry::PointCloud &source,
        const geometry::PointCloud &target,
        const CorrespondenceSet &corres,
        const Eigen::Matrix4d &transformation) const {
    if (source.HasNormals() == false || target.HasNormals() == false) {
        utility::PrintDebug(
                "[CorrespondenceCheckerBasedOnNormal::Check] Pointcloud has no "
                "normals.\n");
        return true;
    }
    double cos_normal_angle_threshold = std::cos(normal_angle_threshold_);
    for (const auto &c : corres) {
        const auto &normal = source.normals_[c(0)];
        Eigen::Vector3d normal_trans =
                (transformation *
                 Eigen::Vector4d(normal(0), normal(1), normal(2), 0.0))
                        .block<3, 1>(0, 0);
        if (target.normals_[c(1)].dot(normal_trans) <
            cos_normal_angle_threshold) {
            return false;
        }
    }
    return true;
}

}  // namespace registration
}  // namespace open3d


/////////////////////////////////////////////////////////////////////////////////////////
// distribution::survival::models::common::importance_sampling::proposals_manager.hpp  //
//                                                                           		   //
//  Copyright 2009 Erwann Rogard. 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)         		   //
/////////////////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_DISTRIBUTION_SURVIVAL_MODELS_IMPORTANCE_SAMPLING_PROPOSALS_MANAGER_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_SURVIVAL_MODELS_IMPORTANCE_SAMPLING_PROPOSALS_MANAGER_HPP_ER_2009
#include <boost/typeof/typeof.hpp>
#include <boost/fusion/include/make_map.hpp>
#include <boost/fusion/include/pair.hpp>
#include <boost/statistics/detail/traits/remove_cvref.hpp>
#include <boost/statistics/detail/distribution_common/meta/value.hpp>
#include <boost/statistics/detail/distribution_common/meta/random/distribution.hpp>
#include <boost/statistics/detail/distribution_common/meta/random/generator.hpp>

namespace boost {
namespace statistics{
namespace detail{
namespace distribution{
namespace survival {
namespace importance_sampling{

    template<typename D>
	class proposals_manager{
        typedef typename distribution::meta::value<D>::type val_;
        typedef distribution::meta::random_distribution<D> meta_random_;
        typedef typename meta_random_::type random_;
        typedef typename random_::result_type par_value1_;
		typedef typename statistics::detail::traits::remove_cvref<
        	par_value1_
        >::type par_value_;

		public:
        proposals_manager(const D& dist):dist_(dist){}
		proposals_manager(const proposals_manager& that):dist_(that.dist_){}

		// Tags
        struct tag{
			struct parameter{};
			struct log_pdf{};
        };

		typedef typename tag::parameter tag_par_;
		typedef typename tag::log_pdf tag_lpdf_;

    	typedef typename boost::fusion::map<
        	boost::fusion::pair<tag_par_,par_value_>,
            boost::fusion::pair<tag_lpdf_,val_>
    	> proposal_;

    	typedef std::vector<proposal_> proposals_type;

        // Generate posterior sample

		const D& distribution()const{ return this->dist_; }

		template<typename U,typename N>
		void refresh(U& urng,N n){
        	BOOST_AUTO(
            	gen,
                distribution::make_random_generator(urng,this->dist_)
            );
            this->proposals_.clear();
			this->proposals_.reserve(n);
            for(unsigned i = 0; i<n; i++)
            {
        		par_value_ p = gen();
            	val_ lpdf = log_unnormalized_pdf(this->dist_,p);
            	this->proposals_.push_back(
                	boost::fusion::make_map<tag_par_,tag_lpdf_>(p,lpdf)
            	);
            }

		}

        const proposals_type& operator()()const{ return this->proposals_; }

        private:
        proposals_type proposals_;
		D dist_;
	};

}// importance_sampling
}// survival
}// distribution
}// detail
}// statistics
}// boost

#endif


#include "itkImage.h"
#include "itkImageFileReader.h"
#include "itkImageRegionIteratorWithIndex.h"
#include "itkImageFileWriter.h"

#include "tkdCmdParser.h"

#include <boost/math/special_functions/erf.hpp>

/**
 * Fisher z' to p-value (Numerical Recipes, 746)
 */
class Fisher2P
{
	typedef double PixelType;
	typedef itk::Image< PixelType, 3 > ImageType;
	typedef itk::ImageFileReader< ImageType > ImageReaderType;
	typedef itk::ImageFileWriter< ImageType > ImageWriterType;
    typedef itk::ImageRegionIteratorWithIndex< ImageType > IteratorType;
    typedef itk::ImageRegionConstIteratorWithIndex< ImageType > ConstIteratorType;

	public:

    void Run( const std::string& inputFileName, unsigned int N )
	{
		ImageReaderType::Pointer reader = ImageReaderType::New();
		reader->SetFileName( inputFileName.c_str() );
		reader->Update();

		ImageType::Pointer image = reader->GetOutput();

        ImageType::Pointer negative = ImageType::New();
        negative->SetRegions( image->GetLargestPossibleRegion() );
        negative->SetSpacing( image->GetSpacing() );
        negative->SetOrigin( image->GetOrigin() );
        //negative->CopyInformation( image );
        negative->Allocate();
        negative->FillBuffer( 0 );

        ImageType::Pointer positive = ImageType::New();
        positive->SetRegions( image->GetLargestPossibleRegion() );
        positive->SetSpacing( image->GetSpacing() );
        positive->SetOrigin( image->GetOrigin() );
        positive->Allocate();
        positive->FillBuffer( 0 );

        ConstIteratorType it( image, image->GetLargestPossibleRegion() );
        it.GoToBegin();

        while( ! it.IsAtEnd() )
        {
            ImageType::IndexType index = it.GetIndex();
            PixelType z = image->GetPixel( index );
            PixelType p = P( z, N );

            std::cout << "P-value: " << p << std::endl;

            if( z < 0 )
                negative->SetPixel( index, p );
            else
                positive->SetPixel( index, p );

            ++it;
        }

        // Write
        std::string outputFileNameNegative = "neg.nii.gz";
        std::string outputFileNamePositive = "pos.nii.gz";

        if( !outputFileNameNegative.empty() )
        {
            std::cout << "Write p-values for negative z' values to: " << outputFileNameNegative << std::endl;
            ImageWriterType::Pointer writer = ImageWriterType::New();
            writer->SetInput( negative );
            writer->SetFileName( outputFileNameNegative );
            writer->Update();
        }
        if( !outputFileNamePositive.empty() )
        {
            std::cout << "Write p-values for positive z' values to: " << outputFileNamePositive << std::endl;
            ImageWriterType::Pointer writer = ImageWriterType::New();
            writer->SetInput( positive );
            writer->SetFileName( outputFileNamePositive );
            writer->Update();
        }
	}

	private:

    double P( double z, double N )
    {
        if( N < 4 )
        {
            std::cerr << "*** ERROR ***: N should be larger than 3!" << std::endl;
            exit( EXIT_FAILURE );
        }
        // Like randomise output ...
        //return 1 - erfc( std::abs( z ) * std::sqrt( N - 3 ) ) / static_cast< double >( sqrt( 2. ) );
        return 1 - erfc( std::abs( z ) * std::sqrt( N - 3 ) ) / static_cast< double >( sqrt( 2. ) );
    }

};

int main( int argc, char ** argv )
{
    tkd::CmdParser p( "fisher2p", "Calculate p-value from given z' value and N" );
    std::string inputFileName;
    int N;

    p.AddArgument( inputFileName, "input" )
    ->AddAlias( "i" )
    ->SetInput( "input" )
    ->SetDescription( "Fisher z' input file name" )
    ->SetRequired( true );

    p.AddArgument( N, "N" )
    ->SetInput( "N" )
    ->SetDescription( "Sample size used to calculate z'" )
    ->SetRequired( true );

  if ( !p.Parse( argc, argv ) )
    {
    p.PrintUsage( std::cout );
    return -1;
    }

  Fisher2P fisher2p;
  fisher2p.Run( inputFileName, N );

  return EXIT_SUCCESS;
}


// This file is part of LatNet Builder.
//
// Copyright (C) 2012-2021  The LatNet Builder author's, supervised by Pierre L'Ecuyer, Universite de Montreal.
//
// 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 "latbuilder/Parser/SizeParam.h"
#include "latbuilder/Parser/CoordUniformFigureOfMerit.h"
#include "latbuilder/Parser/Weights.h"
#include "latbuilder/Parser/MeritFilterList.h"

#include "latbuilder/MeritSeq/LatSeqOverCBC.h"
#include "latbuilder/MeritSeq/CoordUniformCBC.h"
#include "latbuilder/LatSeq/Combiner.h"
#include "latbuilder/GenSeq/VectorCreator.h"
#include "latbuilder/GenSeq/GeneratingValues.h"
#include "latbuilder/Traversal.h"
#include "latbuilder/LFSR258.h"
#include "latbuilder/TextStream.h"

#include <iostream>
#include <iomanip>
#include <functional>
#include <vector>

#include <boost/ref.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/count.hpp>
#include <boost/accumulators/statistics/min.hpp>
#include <boost/accumulators/statistics/max.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/tail_quantile.hpp>

using namespace LatBuilder;
using TextStream::operator<<;

template <typename T1, typename T2>
void printTableRow(const T1& x1, const T2& x2)
{
   using namespace std;
   cout << fixed << setprecision(3) << x1 << '\t'
      << scientific << setprecision(8) << x2 << std::endl;
}

struct Execute {
   template <class FIGURE, class LATSEQ>
   void execute(
         FIGURE figure,
         LatBuilder::SizeParam<LatticeType::ORDINARY, EmbeddingType::UNILEVEL> size,
         LATSEQ latSeq,
         unsigned int numSamples
         ) const
   {
      Storage<LatticeType::ORDINARY, EmbeddingType::UNILEVEL, FIGURE::suggestedCompression()> storage(std::move(size));

      auto latSeqOverCBC = MeritSeq::latSeqOverCBC(MeritSeq::cbc(storage, figure));

      //! [accumulator setup]
      using namespace boost::accumulators;

      accumulator_set<Real, features<tag::count, tag::min, tag::max, tag::mean, tag::tail_quantile<boost::accumulators::left>>>
          acc(tag::tail<boost::accumulators::left>::cache_size = numSamples);

      auto meritSeq = latSeqOverCBC.meritSeq(latSeq);
      for (const auto& val : meritSeq)
         acc(val);
      //! [accumulator setup]

      //! [output]
      unsigned int numBins = 20;
      printTableRow("# mean:", mean(acc));
      printTableRow("prob", "quantile");
      printTableRow(0.0, boost::accumulators::min(acc));
      for (unsigned int i = 1; i < numBins; i++) {
          double p = double(i) / numBins;
          Real q = quantile(acc, quantile_probability = p);
          printTableRow(p, q);
      }
      printTableRow(1.0, boost::accumulators::max(acc));
      //! [output]
   }

   // random sampling
   template <class FIGURE>
   void operator()(FIGURE figure, LatBuilder::SizeParam<LatticeType::ORDINARY, EmbeddingType::UNILEVEL> size, Dimension dimension, unsigned int nrand) const
   {
      typedef GenSeq::GeneratingValues<LatticeType::ORDINARY, FIGURE::suggestedCompression(), Traversal::Random<LFSR258>> Coprime;
      auto genSeqs = GenSeq::VectorCreator<Coprime>::create(size, dimension, nrand);
      genSeqs[0] = GenSeq::Creator<Coprime>::create(SizeParam<LatticeType::ORDINARY, EmbeddingType::UNILEVEL>(2), nrand);

      auto latSeq = LatSeq::combine<Zip>(size, std::move(genSeqs));

      std::cout << "# random samples: " << nrand << std::endl;

      execute(
            std::move(figure),
            std::move(size),
            std::move(latSeq),
            nrand
            );
   }

   // exhaustive sampling
   template <class FIGURE>
   void operator()(FIGURE figure, LatBuilder::SizeParam<LatticeType::ORDINARY, EmbeddingType::UNILEVEL> size, Dimension dimension) const
   {
      typedef GenSeq::GeneratingValues<LatticeType::ORDINARY, FIGURE::suggestedCompression()> Coprime;
      auto genSeqs = GenSeq::VectorCreator<Coprime>::create(size, dimension);
      genSeqs[0] = GenSeq::Creator<Coprime>::create(SizeParam<LatticeType::ORDINARY, EmbeddingType::UNILEVEL>(2));

      unsigned int numSamples = 1;
      for (const auto& g : genSeqs)
         numSamples *= g.size();

      auto latSeq = LatSeq::combine<CartesianProduct>(size, std::move(genSeqs));

      std::cout << "# deterministic samples: " << numSamples << std::endl;

      execute(
            std::move(figure),
            std::move(size),
            std::move(latSeq),
            numSamples
            );
   }
};

int main(int argc, const char *argv[])
{
   try {
      if (argc < 4 + 1 || argc > 5 + 1) {
         std::cout << "usage: quantiles <size> <dimension> <figure-of-merit> <weights> [<random samples>]" << std::endl;
         std::cout << "Do not use the prefix `CU:' on <figure-of-merit>; it will be automatically added." << std::endl;
         return -1;
      }

      int iarg = 0;
      std::string sizeSpec = argv[++iarg];
      Dimension dimension = (Dimension)atoi(argv[++iarg]);
      std::string figureSpec = argv[++iarg];
      std::string weightsSpec = argv[++iarg];
      auto weights = Parser::Weights::parse(weightsSpec);
      unsigned int nrand = 0;
      if (++iarg < argc)
        nrand =  (unsigned int)atoi(argv[iarg]);

      auto size = Parser::SizeParam<LatticeType::ORDINARY, EmbeddingType::UNILEVEL>::parse(sizeSpec);

      if (nrand) {
         Parser::CoordUniformFigureOfMerit<LatticeType::ORDINARY>::parse(
               "2",
               figureSpec,
               1,
               std::move(weights),
               Execute(),
               size,
               dimension,
               nrand);
      }
      else {
         Parser::CoordUniformFigureOfMerit<LatticeType::ORDINARY>::parse(
               "2",
               figureSpec,
               1,
               std::move(weights),
               Execute(),
               size,
               dimension);
      }
   }
   catch (Parser::ParserError& e) {
      std::cerr << "COMMAND LINE ERROR: " << e.what() << std::endl;
      std::exit(1);
   }
   catch (std::exception& e) {
      std::cerr << "ERROR: " << e.what() << std::endl;
      std::exit(1);
   }

   return 0;
}


/*
 * AdaptiveMesh.cpp
 *
 *  Created on: 09.08.2017
 *      Author: thies
 */

#include <deal.II/base/exceptions.h>
#include <deal.II/base/logstream.h>
#include <deal.II/base/memory_consumption.h>
#include <deal.II/dofs/dof_tools.h>
#include <deal.II/grid/cell_id.h>
#include <deal.II/lac/dynamic_sparsity_pattern.h>
#include <deal.II/numerics/matrix_tools.h>
#include <deal.II/numerics/solution_transfer.h>

#include <base/AdaptiveMesh.h>

#include <stdio.h>
#include <algorithm>
#include <iostream>
#include <list>

namespace wavepi {
namespace base {

template <int dim>
AdaptiveMesh<dim>::AdaptiveMesh(std::vector<double> times, FE_Q<dim> fe, Quadrature<dim> quad,
                                std::shared_ptr<Triangulation<dim>> tria)
    : SpaceTimeMesh<dim>(times, fe, quad), initial_triangulation(tria) {
  forward_patches  = std::vector<Patch>(this->length() - 1);
  backward_patches = std::vector<Patch>(this->length() - 1);

  vector_sizes      = std::vector<size_t>(this->length(), 0);
  sparsity_patterns = std::vector<std::shared_ptr<SparsityPattern>>(this->length());
  mass_matrices     = std::vector<std::shared_ptr<SparseMatrix<double>>>(this->length());
  laplace_matrices  = std::vector<std::shared_ptr<SparseMatrix<double>>>(this->length());
  constraints       = std::vector<std::shared_ptr<AffineConstraints<double>>>(this->length());

  working_time_idx = 0;

  working_triangulation = std::make_shared<Triangulation<dim>>();
  working_triangulation->copy_triangulation(*initial_triangulation);

  working_dof_handler = std::make_shared<DoFHandler<dim>>();
  working_dof_handler->initialize(*working_triangulation, this->fe);
}

template <int dim>
void AdaptiveMesh<dim>::reset() {
  vector_sizes      = std::vector<size_t>(this->length(), 0);
  mass_matrices     = std::vector<std::shared_ptr<SparseMatrix<double>>>(this->length());
  laplace_matrices  = std::vector<std::shared_ptr<SparseMatrix<double>>>(this->length());
  sparsity_patterns = std::vector<std::shared_ptr<SparsityPattern>>(this->length());
  constraints       = std::vector<std::shared_ptr<AffineConstraints<double>>>(this->length());

  working_time_idx = 0;

  working_dof_handler = std::make_shared<DoFHandler<dim>>();

  working_triangulation = std::make_shared<Triangulation<dim>>();
  working_triangulation->copy_triangulation(*initial_triangulation);

  working_dof_handler->initialize(*working_triangulation, this->fe);
}

template <int dim>
size_t AdaptiveMesh<dim>::n_dofs(size_t idx) {
  Assert(idx >= 0 && idx < this->length(), ExcIndexRange(idx, 0, this->length()));

  if (vector_sizes[idx] == 0) {
    get_dof_handler(idx);
    vector_sizes[idx] = working_dof_handler->n_dofs();
  }

  return vector_sizes[idx];
}

template <int dim>
std::shared_ptr<AffineConstraints<double>> AdaptiveMesh<dim>::get_constraint_matrix(size_t idx) {
  if (!constraints[idx]) {
    {
      LogStream::Prefix p("AdaptiveMesh");
      deallog << "making constraint matrix for time step " << idx << std::endl;
    }

    get_dof_handler(idx);
    constraints[idx] = std::make_shared<AffineConstraints<double>>();
    DoFTools::make_hanging_node_constraints(*working_dof_handler, *constraints[idx]);
    constraints[idx]->close();
  }

  return constraints[idx];
}

template <int dim>
std::shared_ptr<SparseMatrix<double>> AdaptiveMesh<dim>::get_mass_matrix(size_t idx) {
  Assert(idx >= 0 && idx < this->length(), ExcIndexRange(idx, 0, this->length()));

  if (!mass_matrices[idx]) {
    //{
    //   LogStream::Prefix p("AdaptiveMesh");
    //   deallog << "making mass matrix for time step " << idx << std::endl;
    //}

    get_sparsity_pattern(idx);
    get_dof_handler(idx);

    mass_matrices[idx] = std::make_shared<SparseMatrix<double>>(*sparsity_patterns[idx]);
    dealii::MatrixCreator::create_mass_matrix(*working_dof_handler, quad, *mass_matrices[idx]);
  }

  return mass_matrices[idx];
}

template <int dim>
std::shared_ptr<SparseMatrix<double>> AdaptiveMesh<dim>::get_laplace_matrix(size_t idx) {
  Assert(idx >= 0 && idx < this->length(), ExcIndexRange(idx, 0, this->length()));

  if (!laplace_matrices[idx]) {
    //{
    //   LogStream::Prefix p("AdaptiveMesh");
    //   deallog << "making laplace matrix for time step " << idx << std::endl;
    //}

    get_sparsity_pattern(idx);
    get_dof_handler(idx);

    laplace_matrices[idx] = std::make_shared<SparseMatrix<double>>(*sparsity_patterns[idx]);
    dealii::MatrixCreator::create_laplace_matrix(*working_dof_handler, quad, *laplace_matrices[idx]);
  }

  return laplace_matrices[idx];
}

template <int dim>
std::shared_ptr<SparsityPattern> AdaptiveMesh<dim>::get_sparsity_pattern(size_t idx) {
  Assert(idx >= 0 && idx < this->length(), ExcIndexRange(idx, 0, this->length()));

  if (!sparsity_patterns[idx]) {
    //{
    //   LogStream::Prefix p("AdaptiveMesh");
    //   deallog << "making sparsity pattern for time step " << idx << std::endl;
    //}

    get_constraint_matrix(idx);
    get_dof_handler(idx);

    DynamicSparsityPattern dsp(working_dof_handler->n_dofs(), working_dof_handler->n_dofs());
    DoFTools::make_sparsity_pattern(*working_dof_handler, dsp, *constraints[idx], true);

    sparsity_patterns[idx] = std::make_shared<SparsityPattern>();
    sparsity_patterns[idx]->copy_from(dsp);
  }

  return sparsity_patterns[idx];
}

template <int dim>
std::shared_ptr<DoFHandler<dim>> AdaptiveMesh<dim>::get_dof_handler(size_t idx) {
  Assert(idx >= 0 && idx < this->length(), ExcIndexRange(idx, 0, this->length()));

  transfer(idx);
  return working_dof_handler;
}

template <int dim>
std::shared_ptr<Triangulation<dim>> AdaptiveMesh<dim>::get_triangulation(size_t idx) {
  Assert(idx >= 0 && idx < this->length(), ExcIndexRange(idx, 0, this->length()));

  transfer(idx);
  return working_triangulation;
}

template <int dim>
std::shared_ptr<DoFHandler<dim>> AdaptiveMesh<dim>::transfer(size_t source_time_index, size_t target_time_index,
                                                             std::initializer_list<Vector<double> *> vectors) {
  Assert(source_time_index >= 0 && source_time_index < this->length(),
         ExcIndexRange(source_time_index, 0, this->length()));
  Assert(target_time_index >= 0 && target_time_index < this->length(),
         ExcIndexRange(target_time_index, 0, this->length()));

  transfer(source_time_index);

  std::vector<Vector<double> *> vecs;
  for (auto vec : vectors)
    vecs.push_back(vec);

  transfer(target_time_index, vecs);

  return working_dof_handler;
}

template <int dim>
bool AdaptiveMesh<dim>::any(const std::vector<bool> &v) const {
  for (auto b : v)
    if (b) return true;

  return false;
}

template <int dim>
void AdaptiveMesh<dim>::patch(const Patch &patch, std::vector<Vector<double> *> &vectors) {
  for (auto p : patch) {
    auto cells_to_refine  = p.first;
    auto cells_to_coarsen = p.second;

    LogStream::Prefix lp("patch");
    deallog << "Applying patch (working_time_idx = " << working_time_idx << ") in a set of " << patch.size()
            << std::endl;

    if (vectors.size() == 0) {
      working_triangulation->load_refine_flags(cells_to_refine);
      working_triangulation->load_coarsen_flags(cells_to_coarsen);

      working_triangulation->prepare_coarsening_and_refinement();
      working_triangulation->execute_coarsening_and_refinement();

      working_dof_handler->distribute_dofs(fe);
    } else if (!any(cells_to_coarsen)) {
      SolutionTransfer<dim, Vector<double>> trans(*working_dof_handler);

      working_triangulation->load_refine_flags(cells_to_refine);
      working_triangulation->prepare_coarsening_and_refinement();
      trans.prepare_for_pure_refinement();

      working_triangulation->execute_coarsening_and_refinement();
      working_dof_handler->distribute_dofs(fe);

      for (auto vec : vectors) {
        Vector<double> tmp(*vec);
        vec->reinit(working_dof_handler->n_dofs());
        trans.refine_interpolate(tmp, *vec);
      }
    } else {
      SolutionTransfer<dim, Vector<double>> trans(*working_dof_handler);

      std::vector<Vector<double>> all_in;
      for (auto vec : vectors)
        all_in.emplace_back(*vec);

      working_triangulation->load_refine_flags(cells_to_refine);
      working_triangulation->load_coarsen_flags(cells_to_coarsen);
      working_triangulation->prepare_coarsening_and_refinement();
      trans.prepare_for_coarsening_and_refinement(all_in);

      working_triangulation->execute_coarsening_and_refinement();
      working_dof_handler->distribute_dofs(fe);

      std::vector<Vector<double>> all_out(vectors.size(), Vector<double>(working_dof_handler->n_dofs()));
      trans.interpolate(all_in, all_out);

      for (size_t i = 0; i < vectors.size(); i++)
        *vectors[i] = all_out[i];
    }
  }
}
template <int dim>
void AdaptiveMesh<dim>::transfer(size_t target_idx) {
  std::vector<Vector<double> *> vecs;
  transfer(target_idx, vecs);
}

template <int dim>
void AdaptiveMesh<dim>::transfer(size_t target_idx, std::vector<Vector<double> *> &vectors) {
  Assert(target_idx >= 0 && target_idx < this->length(), ExcIndexRange(target_idx, 0, this->length()));

  LogStream::Prefix p("AdaptiveMesh");
  LogStream::Prefix pp("transfer");

  if (target_idx != working_time_idx)
    deallog << working_time_idx << " → " << target_idx << ", taking " << vectors.size() << " vector(s) along"
            << std::endl;

  if (working_time_idx < target_idx)
    for (size_t idx = working_time_idx; idx < target_idx; idx++) {
      patch(forward_patches[idx], vectors);
      working_time_idx++;

      // hanging node constraints
      // (only needed if we coarsened any cells)
      // is this even required after each patch element? would require a lot more ConstraintMatrices ...
      // commented out because constraining in WaveEquation results in reflections from the 'refinement boundary'
      // the solution fullfills the constraints because of distribute calls in WaveEquation and WaveEquationAdjoint.
      // for (size_t i = 0; i < vectors.size(); i++)
      //   get_constraint_matrix(working_time_idx)->distribute(*vectors[i]);
    }
  else if (working_time_idx > target_idx)
    for (ssize_t idx = working_time_idx; idx > (ssize_t)target_idx; idx--) {
      patch(backward_patches[idx - 1], vectors);
      working_time_idx--;

      // hanging node constraints!
      // (only needed if we coarsened any cells)
      // is this even required after each patch element? would require a lot more ConstraintMatrices ...
      // commented out because constraining in WaveEquation results in reflections from the 'refinement boundary'
      // the solution fullfills the constraints because of distribute calls in WaveEquation and WaveEquationAdjoint.
      // for (size_t i = 0; i < vectors.size(); i++)
      //   get_constraint_matrix(working_time_idx)->distribute(*vectors[i]);
    }
}

template <int dim>
size_t AdaptiveMesh<dim>::memory_consumption() const {
  size_t mem =
      MemoryConsumption::memory_consumption(*working_dof_handler) +
      MemoryConsumption::memory_consumption(working_dof_handler) +
      MemoryConsumption::memory_consumption(*working_triangulation) +
      MemoryConsumption::memory_consumption(working_triangulation) +
      MemoryConsumption::memory_consumption(*initial_triangulation) +
      MemoryConsumption::memory_consumption(initial_triangulation) + MemoryConsumption::memory_consumption(times) +
      MemoryConsumption::memory_consumption(sparsity_patterns) + MemoryConsumption::memory_consumption(mass_matrices) +
      MemoryConsumption::memory_consumption(vector_sizes) + MemoryConsumption::memory_consumption(working_time_idx);

  for (size_t i = 0; i < this->length(); i++) {
    if (mass_matrices[i]) mem += MemoryConsumption::memory_consumption(*mass_matrices[i]);

    if (sparsity_patterns[i]) mem += MemoryConsumption::memory_consumption(*sparsity_patterns[i]);
  }

  return mem;
}

template <int dim>
const std::vector<Patch> &AdaptiveMesh<dim>::get_forward_patches() const {
  return forward_patches;
}

template <int dim>
void AdaptiveMesh<dim>::set_forward_patches(const std::vector<Patch> &forward_patches) {
  this->forward_patches = forward_patches;

  reset();
  generate_backward_patches();
}

template <int dim>
void AdaptiveMesh<dim>::generate_backward_patches() {
  Assert(working_time_idx == 0, ExcInternalError());

  for (size_t idx = 0; idx < this->length() - 1; idx++, working_time_idx++) {
    Patch bw_patches;

    for (auto p : forward_patches[idx]) {
      auto cells_to_refine  = p.first;
      auto cells_to_coarsen = p.second;

      working_triangulation->load_refine_flags(cells_to_refine);
      working_triangulation->load_coarsen_flags(cells_to_coarsen);
      working_triangulation->prepare_coarsening_and_refinement();

      // Save cell ids of cells to be refined,
      // and parents of cells that will be coarsened (will appear multiple times!)
      std::list<CellId> ids_to_refine;
      std::list<CellId> ids_to_coarsen;

      for (auto cell : working_triangulation->active_cell_iterators())
        if (cell->coarsen_flag_set())
          ids_to_coarsen.push_back(cell->parent()->id());
        else if (cell->refine_flag_set())
          ids_to_refine.push_back(cell->id());

      working_triangulation->execute_coarsening_and_refinement();
      working_dof_handler->distribute_dofs(fe);

      auto refine_it  = ids_to_refine.begin();
      auto coarsen_it = ids_to_coarsen.begin();

      // Set refinement flags and coarsening flags according to saved cell ids
      for (auto cell : working_triangulation->active_cell_iterators()) {
        auto parent_id = cell->parent()->id();

        while (refine_it != ids_to_refine.end() && *refine_it < parent_id)
          refine_it++;

        if (refine_it != ids_to_refine.end() && *refine_it == parent_id) cell->set_coarsen_flag();

        auto my_id = cell->id();

        while (coarsen_it != ids_to_coarsen.end() && *coarsen_it < my_id)
          coarsen_it++;

        if (coarsen_it != ids_to_coarsen.end() && *coarsen_it == my_id) cell->set_refine_flag();
      }

      cells_to_refine.clear();
      cells_to_coarsen.clear();

      working_triangulation->save_refine_flags(cells_to_refine);
      working_triangulation->save_coarsen_flags(cells_to_coarsen);

      bw_patches.emplace_back(cells_to_refine, cells_to_coarsen);
    }

    // patches need to be applied in reverse order
    std::reverse(bw_patches.begin(), bw_patches.end());
    backward_patches[idx] = bw_patches;
  }
}

template <int dim>
void AdaptiveMesh<dim>::refine_and_coarsen(std::vector<size_t> refine_intervals __attribute((unused)),
                                           std::vector<size_t> coarsen_time_points __attribute((unused)),
                                           std::vector<std::vector<bool>> refine_trias __attribute((unused)),
                                           std::vector<std::vector<bool>> coarsen_trias __attribute((unused)),
                                           std::initializer_list<DiscretizedFunction<dim> *> interpolate_vectors
                                           __attribute((unused))) {
  // TODO: implement refine_and_coarsen
  AssertThrow(false, ExcNotImplemented());
}

template class AdaptiveMesh<1>;
template class AdaptiveMesh<2>;
template class AdaptiveMesh<3>;

}  // namespace base
} /* namespace wavepi */


/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
   Copyright 2021 Kenneth W. Chapman

   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 "log.h"
#include "kalman.h"
#include "csvreader.h"
#include <vector>
#include <fstream>
#include <opencv2/video/tracking.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/json_parser.hpp>
#include <boost/exception/exception.hpp>
#include <boost/exception/diagnostic_information.hpp>
#include "timestampconvert.h"

using namespace cv;
using namespace std;
using namespace boost;

namespace gc
{

Kalman::Kalman()
{

}

GC_STATUS Kalman::ApplyFromFile( const string jsonFilepath )
{
    GC_STATUS retVal = GC_OK;

    try
    {
        ifstream inStream( jsonFilepath );
        if ( !inStream.is_open() )
        {
            FILE_LOG( logERROR ) << "[Kalman::ApplyFromFile] Could not open json parameters file: " << jsonFilepath;
            retVal = GC_ERR;
        }
        else
        {
            string jsonString( ( istreambuf_iterator< char >( inStream ) ),
                                 istreambuf_iterator< char >() );
            if ( jsonString.empty() )
            {
                FILE_LOG( logERROR ) << "[Kalman::ApplyFromFile] Json file held no parameters: " << jsonFilepath;
                retVal = GC_ERR;
            }
            else
            {
                retVal = ApplyFromString( jsonString );
            }
            inStream.close();
        }
    }
    catch( std::exception &e )
    {
        FILE_LOG( logERROR ) << "[Kalman::ApplyFromFile] " << e.what();
        retVal = GC_EXCEPT;
    }
    return retVal;
}
GC_STATUS Kalman::ApplyFromString( const string jsonString )
{
    KalmanParams params;
    GC_STATUS retVal = ParamsFromJson( jsonString, params );
    if ( GC_OK == retVal )
    {
        retVal = Apply( params );
    }
    return retVal;
}
GC_STATUS Kalman::Apply( const KalmanParams params )
{
    GC_STATUS retVal = GC_OK;

    try
    {
        CSVReader reader( params.inputCSVFilepath );
        vector< vector< string > > data = reader.getData();
        if ( data.empty() )
        {
            FILE_LOG( logERROR ) << "[Kalman::Apply] No data in input file " << params.inputCSVFilepath;
            retVal = GC_ERR;
        }
        else
        {
            ofstream outFile( params.outputCSVFilepath );
            if ( !outFile.is_open() )
            {
                FILE_LOG( logERROR ) << "[ApplyKalmanFilterToCSV] Could not open output file for writing " << params.outputCSVFilepath ;
                retVal = GC_ERR;
            }
            else
            {
                outFile << "Timestamp, measured, estimated" << endl;
                KalmanItem item;
                GC_STATUS retTimeGet = GcTimestampConvert::ConvertDateToSeconds( data[ 0 ][ params.datetimeColumn ],
                        params.timeStringStartCol, params.timeStringLength, params.datetimeFormat, item.secsSinceEpoch );
                if ( GC_OK == retTimeGet )
                {
                    item.measurement = stod( data[ 0 ][ params.measurementColumn ] );
                    item.prediction = item.measurement;
                    outFile << data[ 0 ][ params.datetimeColumn ] << "," << item.measurement << "," << item.prediction << endl;
                }

                KalmanFilter kf( 4, 2, 0 );
                kf.transitionMatrix = ( Mat_< float >( 4, 4 ) << 1,0,1,0,  0,1,0,1,  0,0,1,0,  0,0,0,1 );
                Mat_< float > measurement( 2, 1 ); measurement.setTo( Scalar( 0 ) );

                kf.statePre.at< float >( 0 ) = static_cast< float >( item.secsSinceEpoch );
                kf.statePre.at< float >( 1 ) = static_cast< float >( item.measurement );
                kf.statePre.at< float >( 2 ) = 0.0f;
                kf.statePre.at< float >( 3 ) = 0.0f;

                kf.statePost = kf.statePre;

                // setIdentity( kf.processNoiseCov, Scalar::all( 1e-4 ) );
                // setIdentity( kf.measurementNoiseCov, Scalar::all( 10 ) );
                // setIdentity( kf.errorCovPost, Scalar::all( 0.1 ) );

                setIdentity( kf.measurementMatrix );
                setIdentity( kf.processNoiseCov, Scalar::all( 1e-6 ) );
                setIdentity( kf.measurementNoiseCov, Scalar::all( 20 ) );
                setIdentity( kf.errorCovPost, Scalar::all( 1 ) );

                for ( size_t i = 1; i < data.size(); ++i )
                {
                    if ( GC_OK == retTimeGet )
                    {
                        retTimeGet = GcTimestampConvert::ConvertDateToSeconds( data[ i ][ params.datetimeColumn ],
                                params.timeStringStartCol, params.timeStringLength, params.datetimeFormat, item.secsSinceEpoch );
                        item.measurement = stod( data[ i ][ params.measurementColumn ] );
                        if ( -1.0 < item.measurement )
                        {
                            Mat prediction = kf.predict();
                            measurement( 0 ) = static_cast< float >( item.secsSinceEpoch );
                            measurement( 1 ) = static_cast< float >( item.measurement );

                            Mat estimated = kf.correct( measurement );
                            item.prediction = estimated.at< float >( 1 );
                            outFile << data[ i ][ params.datetimeColumn ] << "," << item.measurement << "," << item.prediction << endl;
                        }
                    }
                }
            }
        }
    }
    catch( std::exception &e )
    {
        FILE_LOG( logERROR ) << "[Kalman::Apply] " << e.what();
        retVal = GC_EXCEPT;
    }
    return retVal;
}
GC_STATUS Kalman::ParamsToJsonFile( const KalmanParams params, string jsonFilepath )
{
    GC_STATUS retVal = GC_OK;

    try
    {
        string jsonString;
        retVal = ParamsToJson( params, jsonString );
        if ( GC_OK == retVal )
        {
            ofstream outFile( jsonFilepath );
            if ( !outFile.is_open() )
            {
                FILE_LOG( logERROR ) << "[Kalman::ParamsToJsonFile] Could not open file to write: " << jsonFilepath;
                retVal = GC_ERR;
            }
            else
            {
                outFile << jsonString;
                outFile.close();
            }
        }
    }
    catch( std::exception &e )
    {
        FILE_LOG( logERROR ) << "[Kalman::ParamsToJsonFile] " << e.what();
        retVal = GC_EXCEPT;
    }
    return retVal;
}
GC_STATUS Kalman::ParamsFromJson( const string jsonString, KalmanParams &params )
{
    GC_STATUS retVal = GC_OK;

    try
    {
        stringstream ss( jsonString );
        property_tree::ptree pt;
        property_tree::json_parser::read_json( ss, pt );
        params.datetimeFormat = pt.get< string >( "datetime_format" );
        params.outputCSVFilepath = pt.get< string >( "output_csv_filepath" );
        params.inputCSVFilepath = pt.get< string >( "input_csv_filepath" );
        params.firstDataRow = pt.get< int >( "first_data_row" );
        params.datetimeColumn = pt.get< int >( "datetime_column" );
        params.measurementColumn = pt.get< int >( "measurement_column" );
    }
    catch( boost::exception &e )
    {
        FILE_LOG( logERROR ) << "[Kalman::ParamsFromJson] " << boost::diagnostic_information( e );
        retVal = GC_EXCEPT;
    }
    return retVal;
}
GC_STATUS Kalman::ParamsToJson( const KalmanParams params, string &jsonString )
{
    GC_STATUS retVal = GC_OK;

    try
    {
        stringstream ss;
        ss << "{" << endl;
        ss << "   \"datetime_format\":" << params.datetimeFormat << "," << endl;
        ss << "   \"output_csv_filepath\":" << params.outputCSVFilepath << "," << endl;
        ss << "   \"input_csv_filepath\":" << params.inputCSVFilepath << "," << endl;
        ss << "   \"first_data_row\":" << params.firstDataRow << "," << endl;
        ss << "   \"datetime_column\":" << params.datetimeColumn << "," << endl;
        ss << "   \"measurement_column\":" << params.measurementColumn << endl;
        ss << "}" << endl;
        jsonString = ss.str();
    }
    catch( std::exception &e )
    {
        FILE_LOG( logERROR ) << "[Kalman::ParamsToJson] " << e.what();
        retVal = GC_EXCEPT;
    }
    return retVal;
}

} // namespace gc


#define CATCH_CONFIG_MAIN

#include "CALPHADFreeEnergyFunctionsTernary.h"

#include "catch.hpp"

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

#include <array>
#include <fstream>
#include <string>

namespace pt = boost::property_tree;

TEST_CASE("CALPHADternaryTieLine", "[CALPHADternaryTieLine]")
{
    Thermo4PFM::EnergyInterpolationType energy_interp_func_type
        = Thermo4PFM::EnergyInterpolationType::PBG;
    Thermo4PFM::ConcInterpolationType conc_interp_func_type
        = Thermo4PFM::ConcInterpolationType::PBG;

    pt::ptree calphad_db;
    try
    {
        pt::read_json("../thermodynamic_data/calphadMoNbTa.json", calphad_db);
    }
    catch (std::exception& e)
    {
        std::cerr << "exception caught: " << e.what() << std::endl;
    }

    boost::optional<pt::ptree&> newton_db;

    Thermo4PFM::CALPHADFreeEnergyFunctionsTernary cafe(
        calphad_db, newton_db, energy_interp_func_type, conc_interp_func_type);

    double temperature[2] = { 2923., 3073. };

    // initial guesses
    double init_guess[2][5]
        = { { 0.33, 0.38, 0.32, 0.33, 0.8 }, { 0.13, 0.21, 0.12, 0.15, 0.8 } };

    // nominal composition
    double nominalc[2][2] = { { 0.33, 0.33 }, { 0.16, 0.225 } };

    // expected compositions and phase fractions
    double expected_cl[2][2] = { { 0.344909, 0.380528 }, { 0.164344, 0.2321 } };
    double expected_cs[2][2]
        = { { 0.329373, 0.327874 }, { 0.13675, 0.186999 } };
    double expected_fs[2] = { 0.959621, 0.157429 };

    int maxits = 20;

    for (int itest = 0; itest < 2; itest++)
    {
        std::cout << "----------------------------------------" << std::endl;
        double lceq[5] = { init_guess[itest][0], init_guess[itest][1], // liquid
            init_guess[itest][2], init_guess[itest][3], // solid
            init_guess[itest][4] };

        bool found_ceq = cafe.computeTieLine(temperature[itest],
            nominalc[itest][0], nominalc[itest][1], &lceq[0], maxits);
        if (lceq[0] != lceq[0]) found_ceq = false;
        if (lceq[0] > 1.) found_ceq = false;
        if (lceq[0] < 0.) found_ceq = false;
        if (lceq[1] > 1.) found_ceq = false;
        if (lceq[1] < 0.) found_ceq = false;

        std::cout << "Temperature = " << temperature[itest] << std::endl;
        std::cout << "Nominal composition " << nominalc[itest][0] << ","
                  << nominalc[itest][1] << std::endl;
        REQUIRE(found_ceq);
        std::cout << "Found equilibrium concentrations: " << std::endl;
        std::cout << "Liquid: " << lceq[0] << "," << lceq[1] << std::endl;
        std::cout << "Solid:  " << lceq[2] << "," << lceq[3] << std::endl;
        std::cout << "Solid fraction: " << lceq[4] << std::endl;

        // test values
        const double tol = 1.e-5;
        CHECK(lceq[0] == Approx(expected_cl[itest][0]).margin(tol));
        CHECK(lceq[1] == Approx(expected_cl[itest][1]).margin(tol));
        CHECK(lceq[2] == Approx(expected_cs[itest][0]).margin(tol));
        CHECK(lceq[3] == Approx(expected_cs[itest][1]).margin(tol));
        CHECK(lceq[4] == Approx(expected_fs[itest]).margin(1.e-4));
    }
}