/*
 * =====================================================================================
 *
 *       Filename:  tutor.cpp
 *
 *    Description:  Tutor class implementation
 *
 *        Version:  1.0
 *        Created:  12/13/2011 08:35:46 AM
 *       Revision:  none
 *       Compiler:  gcc
 *
 *         Author:  Sabelnikov Daniil (), dsabelnikov@gmail.com
 *        Company:
 *
 * =====================================================================================
 */

#include "tutor.h"

#include "common/common_include.h"

#include <boost/filesystem.hpp>
#include <boost/filesystem/fstream.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/regex.hpp>
#include <boost/algorithm/string.hpp>
#include "tools/topology_factory.h"
#include <boost/tokenizer.hpp>
#include "cpu_tutor_algorithms.h"
#include "cpu_tutor_algorithms_alpha.h"

#ifdef CUDA_ENABLED
#include "cuda_tutor_algorithms.h"
#endif

#include <utility>

using namespace mb;
using namespace std;

namespace fs = boost::filesystem;
namespace algo = boost::algorithm;

typedef boost::tokenizer< boost::escaped_list_separator<char> > t_tokenizer;

/* Constants */
const static char* DEVICE_OPTION = "computation-device";
const static char* POOL_SIZE_OPTION = "pool-size";
const static char* TOPOLOGY_TYPE_OPTION = "topology-type";
const static char* TOPOLOGY_INPUT_SIZE_OPTION = "topology-input-size";
const static char* TOPOLOGY_OUTPUT_SIZE_OPTION = "topology-output-size";
const static char* CONNECTED_LAYERS_TYPE = "connected-layers";
const static char* CONNECTED_LAYERS_SIZES = "connected-layers-sizes";
const static char* STEPS_OPTION = "steps";
const static char* MAX_WEIGHT_OPTION = "max-weight";
const static char* MAX_BIAS_OPTION = "max-bias";
const static char* NETWORK_TYPE_OPTION = "network-type";

const static boost::regex UINT_REGEX( "[+]?[0-9]+" );

/*
 *--------------------------------------------------------------------------------------
 *       Class:  Tutor
 *      Method:  Tutor
 * Description:  constructor
 * 				 During class initialization configuration will be parsed from a
 * 				 configuration file.
 *--------------------------------------------------------------------------------------
 */
Tutor::Tutor () : config_file( "conf/tutor.conf" )
{
	init();
}  /* -----  end of method Tutor::Tutor  (constructor)  ----- */
Tutor::Tutor ( string config_file ) : config_file( config_file )
{
	init();
}  /* -----  end of method Tutor::Tutor  (constructor)  ----- */

void Tutor::init ()
{
	LOG(INFO) << "Tutor initializaton begins...";
	// Parse parameters from config file
	fs::path config_path( config_file );

	if ( fs::exists( config_path ) ){
		try{
			fs::ifstream config_ifs( config_path, ios_base::in );
			po::store( po::parse_config_file( config_ifs, *(get_options()), true), options_vm );
			config_ifs.close();
			po::notify( options_vm );
		} catch (po::required_option err) {
			LOG(FATAL) << "Required option not found: " << err.get_option_name();
		} catch (po::validation_error err) {
			LOG(FATAL) << "Option value invalid: " << err.get_option_name();
		}
	} else {
		LOG(FATAL) << "Tutor config file " << config_file << " not found!";
	}

	// Initializing topology

	/* Topology type */
	string topology_type = options_vm[ TOPOLOGY_TYPE_OPTION ].as<string>();

	/* Input size */
	uint input_size = options_vm[ TOPOLOGY_INPUT_SIZE_OPTION ].as<uint>();

	/* Output size */
	uint output_size = options_vm[ TOPOLOGY_OUTPUT_SIZE_OPTION ].as<uint>();

	/* Type-specific initialization */

	if (topology_type == CONNECTED_LAYERS_TYPE){

		/* Connected layers sizes */
		if (options_vm.count(CONNECTED_LAYERS_SIZES) < 1){
			LOG(FATAL) << "Configuration error: \"" << CONNECTED_LAYERS_SIZES << "\" option is required for connected layers topology";
		}
		string layers_sizes = options_vm[ CONNECTED_LAYERS_SIZES ].as< string >();
		t_tokenizer tok( layers_sizes );

		vector<uint> layers;
		foreach( string s, make_pair( tok.begin(), tok.end() ) ){
			try{
				layers.push_back( boost::lexical_cast<uint>( s ) );
			}catch(boost::bad_lexical_cast err){
				LOG(WARNING) << "Configuration error: \"" << CONNECTED_LAYERS_SIZES <<
					"\", invalid format value \"" << s << "\" skipped";
			}
		};

		topology = TopologyFactory::create_connected_layers_topology(input_size, output_size, layers);
	}

	LOG(INFO) << "Tutor initialization finished successfully.";
}		/* -----  end of method Tutor::init  ----- */

	NetworkPtr
Tutor::get_best ()
{
	// TODO
	NetworkPtr empty;
	if (pool.size() == 0){
		return empty;
	}
	NetworkPtr best = pool[0];
	float r_max = 0;
	foreach(NetworkPtr net, pool){
		float r = NetworkTest::getReliability(net, tests);
		if (r > r_max){
			r_max = r;
			best = net;
		}
	}
	return best;
}		/* -----  end of method Tutor::get_best  ----- */

void Tutor::set_tests ( vector<TestCasePtr> tests )
{
	this->tests = tests;
}		/* -----  end of method Tutor::set_tests  ----- */

	const boost::shared_ptr<po::options_description>
Tutor::get_options ()
{
	if (options.get() == 0){
		init_options();
	}
	return options;
}		/* -----  end of method Tutor::get_options  ----- */

	const boost::shared_ptr<Topology>
Tutor::get_topology ()
{
	return topology;
}		/* -----  end of method Tutor::get_topology  ----- */

void Tutor::init_options ()
{
	po::options_description* opt = new po::options_description("Tutor options");
	opt->add_options()
		( DEVICE_OPTION, po::value<string>(), "CPU or CUDA")
		( TOPOLOGY_TYPE_OPTION, po::value<string>()->required(), "Type of topology, currently supported: connected-layers")
		( TOPOLOGY_INPUT_SIZE_OPTION, po::value<uint>()->required(), "Input layer size")
		( TOPOLOGY_OUTPUT_SIZE_OPTION, po::value<uint>()->required(), "Output layer size")
		( CONNECTED_LAYERS_SIZES, po::value< string >(), "'connected-layers' topology settings: sizes of layers, delimeted by commas or whitespaces.")
		( POOL_SIZE_OPTION, po::value< uint >()->required(), "Network pool size, the number of networks in one generation")
		( STEPS_OPTION, po::value<uint>()->required(), "The number of education steps to perform")
		( MAX_WEIGHT_OPTION, po::value<t_weight>(), "The maximum connection weight for generated networks")
		( MAX_BIAS_OPTION, po::value<t_weight>(), "The maximum bias for generated networks")
		( NETWORK_TYPE_OPTION, po::value<string>()->default_value("omega"), "The type of the neuro-network");
	options.reset( opt );
}		/* -----  end of method Tutor::init_options  ----- */

	boost::shared_ptr<TutorAlgorithms>
Tutor::init_algorithms ()
{
	boost::shared_ptr<TutorAlgorithms> ptr;
	string device = options_vm[ DEVICE_OPTION ].as< string >();

#ifndef CUDA_ENABLED
	if(device == "CUDA"){
		LOG(WARNING) << "Tutor has been compiled without CUDA support, 'CUDA' device not allowed. Falling back to CPU.";
		device = "CPU";
	}
#endif
	if (device != "CPU" && device != "CUDA"){
		LOG(WARNING) << "Specified computation device " << device << " is not supported. Using default - CPU.";
		device = "CPU";
	}

	string network_type = options_vm[ NETWORK_TYPE_OPTION ].as<string>();

	if (device == "CPU"){
		if (network_type == "alpha"){
			LOG(INFO) << "Network type: Alpha";
			ptr.reset( new CPUTutorAlgorithmsAlpha() );
		} else {
			LOG(INFO) << "Network type: Omega";
			ptr.reset( new CPUTutorAlgorithms() );
		}
	} else if (device == "CUDA"){
#ifdef CUDA_ENABLED
		if (network_type == "alpha"){
			LOG(INFO) << "Network type: Alpha";
			ptr.reset( new CUDATutorAlgorithms("alpha") );
		} else {
			LOG(INFO) << "Network type: Omega";
			ptr.reset( new CUDATutorAlgorithms() );
		}
#endif
	}
	return ptr;
}		/* -----  end of method Tutor::init_algorithms  ----- */

bool
Tutor::is_CUDA ()
{
	string device = options_vm[ DEVICE_OPTION ].as< string >();
	return device == "CUDA";
}		/* -----  end of method Tutor::is_CUDA  ----- */

/*
 *--------------------------------------------------------------------------------------
 *       Class:  Tutor
 *      Method:  Tutor :: educate
 * Description:  Perform neural network education. This method calls the TutorAlgorithms
 * 				 routines to perform following steps in education process:
 * 				 1. reproduction() - fill certain amount of next generation pool with
 * 				    children originated from current pool.
 * 				 2. mutation() - perform a rendom mutation on current generation pool.
 * 				 3. examination() - test all network with loaded testcases,
 * 				    result of the examination will be returned in an array of scores
 * 				    that will be used to select the best network on next step.
 * 				 4. selection() - do a natural selection, this is a partly rendom process
 * 				    that selects best networks according to their scores at examination.
 * 				 5. check if stop condition emerged.
 *--------------------------------------------------------------------------------------
 */
void Tutor::educate ()
{
	if (tests.size() == 0){
		LOG(WARNING) << "No test cases loaded. Ignoring call to education()...";
		return;
	}

	LOG(INFO) << "Education process start.";

	boost::shared_ptr<TutorAlgorithms> algos = init_algorithms();
	uint pool_size = options_vm[ POOL_SIZE_OPTION ].as< uint >();

	float reproduction_ratio = 0.5;
	float mutation_ratio = 0.5;

	// -- Load Max weight and max bias from options
	//
	t_weight max_weight = 1;
	t_weight max_bias = 1;

	if ( options_vm.count( MAX_WEIGHT_OPTION ) != 0 ){
		max_weight = options_vm[ MAX_WEIGHT_OPTION ].as<t_weight>();
	}
	if ( options_vm.count( MAX_BIAS_OPTION ) != 0 ){
		max_bias = options_vm[ MAX_BIAS_OPTION ].as<t_weight>();
	}

	uint steps = options_vm[ STEPS_OPTION ].as<uint>();

	// -- Clear network pools
	//
	pool.clear();
	_pool.clear();

	// -- Fill first pool with random networks
	//
	srand( time(NULL) );
	NetworkPtr *ptr;
	for (uint i = 0; i < pool_size; i++){
		ptr = new NetworkPtr( new Network(topology.get(), max_weight, max_bias) );
		pool.push_back( *ptr );
	}

	// -- Education cycle start
	//
	uint step = 1;
	while (true){

		LOG(INFO) << " -- Starting step #" << step;
		clock_t step_start = clock();

		// -- 1. Reproduction.
		//
		foreach( NetworkPtr ptr, algos->reproduction( pool, reproduction_ratio ) ){
			_pool.push_back(ptr);
		}
		// FIXME experimentail feature: throwing in two new networks
		if (_pool.size() > 2){
			_pool.pop_back();
			_pool.pop_back();
			_pool.push_back( *(new NetworkPtr(new Network(topology.get(), max_weight, max_bias))) );
			_pool.push_back( *(new NetworkPtr(new Network(topology.get(), max_weight, max_bias))) );
		}
		assert( pool.size() == pool_size );

		// -- 2. Mutation.
		//
//		algos->mutation( _pool, mutation_ratio, step/100 );
		algos->mutation( pool, mutation_ratio, 0 );
		assert( pool.size() == pool_size );

		// -- 3. Examination.
		//
		vector<float> scores = algos->examination( pool, tests );
		assert( pool.size() == pool_size );

		// -- Print scores
		//
		vector<float> sorted_scores;
		sorted_pool.clear();
		for(uint i = 0; i < scores.size(); i++){
			float score = scores[i];
			vector<float>::iterator iter = lower_bound(sorted_scores.begin(), sorted_scores.end(), score);
			int index = iter - sorted_scores.begin();
			sorted_scores.insert(iter, score);
			sorted_pool.insert(sorted_pool.begin() + index, pool[i]);
		}
		LOG(INFO) << "  Sorted scores:";
		for (uint i = 0; i < 5 && i < sorted_scores.size(); i++){
			LOG(INFO) << "    " << sorted_scores[i];
		}

		// -- 4. Selection.
		//
		foreach( NetworkPtr ptr, algos->selection( pool, scores, 1 - reproduction_ratio ) ){
			_pool.push_back( ptr );
		}

		assert( pool.size() == pool_size );
		assert( _pool.size() == pool_size );

		// -- 5. Check stop condition
		//
		if (step >= steps){
			break;
		}

		// -- Increment step counter
		step++;

		// -- Swap pools
		pool.clear();
		foreach( NetworkPtr ptr, _pool ){
			pool.push_back( ptr );
		}
		_pool.clear();

		// -- Step finished
		LOG(INFO) << " -- Step finished in: " << (float)( clock() - step_start ) / CLOCKS_PER_SEC << "sec.";

		// -- Debug
		if (step % 20 == 0){
			cout << "Starting step #" << step << endl;
			if (pool.size() >= 5){
				cout << "Top 5 reliabilities: ";
				for (uint i = 0; i < 5; i++){
					cout << NetworkTest::getReliability(sorted_pool[i], tests) << ", ";
				}
				cout << endl;
				cout << "Top 5 advantages: ";
				for (uint i = 0; i < 5; i++){
					cout << NetworkTest::getAdvantage(sorted_pool[i], tests) << ", ";
				}
				cout << endl;
			}
		}

	}

#ifdef CUDA_ENABLED
	// -- Releasing memory for CUDA device
	//
	if (is_CUDA()){
		boost::static_pointer_cast<CUDATutorAlgorithms>(algos)->free();
	}
#endif

	LOG(INFO) << "Education finished";
}		/* -----  end of method Tutor::educate  ----- */


#include "OpenCVStitcher.h"

#include <iostream>
#include <sstream>
#include <cstdint>

#include <Eigen/LU>

#include <opencv2/imgcodecs.hpp>
#include <opencv2/core.hpp>
#include <opencv2/core/utility.hpp>
#include <opencv2/imgproc.hpp>

OpenCVStitcher::OpenCVStitcher()
{
}

OpenCVStitcher::~OpenCVStitcher()
{

}

void OpenCVStitcher::setup()
{
	cv::setUseOptimized(true);

	cv::Ptr<cv::kinfu::Params> params = cv::kinfu::Params::defaultParams();
	params->volumeDims(0) = 128;
	params->volumeDims(1) = 128;
	params->volumeDims(2) = 128;
	params->depthFactor = 1000.0;
	depthFactor_ = params->depthFactor;

	dynfu_ = cv::dynafu::DynaFu::create(params);
	//dynfu_ = cv::kinfu::KinFu::create(params);
}

void OpenCVStitcher::addNewImage(const open3d::geometry::Image& colorImg, const open3d::geometry::Image& depthImg)
{
	std::unique_lock<std::mutex> lock(mutex_);

	cv::Mat depth, depthF;
	depth.create(depthImg.height_, depthImg.width_, CV_16UC1);
	std::int_fast16_t* cvPtr = depth.ptr<std::int_fast16_t>(0);
	unsigned char* dataPtr = depthImg.PointerAt<unsigned char>(0, 0, 0);
	memcpy(cvPtr, &(depthImg.data_[0]), 2 * depthImg.height_ * depthImg.width_);

	depth.convertTo(depthF, CV_32F);

	cv::UMat cvt8;
	convertScaleAbs(depthF, cvt8, 0.25 * 256. / depthFactor_);

	bool isOk = dynfu_->update(cvt8);

	if (isOk)
		std::cout << "Registering successful" << std::endl;
	else
		std::cout << "Registering not successful" << std::endl;
}

void OpenCVStitcher::saveVolume()
{
	std::unique_lock<std::mutex> lock(mutex_);

	//cv::Mat3d points, normals;
	cv::Mat points, normals;

	dynfu_->getCloud(points, normals);

	cv::Mat img;
	img.create(512, 512, CV_8UC3);
	dynfu_->render(img);
	cv::imwrite("render.png", img);
}

void OpenCVStitcher::reset()
{
	std::unique_lock<std::mutex> lock(mutex_);

}

open3d::geometry::Image OpenCVStitcher::convertCVToO3D(const cv::Mat& cvimg)
{
	open3d::geometry::Image img;
	if (cvimg.channels() == 3)
	{
		// Color
		cv::Mat tmpI;
		cv::cvtColor(cvimg, tmpI, cv::COLOR_RGB2BGR);
		img.Prepare(tmpI.cols, tmpI.rows, 3, 1);
		unsigned char* dataPtr = img.PointerAt<unsigned char>(0, 0, 0);
		memcpy(dataPtr, tmpI.ptr(), 3 * tmpI.cols * tmpI.rows);

	}
	else
	{
		// Depth
		//double minVal, maxVal;
		//cv::minMaxLoc(cvimg, &minVal, &maxVal);
		//std::cout << "Min: " << minVal << ", max: " << maxVal << std::endl;

		img.Prepare(cvimg.cols, cvimg.rows, 1, 2);
		unsigned char* dataPtr = img.PointerAt<unsigned char>(0, 0, 0);
		memcpy(dataPtr, cvimg.ptr(), 2 * cvimg.cols * cvimg.rows);
	}

	return img;
}


/***************************************************************************
 * Copyright 1998-2017 by authors (see AUTHORS.txt)                        *
 *                                                                         *
 *   This file is part of LuxRender.                                       *
 *                                                                         *
 * 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 <stdexcept>
#include <boost/foreach.hpp>
#include <boost/regex.hpp>

#include "luxrays/kernels/kernels.h"
#include "slg/kernels/kernels.h"
#include "slg/film/film.h"
#include "slg/film/imagepipeline/plugins/mist.h"

using namespace std;
using namespace luxrays;
using namespace slg;

//------------------------------------------------------------------------------
// Mist plugin
//------------------------------------------------------------------------------

BOOST_CLASS_EXPORT_IMPLEMENT(slg::MistPlugin)

MistPlugin::MistPlugin(const luxrays::Spectrum &color, float amount, float start, float end, bool excludeBackground)
	: color(color), amount(amount), start(start), end(end), excludeBackground(excludeBackground)
{}

MistPlugin::MistPlugin()
	: color(1.f), amount(1.f), start(0.f), end(1000.f), excludeBackground(false)
{}

ImagePipelinePlugin *MistPlugin::Copy() const {
	return new MistPlugin(color, amount, start, end, excludeBackground);
}

//------------------------------------------------------------------------------
// CPU version
//------------------------------------------------------------------------------

void MistPlugin::Apply(Film &film, const u_int index) {
	if (!film.HasChannel(Film::DEPTH)) {
		// I can not work without depth channel
		return;
	}

	Spectrum *pixels = (Spectrum *)film.channel_IMAGEPIPELINEs[index]->GetPixels();
	const u_int pixelCount = film.GetWidth() * film.GetHeight();

	// Optimization: invert to avoid division in the loop
	const float rangeInv = 1.f / (end - start);

	#pragma omp parallel for
	for (
			// Visual C++ 2013 supports only OpenMP 2.5
#if _OPENMP >= 200805
			unsigned
#endif
			int i = 0; i < pixelCount; ++i) {
		if (*(film.channel_FRAMEBUFFER_MASK->GetPixel(index))) {
			const float depthValue = *(film.channel_DEPTH->GetPixel(i));
			if(depthValue <= start) {
				continue;
			}

			if(isinf(depthValue)) {
				if(excludeBackground) {
					continue;
				}
				else {
					pixels[i] = Lerp(amount, pixels[i], color);
				}
			}
			else {
				// The use of -3 instead of -1 will cause weight to be 95% at end and then slowly go to 100% towards infinity
				const float weight = 1.f - exp(-3.f * (depthValue - start) * rangeInv);
				pixels[i] = Lerp(weight * amount, pixels[i], color);
			}
		}
	}
}


#include <iostream>
#include <unordered_map>
#include <cstdlib>

#include "corpus/corpus.h"
#include "cpyp/m.h"
#include "cpyp/random.h"
#include "cpyp/crp.h"
#include "cpyp/mf_crp.h"
#include "cpyp/tied_parameter_resampler.h"
#include "uvector.h"
#include "dhpyplm.h"

#include "cpyp/boost_serializers.h"
#include <boost/serialization/vector.hpp>
#include <boost/archive/binary_oarchive.hpp>

// A not very memory-efficient implementation of a domain adapting
// HPYP language model, as described by Wood & Teh (AISTATS, 2009)
//
// I use templates to handle the recursive formalation of the prior, so
// the order of the model has to be specified here, at compile time:
#define kORDER 3

using namespace std;
using namespace cpyp;

Dict dict;

int main(int argc, char** argv) {
  if (argc < 4) {
    cerr << argv[0] << " <training1.txt> <training2.txt> [...] <output.dlm> <nsamples>\n\nInfer a " << kORDER << "-gram HPYP LM and write the trained model\n100 is usually sufficient for <nsamples>\n";
    return 1;
  }
  MT19937 eng;
  vector<string> train_files;
  for (int i = 1; i < argc - 2; ++i)
    train_files.push_back(argv[i]);
  string output_file = argv[argc - 2];
  int samples = atoi(argv[argc - 1]);
  assert(samples > 0);
  {
    ifstream test(output_file);
    if (test.good()) {
      cerr << "File " << output_file << " appears to exist: please remove\n";
      return 1;
    }
  }

  int d = 1;
  for (auto& tf : train_files)
    cerr << (d++==1 ? "  [primary] " : "[secondary] ")
         << "training corpus "<< ": " << tf << endl;
  set<unsigned> vocab;
  const unsigned kSOS = dict.Convert("<s>");
  const unsigned kEOS = dict.Convert("</s>");
  vector<vector<vector<unsigned> > > corpora(train_files.size());
  d = 0;
  for (const auto& train_file : train_files)
    ReadFromFile(train_file, &dict, &corpora[d++], &vocab);

  PYPLM<kORDER> latent_lm(vocab.size(), 1, 1, 1, 1);
  vector<DAPYPLM<kORDER>> dlm(corpora.size(), DAPYPLM<kORDER>(latent_lm)); // domain LMs
  vector<unsigned> ctx(kORDER - 1, kSOS);
  for (int sample=0; sample < samples; ++sample) {
    int ci = 0;
    for (const auto& corpus : corpora) {
      DAPYPLM<kORDER>& lm = dlm[ci];
      ++ci;
      for (const auto& s : corpus) {
        ctx.resize(kORDER - 1);
        for (unsigned i = 0; i <= s.size(); ++i) {
          unsigned w = (i < s.size() ? s[i] : kEOS);
          if (sample > 0) lm.decrement(w, ctx, eng);
          lm.increment(w, ctx, eng);
          ctx.push_back(w);
        }
      }
    }
    if (sample % 10 == 9) {
      double llh = latent_lm.log_likelihood();
      for (auto& lm : dlm) llh += lm.log_likelihood();
      cerr << " [LLH=" << llh << "]\n";
      if (sample % 30u == 29) {
        for (auto& lm : dlm) lm.resample_hyperparameters(eng);
        latent_lm.resample_hyperparameters(eng);
      }
    } else { cerr << '.' << flush; }
  }
  cerr << "Writing LM to " << output_file << " ...\n";
  ofstream ofile(output_file.c_str(), ios::out | ios::binary);
  if (!ofile.good()) {
    cerr << "Failed to open " << output_file << " for writing\n";
    return 1;
  }
  boost::archive::binary_oarchive oa(ofile);
  oa & dict;
  oa & latent_lm;
  unsigned num_domains = dlm.size();
  oa & num_domains;
  for (unsigned i = 0; i < num_domains; ++i)
    oa & dlm[i];
  return 0;
}


#include "ros/ros.h"
#include "std_msgs/String.h"
#include"Eigen/Dense"
#include <vector>
#include <iostream>
#include <QString>
#include <QList>
#include "Robot.h"
//#include"TaskSpace.h"
#include"taskspaceoffline.h"
#include <qmath.h>
#include <cstring>
#include<qdebug.h>
#include <Eigen/Geometry>
#include <cstdlib>
//#include <link.h>
#include "Eigen/eiquadprog.h"
#include "Eigen/Core"
#include "Eigen/Cholesky"
#include "Eigen/LU"
#include<std_msgs/Int32MultiArray.h>
#include<std_msgs/Float32MultiArray.h>
#include<math.h>
#include<sensor_msgs/Imu.h>
#include<std_msgs/Float64.h>
#include "qcgenerator.h"

#include<sensor_msgs/Imu.h>
#include<std_msgs/Float64.h>
#include "pidcontroller.h"
#include<rosgraph_msgs/Clock.h>

#include "std_msgs/String.h"
#include<gazebo_msgs/ApplyBodyWrench.h>

#include<gazebo_msgs/ApplyBodyWrenchRequest.h>
#include <gazebo_msgs/GetLinkState.h>
#include<gazebo_msgs/LinkStates.h>
#include <nav_msgs/Odometry.h>
#include "std_srvs/Empty.h"
#include "qcgenerator.h"
#include "/home/cast/humanoid/surena4/devel/include/xsens_msgs/Internal.h"
#include "/home/cast/humanoid/surena4/devel/include/xsens_msgs/velocityEstimate.h"
#include "/home/cast/humanoid/surena4/devel/include/xsens_msgs/sensorSample.h"
#include "/home/cast/humanoid/surena4/devel/include/xsens_msgs/ImuSensorSample.h"
#include "/home/cast/humanoid/surena4/devel/include/xsens_msgs/orientationEstimate.h"
#include "/home/cast/humanoid/surena4/devel/include/xsens_msgs/positionEstimate.h"
#include "/home/cast/humanoid/surena4/devel/include/xsens_msgs/velocityEstimate.h"
#include "/home/cast/humanoid/surena4/devel/include/xsens_msgs/XsensQuaternion.h"

using namespace  std;
using namespace  Eigen;
//data of left foot sensor

double n_f;
double w_y;
double w_y_f;
double pitch;
double pitch_bias;

vector<double> w_y_s(100);
ros::Publisher pub1  ;
ros::Publisher pub2  ;
ros::Publisher pub3  ;
ros::Publisher pub4  ;
ros::Publisher pub5  ;
ros::Publisher pub6  ;
ros::Publisher pub7  ;
ros::Publisher pub8  ;
ros::Publisher pub9  ;
ros::Publisher pub10 ;
ros::Publisher pub11 ;
ros::Publisher pub12 ;
ros::Publisher pub13 ;
ros::Publisher pub14 ;
ros::Publisher pub15 ;
ros::Publisher pub16 ;
ros::Publisher pub17 ;
ros::Publisher pub18 ;
ros::Publisher pub19 ;
ros::Publisher pub20 ;
ros::Publisher pub21 ;
ros::Publisher pub22 ;
ros::Publisher pub23 ;
ros::Publisher pub24 ;
ros::Publisher pub25 ;
ros::Publisher pub26 ;
ros::Publisher pub27 ;
ros::Publisher pub28 ;

//ros::Publisher pid1 ;

PIDController push_recovery_knee;
PIDController push_recovery_hip;
PIDController push_recovery_ankle;

//void  SendGazebo(QList<LinkM> links){
//    if(links.count()<28){qDebug()<<"index err";return;}
//    std_msgs::Float64 data;
//    data.data=links[1].JointAngle;
//    pub1.publish(data);
//    data.data=links[2].JointAngle;
//    pub2.publish(data);
//    data.data=links[3].JointAngle;
//    pub3.publish(data);
//    data.data=links[4].JointAngle;
//    pub4.publish(data);
//    data.data=links[5].JointAngle;
//    pub5.publish(data);
//    data.data=links[6].JointAngle;
//    pub6.publish(data);
//    data.data=links[7].JointAngle;
//    pub7.publish(data);
//    data.data=links[8].JointAngle;
//    pub8.publish(data);
//    data.data=links[9].JointAngle;
//    pub9.publish(data);
//    data.data=links[10].JointAngle;
//    pub10.publish(data);
//    data.data=links[11].JointAngle;
//    pub11.publish(data);
//    data.data=links[12].JointAngle;
//    pub12.publish(data);
//    data.data=links[13].JointAngle;
//    pub13.publish(data);
//    data.data=links[14].JointAngle;
//    pub14.publish(data);
//    data.data=links[15].JointAngle;
//    pub15.publish(data);
//    data.data=links[16].JointAngle;
//    pub16.publish(data);
//    data.data=links[17].JointAngle;
//    pub17.publish(data);
//    data.data=links[18].JointAngle;
//    pub18.publish(data);
//    data.data=links[19].JointAngle;
//    pub19.publish(data);
//    data.data=links[20].JointAngle;
//    pub20.publish(data);
//    data.data=links[21].JointAngle;
//    pub21.publish(data);
//    data.data=links[22].JointAngle;
//    pub22.publish(data);
//    data.data=links[23].JointAngle;
//    pub23.publish(data);
//    data.data=links[24].JointAngle;
//    pub24.publish(data);
//    data.data=links[25].JointAngle;
//    pub25.publish(data);
//    data.data=links[26].JointAngle;
//    pub26.publish(data);
//    data.data=links[27].JointAngle;
//    pub27.publish(data);
//    data.data=links[28].JointAngle;
//    pub28.publish(data);
//}

void  SendGazebo(vector<double> ctrl){

    std_msgs::Float64 data;
    data.data=ctrl[1];
    pub1.publish(data);
    data.data=ctrl[2];
    pub2.publish(data);
    data.data=ctrl[3];
    pub3.publish(data);
    data.data=ctrl[4];
    pub4.publish(data);
    data.data=ctrl[5];
    pub5.publish(data);
    data.data=ctrl[6];
    pub6.publish(data);
    data.data=ctrl[7];
    pub7.publish(data);
    data.data=ctrl[8];
    pub8.publish(data);
    data.data=ctrl[9];
    pub9.publish(data);
    data.data=ctrl[10];
    pub10.publish(data);
    data.data=ctrl[11];
    pub11.publish(data);
    data.data=ctrl[12];
    pub12.publish(data);
    data.data=0;
    pub13.publish(data);
    data.data=0;
    pub14.publish(data);
    data.data=0;
    pub15.publish(data);
    data.data=0-10*M_PI/180;
    pub16.publish(data);
    data.data=0;
    pub17.publish(data);
    data.data=0;
    pub18.publish(data);
    data.data=0;
    pub19.publish(data);
    data.data=0;
    pub20.publish(data);
    data.data=0;
    pub21.publish(data);
    data.data=0;
    pub22.publish(data);
    data.data=0+10*M_PI/180;
    pub23.publish(data);
    data.data=0;
    pub24.publish(data);
    data.data=0;
    pub25.publish(data);
    data.data=0;
    pub26.publish(data);
    data.data=0;
    pub27.publish(data);
    data.data=0;
    pub28.publish(data);

}

void RecievIMU_w(const sensor_msgs::Imu & msg)
{

    for (int var = n_f-1; var > 0; --var) {
        w_y_s[var]=w_y_s[var-1];
    }
    w_y= msg.angular_velocity.y;
    w_y_s[0]=w_y;
    double sum=0;
    for (int var = 0; var < n_f; ++var) {
        sum=sum+w_y_s[var];
    }
    w_y_f=sum/n_f;
}

void RecievIMU_pitch(const xsens_msgs::orientationEstimate & msg)
{
    pitch=msg.pitch;
}

int main(int argc, char **argv)
{
    n_f=5;
    for (int var = 0; var < n_f; ++var) {
        w_y_s[var]=0;
    }
    vector<double> cntrl(13);
    vector<double> push_state(13);
    QCgenerator QC;
    //check _timesteps
    QElapsedTimer timer;
    Robot SURENA;
    TaskSpaceOffline SURENAOffilneTaskSpace;
    QList<LinkM> links;
    MatrixXd PoseRoot;
    MatrixXd PoseRFoot;
    MatrixXd PoseLFoot;
    double dt;

    double k_ankle=0;
    double k_knee=-.3;
    double k_hip=0;

    double StartTime=0;

    double  DurationOfStartPhase=6;
    double DurationOfPitchBias=.1;
    double DurationOfPushPhase=30;
    double  DurationOfBacktoKnee=2;
    double  DurationOfendPhase=6;
    //SURENAOffilneTaskSpace.GetAccVelPos();
    bool startPhase=true;

    PoseRoot.resize(6,1);
    PoseRFoot.resize(6,1);
    PoseLFoot.resize(6,1);

    //*******************This part of code is for initialization of joints of the robot for walking**********************************
    int count = 0;

    ros::init(argc, argv, "myNode");

    ros::NodeHandle nh;
    ros::Publisher  chatter_pub  = nh.advertise<std_msgs::Int32MultiArray>("jointdata/qc",1000);

    ros::Subscriber  IMU_xsense = nh.subscribe("/mti/sensor/imu", 100, RecievIMU_w);
   // ros::Subscriber  IMU_xsense = nh.subscribe("/mti/filter/orientation", 100, RecievIMU_pitch);

        pub1  = nh.advertise<std_msgs::Float64>("rrbot/joint1_position_controller/command",1000);
        pub2  = nh.advertise<std_msgs::Float64>("rrbot/joint2_position_controller/command",1000);
        pub3  = nh.advertise<std_msgs::Float64>("rrbot/joint3_position_controller/command",1000);
        pub4  = nh.advertise<std_msgs::Float64>("rrbot/joint4_position_controller/command",1000);
        pub5  = nh.advertise<std_msgs::Float64>("rrbot/joint5_position_controller/command",1000);
        pub6  = nh.advertise<std_msgs::Float64>("rrbot/joint6_position_controller/command",1000);
        pub7  = nh.advertise<std_msgs::Float64>("rrbot/joint7_position_controller/command",1000);
        pub8  = nh.advertise<std_msgs::Float64>("rrbot/joint8_position_controller/command",1000);
        pub9  = nh.advertise<std_msgs::Float64>("rrbot/joint9_position_controller/command",1000);
        pub10 = nh.advertise<std_msgs::Float64>("rrbot/joint10_position_controller/command",1000);
        pub11 = nh.advertise<std_msgs::Float64>("rrbot/joint11_position_controller/command",1000);
        pub12 = nh.advertise<std_msgs::Float64>("rrbot/joint12_position_controller/command",1000);
        pub13 = nh.advertise<std_msgs::Float64>("rrbot/joint13_position_controller/command",1000);
        pub14 = nh.advertise<std_msgs::Float64>("rrbot/joint14_position_controller/command",1000);
        pub15 = nh.advertise<std_msgs::Float64>("rrbot/joint15_position_controller/command",1000);
        pub16 = nh.advertise<std_msgs::Float64>("rrbot/joint16_position_controller/command",1000);
        pub17 = nh.advertise<std_msgs::Float64>("rrbot/joint17_position_controller/command",1000);
        pub18 = nh.advertise<std_msgs::Float64>("rrbot/joint18_position_controller/command",1000);
        pub19 = nh.advertise<std_msgs::Float64>("rrbot/joint19_position_controller/command",1000);
        pub20 = nh.advertise<std_msgs::Float64>("rrbot/joint20_position_controller/command",1000);
        pub21 = nh.advertise<std_msgs::Float64>("rrbot/joint21_position_controller/command",1000);
        pub22 = nh.advertise<std_msgs::Float64>("rrbot/joint22_position_controller/command",1000);
        pub23 = nh.advertise<std_msgs::Float64>("rrbot/joint23_position_controller/command",1000);
        pub24 = nh.advertise<std_msgs::Float64>("rrbot/joint24_position_controller/command",1000);
        pub25 = nh.advertise<std_msgs::Float64>("rrbot/joint25_position_controller/command",1000);
        pub26 = nh.advertise<std_msgs::Float64>("rrbot/joint26_position_controller/command",1000);
        pub27 = nh.advertise<std_msgs::Float64>("rrbot/joint27_position_controller/command",1000);
        pub28 = nh.advertise<std_msgs::Float64>("rrbot/joint28_position_controller/command",1000);

    //pid1= nh.advertise<std_msgs::Float64>("rrbot/joint28_position_controller/pid/parameter_updates",1000);
    double p_hip ,i_hip ,d_hip;
    double p_knee ,i_knee, d_knee;
    double p_ankle ,i_ankle ,d_ankle;

    ros::Rate loop_rate(100);
    std_msgs::Int32MultiArray msg;
    std_msgs::MultiArrayDimension msg_dim;

    msg_dim.label = "joint_position";
    msg_dim.size = 1;
    msg.layout.dim.clear();
    msg.layout.dim.push_back(msg_dim);
    pitch_bias=0;

    bool b;
    int bias_count=0;
    bool pitch_bias_ok;
    while (ros::ok())
    {
        // qDebug()<<StartTime;

        //start phase
        if (startPhase==true && StartTime<=DurationOfStartPhase) {
            MinimumJerkInterpolation Coef;
            MatrixXd ZPosition(1,2);
            ZPosition<<0.95100,0.9000;
            MatrixXd ZVelocity(1,2);
            ZVelocity<<0.000,0.000;
            MatrixXd ZAcceleration(1,2);
            ZAcceleration<<0.000,0.000;

            MatrixXd Time(1,2);
            Time<<0,DurationOfStartPhase;
            MatrixXd CoefZStart =Coef.Coefficient(Time,ZPosition,ZVelocity,ZAcceleration);

            //R1=AnkleRollRight
            //R2=AnklePitchRight
            //R3=KneePitchRight
            //R4=HipPitchRight
            //R5=HipRollRight
            //R6=HipYawRight

            //L1=AnkleRollLeft
            //L2=AnklePitchLeft
            //L3=KneePitchLeft
            //L4=HipPitchLeft
            //L5=HipRollLeft
            //L6=HipYawLeft

            //R1=0*(1/2Pi)*(2304)*100  R2=1*-1*(1/2Pi)*(2304)*100   R3=2(1/2Pi)*(2304)*50   R4=3*-1(1/2Pi)*(2304)*80    L2=4*(1/2Pi)*(2304)*100    L1=5*(1/2Pi)*(2304)*100   L3=6*-1*(1/2Pi)*(2304)*50     L4=7*(1/2Pi)*(2304)*80   R6=8*-1*(1/2Pi)*(2304)*120   R5=9*(1/2Pi)*(2304)*120    L5=10*(1/2Pi)*(2304)*120    L6=11*-1*(1/2Pi)*(2304)*120

            //qDebug()<<mm(0,0);
            double zStart=0;
            double yStart=0;
            double xStart=0;
            StartTime=StartTime+SURENAOffilneTaskSpace._timeStep;

            MatrixXd outputZStart= SURENAOffilneTaskSpace.GetAccVelPos(CoefZStart,StartTime,0,5);
            zStart=outputZStart(0,0);

            PoseRoot<<xStart,yStart,zStart,0,0,0;

            PoseRFoot<<0,
                    -0.11500,
                    0.112000,
                    0,
                    0,
                    0;

            PoseLFoot<<0,
                    0.11500,
                    0.11200,
                    0,
                    0,
                    0;

            SURENA.doIK("LLeg_AnkleR_J6",PoseLFoot,"Body", PoseRoot);
            SURENA.doIK("RLeg_AnkleR_J6",PoseRFoot,"Body", PoseRoot);
            links = SURENA.GetLinks();

            cntrl[0]=0.0;
            cntrl[1]=links[1].JointAngle;
            cntrl[2]=links[2].JointAngle;
            cntrl[3]=links[3].JointAngle;
            cntrl[4]=links[4].JointAngle;
            cntrl[5]=links[5].JointAngle;//+teta_motor_R;//pitch
            cntrl[6]=links[6].JointAngle;//roll
            cntrl[7]=links[7].JointAngle;
            cntrl[8]=links[8].JointAngle;
            cntrl[9]=links[9].JointAngle;
            cntrl[10]=links[10].JointAngle;
            cntrl[11]=links[11].JointAngle;
            cntrl[12]=links[12].JointAngle;

            b=true;
            ROS_INFO("wait!");
            // ROS_INFO("%f\t%f\t%f\t%f\t%f\t%f\t",cntrl[3],cntrl[4],cntrl[5],cntrl[9],cntrl[10],cntrl[11]);
            //tested for orientation
//            p_hip=.007;//.007;//.01;//.007;//.005;//.01;(jumping)//.02;(jumping)//008;
//            d_hip=0;//.00000000001;//.02;//.006;//.002;//.001;//0;//0.00006;

            //tested for angular velocity
            p_hip=.02;
            d_hip=0;
            i_hip=.0;

            push_recovery_hip.Init(.01,0.3,-.3,p_hip,d_hip,i_hip);

//            p_knee=.007;//.007;//.007;//.01;//.007;//.005;//.01;(jumping)//.03;(jumping)//.016;
//            d_knee=0;//.00000000001;//.04;//.02;//.01;//.002;//.001;//0;//0.00016;
            p_knee=.04;//.02;
            d_knee=0;
            i_knee=.0;

            push_recovery_knee.Init(.01,0.3,-.3,p_knee,d_knee,i_knee);

            p_ankle=.001;//.005;//.01;(jumping)//.02;(jumping)//.004;
            i_ankle=0.0;
            d_ankle=0;//.002;//.001;//0;//0.00003;
            push_recovery_ankle.Init(.01,0.3,-.3,p_ankle,d_ankle,i_ankle);

        }
        //bias phase
        if( StartTime>DurationOfStartPhase && StartTime<=DurationOfStartPhase+DurationOfPitchBias){
            StartTime=StartTime+SURENAOffilneTaskSpace._timeStep;
            pitch_bias+=pitch;
            bias_count++;
            pitch_bias_ok=true;
        }
        //push phase
        if( StartTime>DurationOfStartPhase+DurationOfPitchBias && StartTime<DurationOfStartPhase+DurationOfPitchBias+DurationOfPushPhase) {
            StartTime=StartTime+SURENAOffilneTaskSpace._timeStep;
            if(pitch_bias_ok){
                pitch_bias/=double(bias_count);
                pitch_bias_ok=false;
            }
            //ROS_INFO("push!\tpitch_bias=%f\tpitch=%f\tpitch-pitch_bias=%f",pitch_bias,pitch,pitch-pitch_bias);
            ROS_INFO("push!\twy=%f",w_y);

//            double p_r_ankle=push_recovery_ankle.Calculate(0,pitch-pitch_bias);
//            double p_r_knee=push_recovery_knee.Calculate(0,pitch-pitch_bias);
//            double p_r_hip=push_recovery_hip.Calculate(0,pitch-pitch_bias);
            double p_r_ankle=push_recovery_ankle.Calculate(0,w_y_f);
            double p_r_knee=push_recovery_knee.Calculate(0,w_y_f);
            double p_r_hip=push_recovery_hip.Calculate(0,w_y_f);
            cntrl[0]=0.0;
            cntrl[1]=links[1].JointAngle;
            cntrl[2]=links[2].JointAngle;
            cntrl[3]=links[3].JointAngle+p_r_hip;//+k_hip*w_y_f;
            cntrl[4]=links[4].JointAngle+p_r_knee;//;//-push_recovery_knee.Calculate(0,w_y_f);//k_knee*w_y_f;
            cntrl[5]=links[5].JointAngle+p_r_ankle;//+k_ankle*w_y_f;
            cntrl[6]=links[6].JointAngle;
            cntrl[7]=links[7].JointAngle;
            cntrl[8]=links[8].JointAngle;
            cntrl[9]=links[9].JointAngle+p_r_hip;//+k_hip*w_y_f;
            cntrl[10]=links[10].JointAngle+p_r_knee;//-push_recovery_knee.Calculate(0,w_y_f);//k_knee*w_y_f;
            cntrl[11]=links[11].JointAngle+p_r_ankle;//+k_ankle*w_y_f;
            cntrl[12]=links[12].JointAngle;
            //            ROS_INFO("pitch = %f",pitch);
            ROS_INFO("w_y=%f\tw_y_filtered=%f\t",w_y,w_y_f);
            ROS_INFO("hip=%f\tkee=%f\tankle=%f\t",p_r_hip,p_r_knee,p_r_ankle);

            ROS_INFO("%f\t%f\t%f\t%f\t%f\t%f\t",cntrl[3],cntrl[4],cntrl[5],cntrl[9],cntrl[10],cntrl[11]);
        }
        //back to knee phase
        if (StartTime>=(DurationOfStartPhase+DurationOfPitchBias+DurationOfPushPhase) && StartTime<=DurationOfBacktoKnee+DurationOfStartPhase+DurationOfPushPhase+DurationOfPitchBias) {
            StartTime=StartTime+SURENAOffilneTaskSpace._timeStep;
            ROS_INFO("STOP pushing!");
            cntrl[0]=0.0;
            cntrl[1]=links[1].JointAngle;
            cntrl[2]=links[2].JointAngle;
            cntrl[3]=links[3].JointAngle;
            cntrl[4]=links[4].JointAngle;
            cntrl[5]=links[5].JointAngle;
            cntrl[6]=links[6].JointAngle;
            cntrl[7]=links[7].JointAngle;
            cntrl[8]=links[8].JointAngle;
            cntrl[9]=links[9].JointAngle;
            cntrl[10]=links[10].JointAngle;
            cntrl[11]=links[11].JointAngle;
            cntrl[12]=links[12].JointAngle;

            ROS_INFO("%f\t%f\t%f\t%f\t%f\t%f\t",cntrl[3],cntrl[4],cntrl[5],cntrl[9],cntrl[10],cntrl[11]);

        }
        //stand up phase
        if (StartTime>=(DurationOfStartPhase+DurationOfPitchBias+DurationOfPushPhase+DurationOfBacktoKnee) && StartTime<=DurationOfBacktoKnee+DurationOfendPhase+DurationOfStartPhase+DurationOfPushPhase+DurationOfPitchBias) {
            MinimumJerkInterpolation Coef;
            MatrixXd ZPosition(1,2);
            ZPosition<<0.90,0.95100;
            MatrixXd ZVelocity(1,2);
            ZVelocity<<0.000,0.000;
            MatrixXd ZAcceleration(1,2);
            ZAcceleration<<0.000,0.000;

            MatrixXd Time(1,2);
            Time<<DurationOfStartPhase+DurationOfPitchBias+DurationOfPushPhase+DurationOfBacktoKnee,DurationOfStartPhase+DurationOfPitchBias+DurationOfPushPhase+DurationOfBacktoKnee+DurationOfendPhase;
            MatrixXd CoefZStart =Coef.Coefficient(Time,ZPosition,ZVelocity,ZAcceleration);

            double zStart=0;
            double yStart=0;
            double xStart=0;
            StartTime=StartTime+SURENAOffilneTaskSpace._timeStep;

            MatrixXd outputZStart= SURENAOffilneTaskSpace.GetAccVelPos(CoefZStart,StartTime,DurationOfStartPhase+DurationOfPitchBias+DurationOfPushPhase+DurationOfBacktoKnee,5);
            zStart=outputZStart(0,0);

            PoseRoot<<xStart,yStart,zStart,0,0,0;

            PoseRFoot<<0,
                    -0.11500,
                    0.112000,
                    0,
                    0,
                    0;

            PoseLFoot<<0,
                    0.11500,
                    0.11200,
                    0,
                    0,
                    0;

            SURENA.doIK("LLeg_AnkleR_J6",PoseLFoot,"Body", PoseRoot);
            SURENA.doIK("RLeg_AnkleR_J6",PoseRFoot,"Body", PoseRoot);

            links = SURENA.GetLinks();

            cntrl[0]=0.0;
            cntrl[1]=links[1].JointAngle;
            cntrl[2]=links[2].JointAngle;
            cntrl[3]=links[3].JointAngle;
            cntrl[4]=links[4].JointAngle;
            cntrl[5]=links[5].JointAngle;//+teta_motor_R;//pitch
            cntrl[6]=links[6].JointAngle;//roll
            cntrl[7]=links[7].JointAngle;
            cntrl[8]=links[8].JointAngle;
            cntrl[9]=links[9].JointAngle;
            cntrl[10]=links[10].JointAngle;
            cntrl[11]=links[11].JointAngle;
            cntrl[12]=links[12].JointAngle;
        }

        vector<int> qref(12);
        qref=QC.ctrldata2qc(cntrl);
        msg.data.clear();

        for(int  i = 0;i < 12;i++)
        {
            msg.data.push_back(qref[i]);
        }
        ROS_INFO("%f\t%f\t%f\t%f\t%f\t%f\t",cntrl[3],cntrl[4],cntrl[5],cntrl[9],cntrl[10],cntrl[11]);

        // std::string varAsString = std::to_string(qref[i-1]);
        // msg.data =varAsString;
        SendGazebo(cntrl);
        // SendGazeboPID();
        chatter_pub.publish(msg);
        //  ROS_INFO("t={%d} c={%d}",timer.elapsed(),count);

        ros::spinOnce();
        loop_rate.sleep();
        ++count;

    }
    return 0;
}


// Copyright (c) 2005 - 2015 Marc de Kamps
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
//
//    * Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
//    * 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.
//    * 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 OWNER 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.
//
#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include <TwoDLib.hpp>
#include <vector>
#include <fstream>
#include <iostream>

#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/io.hpp>

using namespace std;
using namespace TwoDLib;
/*
BOOST_AUTO_TEST_CASE(TransitionMatrixConstructionTest)
{
	TransitionMatrix mat("condee2a5ff4-0087-4d69-bae3-c0a223d03693.mat");
}

BOOST_AUTO_TEST_CASE(SimpleMeshTest){
	try {
		Mesh m("simple.mesh");
		vector<Redistribution> vec_dummy;
		Ode2DSystem sys(m,vec_dummy,vec_dummy);

		TransitionMatrix mat("simple.mat");
		CSRMatrix csrmat(mat,sys);

		vector<double> out{ 0., 0., 0., 0., };
		vector<double> v{ 1.0, 0., 0., 0. };

		csrmat.MV(out,v);

		BOOST_REQUIRE(out[0] == 0.5);
		BOOST_REQUIRE(out[1] == 0.0);
		BOOST_REQUIRE(out[2] == 0.5);
		BOOST_REQUIRE(out[3] == 0.0);

	}
	catch(const TwoDLibException& excep){
		std::cout << excep.what() << std::endl;
	}
}

BOOST_AUTO_TEST_CASE(TransitionMatrixUblasTest)
{
	TransitionMatrix mat("condee2a5ff4-0087-4d69-bae3-c0a223d03693.mat");
	// little odd, but TransitionMatrix does not record the size of the system it has to work on,
	// we get that from the mesh and add one for the stationary point

	Mesh mesh("condee2a5ff4-0087-4d69-bae3-c0a223d03693.mesh");
    using namespace boost::numeric::ublas;

    MPILib::Number n_cells = 1;
    for (MPILib::Index i = 0; i < mesh.NrStrips(); i++)
    	for(MPILib::Index j = 0; j < mesh.NrCellsInStrip(i); j++)
    		++n_cells;

    compressed_matrix<double> m (n_cells, n_cells);
    m(10,10) = 5.;

    typedef compressed_matrix<double>::iterator1 i1_t;
    typedef compressed_matrix<double>::iterator2 i2_t;

#ifdef EXPERIMENT
    for (i1_t i1 = m.begin1(); i1 != m.end1(); ++i1) {
       for (i2_t i2 = i1.begin(); i2 != i1.end(); ++i2){
          cout << "(" << i2.index1() << "," << i2.index2()
               << ":" << *i2 << ")  ";
          cout << endl; }
    }
#endif
}
*/


/*
Lightweight Automated Planning Toolkit
Copyright (C) 2012
Miquel Ramirez <miquel.ramirez@rmit.edu.au>
Nir Lipovetzky <nirlipo@gmail.com>

This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

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
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/
#include <boost/program_options.hpp>
#include <aptk/time.hxx>
#include <strips_prob.hxx>
#include <strips_state.hxx>
#include <ff_to_aptk.hxx>
#include <fwd_search_prob.hxx>
#include <h_2.hxx>
#include <conj_comp_prob.hxx>
#include <h_C.hxx>
#include <rp_heuristic.hxx>

#include <iostream>
#include <fstream>

namespace po = boost::program_options;

using   aptk::STRIPS_Problem;
using	aptk::agnostic::Fwd_Search_Problem;
using   aptk::agnostic::H2_Heuristic;
using	aptk::agnostic::CC_Problem;
using	aptk::agnostic::HC_Heuristic;
using	aptk::agnostic::Relaxed_Plan_Extractor;

void process_command_line_options( int ac, char** av, po::variables_map& vars ) {
	po::options_description desc( "Options:" );

	desc.add_options()
		( "help", "Show help message" )
		( "domain", po::value<std::string>(), "Input PDDL domain description" )
		( "problem", po::value<std::string>(), "Input PDDL problem description" )
	;

	try {
		po::store( po::parse_command_line( ac, av, desc ), vars );
		po::notify( vars );
	}
	catch ( std::exception& e ) {
		std::cerr << "Error: " << e.what() << std::endl;
		std::exit(1);
	}
	catch ( ... ) {
		std::cerr << "Exception of unknown type!" << std::endl;
		std::exit(1);
	}

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

}

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

	po::variables_map vm;

	process_command_line_options( argc, argv, vm );

	if ( !vm.count( "domain" ) ) {
		std::cerr << "No PDDL domain was specified!" << std::endl;
		std::exit(1);
	}

	if ( !vm.count( "problem" ) ) {
		std::cerr << "No PDDL problem was specified!" << std::endl;
		std::exit(1);
	}

	STRIPS_Problem	prob;

	aptk::FF_Parser::get_problem_description( vm["domain"].as<std::string>(), vm["problem"].as<std::string>(), prob );

	std::cout << "PDDL problem description loaded: " << std::endl;
	std::cout << "\tDomain: " << prob.domain_name() << std::endl;
	std::cout << "\tProblem: " << prob.problem_name() << std::endl;
	std::cout << "\t#Actions: " << prob.num_actions() << std::endl;
	std::cout << "\t#Fluents: " << prob.num_fluents() << std::endl;

	Fwd_Search_Problem	search_prob( &prob );
	aptk::State 		s0( prob );
	s0.set( prob.init() );

	float t0 = aptk::time_used();
	H2_Heuristic< Fwd_Search_Problem >  h2( search_prob );
	float tf = aptk::time_used();
	std::cout << "Time to initialize h^2: " << tf - t0 << " secs" << std::endl;

	float h;
	t0 = aptk::time_used();
	h2.eval( s0, h );
	tf = aptk::time_used();
	std::cout << "Time to evaluate h^2: " << tf - t0 << " secs" << std::endl;
	std::cout << "h^2 value = " << h << std::endl;

	std::ofstream	h2_values("h2.values");
	h2.print_values( h2_values );
	h2_values.close();

	t0 = aptk::time_used();
	CC_Problem	pi_2_ce_task( prob, 2 );
	HC_Heuristic<>	h2_ce( pi_2_ce_task );
	tf = aptk::time_used();
	std::cout << "Time to initialize h^{2}_{ce}: " << tf - t0 << " secs" << std::endl;
	std::cout << "# Fluents in \\Pi^{2}_{ce} task: " << pi_2_ce_task.num_fluents() << std::endl;

	t0 = aptk::time_used();
	h2_ce.eval( s0, h );
	tf = aptk::time_used();
	std::cout << "Time to evaluate h^{2}_{ce}: " << tf - t0 << " secs" << std::endl;
	std::cout << "h^{2}_{ce} value = " << h << std::endl;

	std::ofstream	h2_ce_values("h2_ce.values");
	h2_ce.print_values( h2_ce_values );
	h2_ce_values.close();

	t0 = aptk::time_used();
	CC_Problem	pi_1_ce_task( prob, 1);
	HC_Heuristic<>	h1_ce( pi_1_ce_task );
	tf = aptk::time_used();
	std::cout << "Time to initialize h^{1}_{ce}: " << tf - t0 << " secs" << std::endl;
	std::cout << "# Fluents in \\Pi^{1}_{ce} task: " << pi_1_ce_task.num_fluents() << std::endl;

	t0 = aptk::time_used();
	h1_ce.eval( s0, h );
	tf = aptk::time_used();
	std::cout << "Time to evaluate h^{1}_{ce}: " << tf - t0 << " secs" << std::endl;
	std::cout << "h^{1}_{ce} value = " << h << std::endl;

	std::ofstream	h1_ce_values("h1_ce.values");
	h1_ce.print_values( h1_ce_values );
	h1_ce_values.close();

	Relaxed_Plan_Extractor< HC_Heuristic<> >	rp_h2_ce( prob, h2_ce );
	std::vector<aptk::Action_Idx> pref_ops;
	rp_h2_ce.compute( s0, h, pref_ops );

	std::cout << "Relaxed plan h^2_ce heuristic = " << h << std::endl;
	std::cout << "Preferred operators: " << std::endl;
	for ( auto it = pref_ops.begin(); it != pref_ops.end(); it++ )
		std::cout << "\t" << prob.actions()[*it]->signature() << std::endl;

	Relaxed_Plan_Extractor< HC_Heuristic<> >	rp_h1_ce( prob, h1_ce );
	pref_ops.clear();
	rp_h1_ce.compute( s0, h, pref_ops );

	std::cout << "Relaxed plan h^1_ce heuristic = " << h << std::endl;
	std::cout << "Preferred operators: " << std::endl;
	for ( auto it = pref_ops.begin(); it != pref_ops.end(); it++ )
		std::cout << "\t" << prob.actions()[*it]->signature() << std::endl;

	return 0;
}


/*

Copyright (c) 2005-2017, University of Oxford.
All rights reserved.

University of Oxford means the Chancellor, Masters and Scholars of the
University of Oxford, having an administrative office at Wellington
Square, Oxford OX1 2JD, UK.

This file is part of Chaste.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
 * Redistributions of source code must retain the above copyright notice,
   this list of conditions and the following disclaimer.
 * 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.
 * Neither the name of the University of Oxford 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 ABSTRACTODEBASEDCELLCYCLEMODEL_HPP_
#define ABSTRACTODEBASEDCELLCYCLEMODEL_HPP_

#include <vector>

#include "ChasteSerialization.hpp"
#include "ClassIsAbstract.hpp"
#include <boost/serialization/base_object.hpp>

#include "AbstractCellCycleModel.hpp"
#include "CellCycleModelOdeHandler.hpp"
#include "SimulationTime.hpp"

/**
 * This class contains all the functionality shared by 'ODE-based' cell-cycle models,
 * where the duration of the cell cycle is evaluated 'on the fly'
 * as the cell ages, according to a system of ordinary differential equations (ODEs)
 * governing (for example) the concentrations of key intracellular proteins. To
 * determine when cell division should occur, one or more stopping conditions for
 * this ODE system may be specified.
 *
 * This class of cell-cycle models is distinct from 'simple' cell-cycle models, where
 * the duration of the cell cycle is determined when the cell-cycle model is
 * created.
 */
class AbstractOdeBasedCellCycleModel : public AbstractCellCycleModel, public CellCycleModelOdeHandler
{
private:

    /** Needed for serialization. */
    friend class boost::serialization::access;
    /**
     * Archive the cell-cycle model and member variables.
     *
     * @param archive the archive
     * @param version the current version of this class
     */
    template<class Archive>
    void serialize(Archive & archive, const unsigned int version)
    {
        archive & boost::serialization::base_object<AbstractCellCycleModel>(*this);
        archive & boost::serialization::base_object<CellCycleModelOdeHandler>(*this);
        archive & mDivideTime;
    }

protected:

    /** The time at which the cell should divide - Set this to DBL_MAX in constructor. */
    double mDivideTime;

    /**
     * Protected copy-constructor for use by CreateCellCycleModel.
     * The only way for external code to create a copy of a cell cycle model
     * is by calling that method, to ensure that a model of the correct subclass is created.
     * This copy-constructor helps subclasses to ensure that all member variables are correctly copied when this happens.
     *
     * This method is called by child classes to set member variables for a daughter cell upon cell division.
     * Note that the parent cell cycle model will have had ResetForDivision() called just before CreateCellCycleModel() is called,
     * so performing an exact copy of the parent is suitable behaviour. Any daughter-cell-specific initialisation
     * can be done in InitialiseDaughterCell().
     *
     * @param rModel the cell cycle model to copy.
     */
    AbstractOdeBasedCellCycleModel(const AbstractOdeBasedCellCycleModel& rModel);

public:

    /**
     * Creates an AbstractOdeBasedCellCycleModel, calls SetBirthTime on the
     * AbstractPhaseBasedCellCycleModel to make sure that can be set 'back in time' for
     * cells which did not divide at the current time.
     *
     * @param lastTime  The birth time of the cell / last time model was evaluated (defaults to the current SimulationTime)
     * @param pOdeSolver An optional pointer to a cell-cycle model ODE solver object (allows the use of different ODE solvers)
     */
    AbstractOdeBasedCellCycleModel(double lastTime = SimulationTime::Instance()->GetTime(),
                                   boost::shared_ptr<AbstractCellCycleModelOdeSolver> pOdeSolver = boost::shared_ptr<AbstractCellCycleModelOdeSolver>());

    /**
     * Destructor.
     */
    virtual ~AbstractOdeBasedCellCycleModel();

    /**
     * Get the time at which the ODE stopping event occurred.
     * Only called in those subclasses for which stopping events
     * are defined.
     *
     * @return the time at which the ODE system reached its stopping event
     */
    double GetOdeStopTime();

    /**
     * This overrides the AbstractCellCycleModel::SetBirthTime(double birthTime)
     * because an ODE based cell-cycle model has more to reset...
     *
     * @param birthTime the simulation time when the cell was born
     */
    void SetBirthTime(double birthTime);

    /**
     * See AbstractCellCycleModel::ResetForDivision()
     *
     * @return whether the cell is ready to divide (enter M phase).
     */
    virtual bool ReadyToDivide();

    /**
     * For a naturally cycling model this does not need to be overridden in the
     * subclasses. But most models should override this function and then
     * call AbstractOdeBasedCellCycleModel::ResetForDivision() from inside their version.
     */
    virtual void ResetForDivision();

    /**
     * Outputs cell cycle model parameters to file.
     *
     * @param rParamsFile the file stream to which the parameters are output
     */
    virtual void OutputCellCycleModelParameters(out_stream& rParamsFile);
};

CLASS_IS_ABSTRACT(AbstractOdeBasedCellCycleModel)

#endif /*ABSTRACTODEBASDCELLCYCLEMODEL_HPP_*/


/* CirKit: A circuit toolkit
 * Copyright (C) 2009-2015  University of Bremen
 * Copyright (C) 2015-2017  EPFL
 *
 * 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 "exact_synthesis.hpp"

#include <cmath>
#include <fstream>
#include <iostream>
#include <tuple>

#include <boost/assign/std/vector.hpp>
#include <boost/assign/std/set.hpp>
#include <boost/dynamic_bitset.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/format.hpp>
#include <boost/range/algorithm.hpp>
#include <boost/range/irange.hpp>
#include <boost/range/iterator_range.hpp>

#include <core/utils/timer.hpp>
#include <formal/utils/z3_utils.hpp>

#include <reversible/functions/fully_specified.hpp>
#include <reversible/functions/add_gates.hpp>
#include <reversible/functions/copy_metadata.hpp>

#include <c++/z3++.h>

namespace cirkit
{
using namespace boost::assign;

using gate_constraint_fun = std::function<void( z3::context& ctx, z3::solver& solver,
                                                circuit&, std::vector<std::vector<z3::expr>>& network,
                                                std::vector<std::vector<z3::expr> >& gate_values, const binary_truth_table&,
                                                unsigned k, unsigned n )>;

using evaluation_fun = std::function<bool( z3::solver& solver, circuit& circ,
                                           std::vector<std::vector<z3::expr>>& network, unsigned k, unsigned n )>;

using symmetry_fun = std::function<void( z3::context&, z3::solver&, std::vector<std::vector<z3::expr>>&, unsigned)>;

void gate_constraints_original( z3::context& ctx, z3::solver& solver, circuit&,
                                std::vector<std::vector<z3::expr> >& network,
                                std::vector<std::vector<z3::expr> >& gate_values,
                                const binary_truth_table& spec, unsigned k, unsigned n )
{
  using boost::str;
  using boost::format;

  auto rows = boost::distance( spec );

  auto zero = ctx.bv_val(0, n);
  auto one = ctx.bv_val(1, n);

  for ( auto i = 0u; i < k; ++i )
  {
    auto control = ctx.bv_const(str(format("control-%d") % i).c_str(), n);
    auto target = ctx.bv_const(str(format("target-%d") % i).c_str(), n);

    network += std::vector<z3::expr>{ control, target };

    solver.add( (control | (one << target)) != control );
    solver.add( z3::ule(target, ctx.bv_val(n, n)) );

    for ( auto j = 0u; j < rows; ++j )
    {
      auto hit = ctx.bool_const(str(format("hit-%d-%d") % i % j).c_str());
      solver.add( hit == ((gate_values[i][j] & control) == control) );
      solver.add(
          gate_values[i + 1][j]
          == (gate_values[i][j] ^ (ite(hit, one, zero) << target)));
    }
  }
}

void gate_constraints_negative_control( z3::context& ctx, z3::solver& solver,
                                        circuit&, std::vector<std::vector<z3::expr> >& network,
                                        std::vector<std::vector<z3::expr> >& gate_values,
                                        const binary_truth_table& spec, unsigned k, unsigned n )
{
  using boost::str;
  using boost::format;

  auto rows = boost::distance( spec );

  auto zero = ctx.bv_val(0, n);
  auto one = ctx.bv_val(1, n);
  for ( auto i = 0u; i < k; ++i )
  {
    auto control = ctx.bv_const(str(format("control-%d") % i).c_str(), n);
    auto target = ctx.bv_const(str(format("target-%d") % i).c_str(), n);
    auto polarity = ctx.bv_const(str(format("polarity-%d") % i).c_str(), n);

    network += std::vector<z3::expr>{ control, target, polarity };

    solver.add( (control | (one << target)) != control );
    solver.add( z3::ule(target, ctx.bv_val(n, n)) );

    for ( auto j = 0u; j < rows; ++j )
    {
      auto hit = ctx.bool_const(str(format("hit-%d-%d") % i % j).c_str());
      solver.add(
          hit == (((gate_values[i][j] ^ polarity) & control) == control));
      solver.add(
          gate_values[i + 1][j]
          == (gate_values[i][j] ^ (ite(hit, one, zero) << target)));
    }
  }
}

void gate_constraints_negative_control_multiple_target( z3::context& ctx,
                                                        z3::solver& solver, circuit&, std::vector<std::vector<z3::expr> >& gates,
                                                        std::vector<std::vector<z3::expr> >& line_values,
                                                        const binary_truth_table& spec, unsigned k, unsigned n )
{

  using boost::str;
  using boost::format;

  auto rows = boost::distance( spec );

  auto zero = ctx.bv_val(0, n);
  auto one = ctx.bv_val(1, n);
  for ( auto i = 0u; i < k; ++i )
  {
    auto control = ctx.bv_const(str(format("control-%d") % i).c_str(), n);
    auto target = ctx.bv_const(str(format("target-%d") % i).c_str(), n);
    auto polarity = ctx.bv_const(str(format("polarity-%d") % i).c_str(), n);

    gates += std::vector<z3::expr>{ control, target, polarity };

    solver.add((control & target) == zero);

    for ( auto j = 0u; j < rows; ++j )
    {
      auto hit = ctx.bool_const(str(format("hit-%d-%d") % i % j).c_str());
      solver.add(
          hit == (((line_values[i][j] ^ polarity) & control) == control));
      solver.add(
          line_values[i + 1][j]
          == ite(hit, line_values[i][j] ^ target, line_values[i][j]));
    }
  }
}

void gate_constraints_multiple_target( z3::context& ctx, z3::solver& solver,
                                       circuit&, std::vector<std::vector<z3::expr> >& network,
                                       std::vector<std::vector<z3::expr> >& gate_values,
                                       const binary_truth_table& spec, unsigned k, unsigned n )
{
  using boost::str;
  using boost::format;

  auto rows = boost::distance( spec );

  auto zero = ctx.bv_val(0, n);
  auto one = ctx.bv_val(1, n);

  for ( auto i = 0u; i < k; ++i )
  {
    auto control = ctx.bv_const(str(format("control-%d") % i).c_str(), n);
    auto target = ctx.bv_const(str(format("target-%d") % i).c_str(), n);

    network += std::vector<z3::expr>{ control, target };

    solver.add((control & target) == zero);

    for ( auto j = 0u; j < rows; ++j )
    {
      auto hit = ctx.bool_const(str(format("hit-%d-%d") % i % j).c_str());
      solver.add(hit == ((gate_values[i][j] & control) == control));
      solver.add(
          gate_values[i + 1][j]
          == ite(hit, gate_values[i][j] ^ target, gate_values[i][j]));
    }
  }
}

bool eval_original(z3::solver& solver, circuit& circ,
    std::vector<std::vector<z3::expr>>& network, unsigned k, unsigned n)
{
  if (solver.check() == z3::sat)
  {
    z3::model m = solver.get_model();
    for (unsigned i : boost::irange(0u, k))
    {
      auto eval_control = to_bitset(m.eval(network[i][0]));
      auto eval_target = to_bitset(m.eval(network[i][1])).to_ulong();

      gate::control_container controls;
      for (unsigned j : boost::irange(0u, n))
      {
        if (eval_control.test(j))
        {
          controls += make_var(j);
        }
      }

      append_toffoli(circ, controls, eval_target);
    }
    return true;
  } else
  {
    return false;
  }
}

bool eval_multiple_target(z3::solver& solver, circuit& circ,
    std::vector<std::vector<z3::expr>>& gates, unsigned k, unsigned n)
{
  if (solver.check() == z3::sat)
  {
    z3::model m = solver.get_model();

    for (unsigned i : boost::irange(0u, k))
    {
      auto eval_control = to_bitset(m.eval(gates[i][0]));
      auto eval_target = to_bitset(m.eval(gates[i][1]));

      gate::control_container controls;
      for (unsigned j : boost::irange(0u, n))
      {
        if (eval_control.test(j))
        {
          controls += make_var(j);
        }
      }

      gate::target_container targets;
      for (unsigned j : boost::irange(0u, n))
      {
        if (eval_target.test(j))
        {
          targets += j;
        }
      }

      for ( const auto& target : targets )
      {
        append_toffoli(circ, controls, target);
      }
    }

    return true;
  } else
  {
    return false;
  }
}

bool eval_negative_control(z3::solver& solver, circuit& circ,
    std::vector<std::vector<z3::expr>>& gates, unsigned k, unsigned n)
{
  if (solver.check() == z3::sat)
  {
    z3::model m = solver.get_model();

    for (unsigned i : boost::irange(0u, k))
    {
      auto eval_control = to_bitset(m.eval(gates[i][0]));
      auto eval_target = to_bitset(m.eval(gates[i][1])).to_ulong();
      auto eval_polarity = to_bitset(m.eval(gates[i][2]));

      gate::control_container controls;
      for (unsigned j : boost::irange(0u, n))
      {
        if (eval_control.test(j))
        {
          controls += make_var(j, !eval_polarity.test(j));
        }
      }

      append_toffoli(circ, controls, eval_target);
    }

    return true;
  } else
  {
    return false;
  }

}

bool eval_negative_control_multiple_target(z3::solver& solver, circuit& circ,
    std::vector<std::vector<z3::expr>>& gates, unsigned k, unsigned n)
{
  if (solver.check() == z3::sat)
  {
    z3::model m = solver.get_model();

    for (unsigned i : boost::irange(0u, k))
    {
      auto eval_control = to_bitset(m.eval(gates[i][0]));
      auto eval_polarity = to_bitset(m.eval(gates[i][2]));
      auto eval_target = to_bitset(m.eval(gates[i][1]));

//      std::cout << "control: " << eval_control << "\n";
//      std::cout << "polarity: " << eval_polarity << "\n";
//      std::cout << "target: " << eval_target << "\n";

      gate::control_container controls;
      for (unsigned j : boost::irange(0u, n))
      {
        if (eval_control.test(j))
        {
          controls += make_var(j, !eval_polarity.test(j));
        }
      }

      gate::target_container targets;
      for (unsigned j : boost::irange(0u, n))
      {
        if (eval_target.test(j))
        {
//          std::cout <<"j: " <<  j << "\n";
          targets += j;
        }
      }

      for ( const auto& target : targets )
      {
        append_toffoli(circ, controls, target);
      }
    }

    return true;
  } else
  {
    return false;
  }
}

void input_output_constraints(z3::context& ctx, z3::solver solver,
    const binary_truth_table& spec,
    std::vector<std::vector<z3::expr> >& gate_values, unsigned k, unsigned n)
{
// Constraints for inputs and outputs. This is
  for (binary_truth_table::const_iterator iter = spec.begin();
      iter != spec.end(); ++iter)
  {
    boost::dynamic_bitset<> in(n);
    boost::dynamic_bitset<> mask(n);
    boost::dynamic_bitset<> out(n);

    unsigned bitpos = 0;
    for (const auto& bit : boost::make_iterator_range(iter->first))
    {
      in.set(bitpos++, *bit);
    }

    bitpos = 0;
    for (const auto& bit : boost::make_iterator_range(iter->second))
    {
      mask.set(bitpos, (bool)bit);
      out.set(bitpos++, (bool)bit && *bit);
    }
    unsigned pos = std::distance<binary_truth_table::const_iterator>(
        spec.begin(), iter);
    solver.add(gate_values[0][pos] == ctx.bv_val((__uint64) in.to_ulong(), n));
    solver.add(
        (gate_values[k][pos] & ctx.bv_val((__uint64) mask.to_ulong(), n))
            == ctx.bv_val((__uint64) out.to_ulong(), n));
  }
}

/******************************************************************************
 * symmetry breaking                                                          *
 ******************************************************************************/

void symmetry_breaking_no_double_gate( z3::context& ctx, z3::solver& solver, const std::vector<std::vector<z3::expr>>& network, unsigned n )
{
  if ( network.size() >= 2 )
  {
    for ( auto i = 0u; i < network.size() - 1; ++i )
    {
      solver.add( !( ( network[i][0] == network[i + 1][0] ) && ( network[i][1] == network[i + 1][1] ) ) );
    }
  }
}

void symmetry_breaking_colexicographic( z3::context& ctx, z3::solver& solver, const std::vector<std::vector<z3::expr>>& network, unsigned n )
{
  const auto one = ctx.bv_val( 1u, n );
  const auto zero = ctx.bv_val( 0u, n );

  if ( network.size() >= 2 )
  {
    for ( auto i = 0u; i < network.size() - 1; ++i )
    {
      const z3::expr move1 = ( ( one << network[i][0] ) & network[i + 1][1] ) == zero;
      const z3::expr move2 = ( network[i][1] & ( one << network[i + 1][0] ) ) == zero;

      const z3::expr cless = z3::ule( network[i][0], network[i + 1][0] );
      const z3::expr ceq   = ( network[i][0] == network[i + 1][0] ) && ( z3::ule( network[i][1], network[i + 1][1] ) );

      solver.add( implies( move1 && move2, cless || ceq ) );
    }
  }
}

void symmetry_breaking_one_not_per_line( z3::context& ctx, z3::solver& solver, const std::vector<std::vector<z3::expr>>& network, unsigned n )
{
  const auto zero = ctx.bv_val( 0u, n );

  for ( auto i = 0u; i < network.size(); ++i )
  {
    for ( auto j = i + 1; j < network.size(); ++j )
    {
      /* if some gate i has no controls (i.e., NOT), then there can't be another on that line */
      solver.add( implies( network[i][0] == zero && network[i][1] == network[j][1], network[j][0] != zero ) );
    }
  }
}

bool synth_len(circuit& circ, const binary_truth_table& spec,
               gate_constraint_fun gate_constraints,
               evaluation_fun eval, const std::vector<symmetry_fun>& symmetry_breaking, bool only_toffoli, unsigned gate_count)
{
  using boost::str;
  using boost::format;

  unsigned n = spec.num_inputs();
  unsigned rows = std::distance(spec.begin(), spec.end());

  z3::context ctx;
  z3::solver solver(ctx);

  // Instead of having vector<pair<exp,exp>> we use vector<vector<exp>>. This
  // allows for more flexibility. The inner vector should have a length of
  // 2 or 3.
  // In case of len 2, the first entry contains the controls, the second the targets
  // In case of len 3, the last entry contains the polarity of the controls
  std::vector<std::vector<z3::expr> > network;

  // this vector of vectores stores, for every input pattern (outer vector index), the
  // line values after the k'th gate (inner vector index)
  std::vector<std::vector<z3::expr> > gate_values;

  // initialize the gate values
  for (int i : boost::irange(0u, gate_count + 1u))
  {
    gate_values += std::vector<z3::expr>();
    for (int j : boost::irange(0u, rows))
    {
      gate_values[i] += ctx.bv_const(
          str(format("gate-value-%d-%d") % i % j).c_str(), n);
    }
  }

  input_output_constraints(ctx, solver, spec, gate_values, gate_count, n);

  gate_constraints(ctx, solver, circ, network, gate_values, spec, gate_count,
      n);

  for ( const auto& f : symmetry_breaking )
  {
    f( ctx, solver, network, n );
  }

  if ( only_toffoli )
  {
    for ( auto i = 0u; i < gate_count; ++i )
    {
      auto constr = ( network[i][0] == ctx.bv_val( 0u, n ) );
      for ( auto j = 0u; j < n; ++j )
      {
        auto mask = 1 << j;
        constr = constr || ( network[i][0] == ctx.bv_val( mask, n ) );

        for ( auto k = j + 1; k < n; ++k )
        {
          constr = constr || ( network[i][0] == ctx.bv_val( mask | ( 1 << k ), n ) );
        }
      }
      solver.add( constr );
    }
  }

  return eval(solver, circ, network, gate_count, n);
}

/******************************************************************************
 * public functions                                                           *
 ******************************************************************************/

bool exact_synthesis(circuit& circ, const binary_truth_table& spec,
    properties::ptr settings, properties::ptr statistics)
{
  const auto start_depth  = get( settings, "start_depth",  0u );
  const auto max_depth    = get( settings, "max_depth",    20u );
  const auto negative     = get( settings, "negative",     false );
  const auto multiple     = get( settings, "multiple",     false );
  const auto only_toffoli = get( settings, "only_toffoli", false );
  const auto verbose      = get( settings, "verbose",      false );

  properties_timer t( statistics );

  circ.set_lines(spec.num_inputs());

  unsigned gate_count = start_depth;
  bool result = false;

  gate_constraint_fun gate_constraints = &gate_constraints_original;
  evaluation_fun eval = &eval_original;
  std::vector<symmetry_fun> symmetry_breaking;

  symmetry_breaking.push_back( symmetry_breaking_no_double_gate );
  if (negative && multiple)
  {
    gate_constraints = &gate_constraints_negative_control_multiple_target;
    eval = &eval_negative_control_multiple_target;
    symmetry_breaking.push_back( symmetry_breaking_one_not_per_line );
  }
  else
  {
    //symmetry_breaking.push_back( symmetry_breaking_colexicographic );
    if (negative)
    {
      gate_constraints = &gate_constraints_negative_control;
      eval = &eval_negative_control;
      symmetry_breaking.push_back( symmetry_breaking_one_not_per_line );
    }
    if (multiple)
    {
      gate_constraints = &gate_constraints_multiple_target;
      eval = &eval_multiple_target;
    }
  }

  do
  {
    if ( verbose )
    {
      std::cout << "[i] check for depth " << gate_count << std::endl;
    }
    result = synth_len( circ, spec, gate_constraints, eval, symmetry_breaking, only_toffoli, gate_count );
  } while (!result && gate_count++ < max_depth);

  if (result)
  {
    copy_metadata(spec, circ);
  } else
  {
    set_error_message(statistics,
        "Could not find a circuit within the predefined depth.");
  }

  return result;
}

truth_table_synthesis_func exact_synthesis_func(properties::ptr settings,
    properties::ptr statistics)
{
  truth_table_synthesis_func f =
      [&settings, &statistics]( circuit& circ, const binary_truth_table& spec )
      {
        return exact_synthesis( circ, spec, settings, statistics );
      };
  f.init(settings, statistics);
  return f;
}

}

// Local Variables:
// c-basic-offset: 2
// eval: (c-set-offset 'substatement-open 0)
// eval: (c-set-offset 'innamespace 0)
// End:


#include <iostream>
#include <fstream>
#include <iomanip>
#include <string>
#include <boost/program_options.hpp>

#include "msa.hpp"
#include "model.hpp"
#include "OptParam.hpp"
#include "PartFunc.hpp"
using namespace std;

namespace po = boost::program_options;

int main(int argc, char *argv[])
{
  po::options_description desc("Allowed options");
  desc.add_options()
    ("help,h","produce help message")
    ("in,i", po::value<string>(), "set input file (multiple FASTA)")
    ("default,d", po::value<bool>(), "Set default (NULL) sequence for training")
    ("ipair", po::value<string>(), "interacting pairs of reference sequence")
    ("rmap", po::value<string>(), "HMMer3 alignment of reference sequence")
    ("param,p", po::value<string>(), "input LGM parameter file")
    ("alpha", po::value<double>(), "alpha (for update)")
    ("niter,n", po::value<int>(), "number of iterations")
    ("logbase,l", po::value<string>(), "basename for log files")
    ;

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

  if(vm.count("help")) {
    cerr << desc << endl;
    return 1;
  }
  string logbase = "exJ";
  if(vm.count("logbase")) {
    logbase = vm["logbase"].as<string>();
  }
  cerr << "# LOGBASE= " << logbase << endl;
  if(!vm.count("in")) {
    cerr << "Specify the input MSA!" << endl;
    exit(1);
  }

  cerr << "Input file was set to " << vm["in"].as<string>() << endl;
  string infile = vm["in"].as<string>();
  Msa msa(infile);
  Model model(msa);
  if(vm.count("param")) {
    model.read_parameters(vm["param"].as<string>());
    cerr << "Setting parameters from " << vm["param"].as<string>() << endl;
  }

  if(vm.count("default")) {
    if(vm["default"].as<bool>()) {
      //      model.set_null_sequence();
      model.set_uniform_sequence();
      cerr << "Setting default NULL sequence for training" << endl;
    }
  }

  if(vm.count("ipair")) {
    cerr << "#reading ipair " << (vm["ipair"].as<string>()) << endl;
    model.read_ipairs(vm["ipair"].as<string>());
    if(vm.count("rmap")) {
      cerr << "#reading rmap " << (vm["rmap"].as<string>()) << endl;
      model.read_reference_3Dalignment(vm["rmap"].as<string>());
    } else {
      cerr << "Give --rmap for 3D contact mapping!" << endl;
      exit(2);
    }
    model.init_KOO_corr();
  } else {
    cerr << "Setting NO ipairs." << endl;
  }

  PartFunc pfunc(model);
  double alpha = 0.1;
  if(vm.count("alpha")) {
    alpha = vm["alpha"].as<double>();
  }
  cerr << "alpha= " << alpha << endl;
  int niter = 300;
  if(vm.count("niter")) {
    niter = vm["niter"].as<int>();
  }
  cerr << "Optimizing for " << niter << " iterations." << endl;
  pfunc.learn_J(model, niter, alpha);
  model.write_parameters(logbase + ".param");

  cerr << "energy(obs)= " << model.get_energy() << endl;
  cerr << "energy(exp)= " << model.get_energy_exp() << endl;

  cerr << "max adel (site) " << model.check_site(false) << endl;
  cerr << "max adel (bonded) " << model.check_bonded(false) << endl;
  cerr << "max adel (nonbonded) " << model.check_nonbonded(false) << endl;

  return 0;
}


/*=============================================================================
    Copyright (c) 2009 Christopher Schmidt

    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_FUSION_VIEW_REVERSE_VIEW_DETAIL_VALUE_AT_IMPL_HPP
#define BOOST_FUSION_VIEW_REVERSE_VIEW_DETAIL_VALUE_AT_IMPL_HPP

#include <boost/fusion/sequence/intrinsic/value_at.hpp>
#include <boost/mpl/minus.hpp>
#include <boost/mpl/int.hpp>

namespace boost { namespace fusion { namespace extension
{
    template <typename>
    struct value_at_impl;

    template <>
    struct value_at_impl<reverse_view_tag>
    {
        template <typename Seq, typename N>
        struct apply
          : result_of::value_at<
                typename Seq::seq_type
              , mpl::minus<typename Seq::size, mpl::int_<1>, N>
            >
        {};
    };
}}}

#endif


/*
 * Copyright (c) 2019 Samsung Electronics Co., Ltd. All Rights Reserved
 *
 * 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 <algorithm>
#include <atomic>
#include <chrono>
#include <forward_list>
#include <fstream>
#include <iostream>
#include <memory>
#include <numeric>
#include <stdexcept>
#include <string>
#include <thread>

#include <boost/filesystem.hpp>
#include <boost/format.hpp>
#include <boost/program_options.hpp>

#include <cmath>
#include <cstdint>
#include <signal.h>

#include <tensorflow/lite/context.h>
#include <tensorflow/lite/interpreter.h>
#include <tensorflow/lite/model.h>

#include "labels.h"
#include "tflite/ext/nnapi_delegate.h"
#include "tflite/ext/kernels/register.h"

const std::string kDefaultImagesDir = "res/input/";
const std::string kDefaultModelFile = "res/model.tflite";

template <typename... Args> void Print(const char *fmt, Args... args)
{
#if __cplusplus >= 201703L
  std::cerr << boost::str(boost::format(fmt) % ... % std::forward<Args>(args)) << std::endl;
#else
  boost::format f(fmt);
  using unroll = int[];
  unroll{0, (f % std::forward<Args>(args), 0)...};
  std::cerr << boost::str(f) << std::endl;
#endif
}

template <typename DataType> struct BaseLabelData
{
  explicit BaseLabelData(int label = -1, DataType confidence = 0)
    : label(label), confidence(confidence)
  {
  }

  static std::vector<BaseLabelData<DataType>> FindLabels(const DataType *output_tensor,
                                                         unsigned int top_n = 5)
  {
    top_n = top_n > 1000 ? 1000 : top_n;
    size_t n = 0;
    std::vector<size_t> indices(1000);
    std::generate(indices.begin(), indices.end(), [&n]() { return n++; });
    std::sort(indices.begin(), indices.end(), [output_tensor](const size_t &i1, const size_t &i2) {
      return output_tensor[i1] > output_tensor[i2];
    });
    std::vector<BaseLabelData<DataType>> results(top_n);
    for (unsigned int i = 0; i < top_n; ++i)
    {
      results[i].label = indices[i];
      results[i].confidence = output_tensor[indices[i]];
    }
    return results;
  }

  int label;
  DataType confidence;
};

class BaseRunner
{
public:
  virtual ~BaseRunner() = default;

  /**
   * @brief Run a model for each file in a directory, and collect and print
   * statistics.
   */
  virtual void IterateInDirectory(const std::string &dir_path, const int labels_offset) = 0;

  /**
   * @brief Request that the iteration be stopped after the current file.
   */
  virtual void ScheduleInterruption() = 0;
};

template <typename DataType_> class Runner : public BaseRunner
{
public:
  using DataType = DataType_;
  using LabelData = BaseLabelData<DataType>;

  const int kInputSize;
  const int KOutputSize = 1001 * sizeof(DataType);

  Runner(std::unique_ptr<tflite::Interpreter> interpreter,
         std::unique_ptr<tflite::FlatBufferModel> model,
         std::unique_ptr<::nnfw::tflite::NNAPIDelegate> delegate, unsigned img_size)
    : interpreter(std::move(interpreter)), model(std::move(model)), delegate(std::move(delegate)),
      interrupted(false), kInputSize(1 * img_size * img_size * 3 * sizeof(DataType))
  {
    inference_times.reserve(500);
    top1.reserve(500);
    top5.reserve(500);
  }

  virtual ~Runner() = default;

  /**
   * @brief Get the model's input tensor.
   */
  virtual DataType *GetInputTensor() = 0;

  /**
   * @brief Get the model's output tensor.
   */
  virtual DataType *GetOutputTensor() = 0;

  /**
   * @brief Load Image file into tensor.
   * @return Class number if present in filename, -1 otherwise.
   */
  virtual int LoadFile(const boost::filesystem::path &input_file)
  {
    DataType *input_tensor = GetInputTensor();
    if (input_file.extension() == ".bin")
    {
      // Load data as raw tensor
      std::ifstream input_stream(input_file.string(), std::ifstream::binary);
      input_stream.read(reinterpret_cast<char *>(input_tensor), kInputSize);
      input_stream.close();
      int class_num = boost::lexical_cast<int>(input_file.filename().string().substr(0, 4));
      return class_num;
    }
    else
    {
      // Load data as image file
      throw std::runtime_error("Runner can only load *.bin files");
    }
  }

  void Invoke()
  {
    TfLiteStatus status;
    if (delegate)
    {
      status = delegate->Invoke(interpreter.get());
    }
    else
    {
      status = interpreter->Invoke();
    }
    if (status != kTfLiteOk)
    {
      throw std::runtime_error("Failed to invoke interpreter.");
    }
  }

  int Process()
  {
    auto t0 = std::chrono::high_resolution_clock::now();
    Invoke();
    auto t1 = std::chrono::high_resolution_clock::now();
    std::chrono::duration<double> fs = t1 - t0;
    auto d = std::chrono::duration_cast<std::chrono::milliseconds>(fs);
    inference_times.push_back(d.count());
    if (d > std::chrono::milliseconds(10))
    {
      Print("  -- inference duration: %lld ms", d.count());
    }
    else
    {
      auto du = std::chrono::duration_cast<std::chrono::microseconds>(fs);
      Print("  -- inference duration: %lld us", du.count());
    }
    return 0;
  }

  void DumpOutputTensor(const std::string &output_file)
  {
    DataType *output_tensor = GetOutputTensor();
    std::ofstream output_stream(output_file, std::ofstream::binary);
    output_stream.write(reinterpret_cast<char *>(output_tensor), KOutputSize);
  }

  void PrintExecutionSummary() const
  {
    Print("Execution summary:");
    Print("  -- # of processed images: %d", num_images);
    if (num_images == 0)
    {
      return;
    }
    // Inference time - mean
    double mean = std::accumulate(inference_times.begin(), inference_times.end(), 0.0) / num_images;
    Print("  -- mean inference time: %.1f ms", mean);
    // Inference time - std
    std::vector<double> diff(num_images);
    std::transform(inference_times.begin(), inference_times.end(), diff.begin(),
                   [mean](size_t n) { return n - mean; });
    double sq_sum = std::inner_product(diff.begin(), diff.end(), diff.begin(), 0.0);
    double std_inference_time = std::sqrt(sq_sum / num_images);
    Print("  -- std inference time: %.1f ms", std_inference_time);
    // Top-1 and Top-5 accuracies
    float num_top1 = std::accumulate(top1.begin(), top1.end(), 0);
    float num_top5 = std::accumulate(top5.begin(), top5.end(), 0);
    Print("  -- top1: %.3f, top5: %.3f", num_top1 / num_images, num_top5 / num_images);
  }

  virtual void ScheduleInterruption() override { interrupted = true; }

  virtual void IterateInDirectory(const std::string &dir_path, const int labels_offset) override
  {
    interrupted = false;
    namespace fs = boost::filesystem;
    if (!fs::is_directory(dir_path))
    {
      throw std::runtime_error("Could not open input directory.");
    }

    inference_times.clear();
    top1.clear();
    top5.clear();
    int class_num;
    num_images = 0;
    std::vector<LabelData> lds;
    fs::directory_iterator end;
    for (auto it = fs::directory_iterator(dir_path); it != end; ++it)
    {
      if (interrupted)
      {
        break;
      }
      if (!fs::is_regular_file(*it))
      {
        continue;
      }
      Print("File : %s", it->path().string());
      try
      {
        class_num = LoadFile(*it) + labels_offset;
        Print("Class: %d", class_num);
      }
      catch (std::exception &e)
      {
        Print("%s", e.what());
        continue;
      }
      int status = Process();
      if (status == 0)
      {
        DataType *output_tensor = GetOutputTensor();
        lds = LabelData::FindLabels(output_tensor, 5);
        bool is_top1 = lds[0].label == class_num;
        bool is_top5 = false;
        for (const auto &ld : lds)
        {
          is_top5 = is_top5 || (ld.label == class_num);
          Print("  -- label: %s (%d), prob: %.3f", ld.label >= 0 ? labels[ld.label] : "", ld.label,
                static_cast<float>(ld.confidence));
        }
        Print("  -- top1: %d, top5: %d", is_top1, is_top5);
        top1.push_back(is_top1);
        top5.push_back(is_top5);
      }
      ++num_images;
    }
    PrintExecutionSummary();
  }

protected:
  std::unique_ptr<tflite::Interpreter> interpreter;
  std::unique_ptr<tflite::FlatBufferModel> model;
  std::unique_ptr<::nnfw::tflite::NNAPIDelegate> delegate;

  std::vector<size_t> inference_times;
  std::vector<bool> top1;
  std::vector<bool> top5;
  uint num_images;
  std::atomic_bool interrupted;
};

class FloatRunner : public Runner<float>
{
public:
  using Runner<float>::DataType;

  FloatRunner(std::unique_ptr<tflite::Interpreter> interpreter,
              std::unique_ptr<tflite::FlatBufferModel> model,
              std::unique_ptr<::nnfw::tflite::NNAPIDelegate> delegate, unsigned img_size)
    : Runner<float>(std::move(interpreter), std::move(model), std::move(delegate), img_size)
  {
  }

  virtual ~FloatRunner() = default;

  virtual DataType *GetInputTensor() override
  {
    return interpreter->tensor(interpreter->inputs()[0])->data.f;
  }

  virtual DataType *GetOutputTensor() override
  {
    return interpreter->tensor(interpreter->outputs()[0])->data.f;
  }
};

class QuantizedRunner : public Runner<uint8_t>
{
public:
  using Runner<uint8_t>::DataType;

  QuantizedRunner(std::unique_ptr<tflite::Interpreter> interpreter,
                  std::unique_ptr<tflite::FlatBufferModel> model,
                  std::unique_ptr<::nnfw::tflite::NNAPIDelegate> delegate, unsigned img_size)
    : Runner<uint8_t>(std::move(interpreter), std::move(model), std::move(delegate), img_size)
  {
  }

  virtual ~QuantizedRunner() = default;

  virtual DataType *GetInputTensor() override
  {
    return interpreter->tensor(interpreter->inputs()[0])->data.uint8;
  }

  virtual DataType *GetOutputTensor() override
  {
    return interpreter->tensor(interpreter->outputs()[0])->data.uint8;
  }
};

enum class Target
{
  TfLiteCpu,      /**< Use Tensorflow Lite's CPU kernels. */
  TfLiteDelegate, /**< Use Tensorflow Lite's NN API delegate. */
  NnfwDelegate    /**< Use NNFW's NN API delegate. */
};

std::unique_ptr<BaseRunner> MakeRunner(const std::string &model_path, unsigned img_size,
                                       Target target = Target::NnfwDelegate)
{
  auto model = tflite::FlatBufferModel::BuildFromFile(model_path.c_str());
  if (not model)
  {
    throw std::runtime_error(model_path + ": file not found or corrupted.");
  }
  Print("Model loaded.");

  std::unique_ptr<tflite::Interpreter> interpreter;
  nnfw::tflite::BuiltinOpResolver resolver;
  tflite::InterpreterBuilder(*model, resolver)(&interpreter);
  if (not interpreter)
  {
    throw std::runtime_error("interpreter construction failed.");
  }
  if (target == Target::TfLiteCpu)
  {
    interpreter->SetNumThreads(std::max(std::thread::hardware_concurrency(), 1U));
  }
  else
  {
    interpreter->SetNumThreads(1);
  }
  if (target == Target::TfLiteDelegate)
  {
    interpreter->UseNNAPI(true);
  }

  int input_index = interpreter->inputs()[0];
  interpreter->ResizeInputTensor(input_index,
                                 {1, static_cast<int>(img_size), static_cast<int>(img_size), 3});
  if (interpreter->AllocateTensors() != kTfLiteOk)
  {
    throw std::runtime_error("tensor allocation failed.");
  }

  if (target == Target::TfLiteDelegate)
  {
    // Do a fake run to load NN API functions.
    interpreter->Invoke();
  }

  std::unique_ptr<::nnfw::tflite::NNAPIDelegate> delegate;
  if (target == Target::NnfwDelegate)
  {
    delegate.reset(new ::nnfw::tflite::NNAPIDelegate);
    delegate->BuildGraph(&(interpreter.get()->primary_subgraph()));
  }

  if (interpreter->tensor(input_index)->type == kTfLiteFloat32)
  {
    return std::unique_ptr<FloatRunner>(
      new FloatRunner(std::move(interpreter), std::move(model), std::move(delegate), img_size));
  }
  else if (interpreter->tensor(input_index)->type == kTfLiteUInt8)
  {
    return std::unique_ptr<QuantizedRunner>(
      new QuantizedRunner(std::move(interpreter), std::move(model), std::move(delegate), img_size));
  }
  throw std::invalid_argument("data type of model's input tensor is not supported.");
}

Target GetTarget(const std::string &str)
{
  static const std::map<std::string, Target> target_names{
    {"tflite-cpu", Target::TfLiteCpu},
    {"tflite-delegate", Target::TfLiteDelegate},
    {"nnfw-delegate", Target::NnfwDelegate}};
  if (target_names.find(str) == target_names.end())
  {
    throw std::invalid_argument(
      str + ": invalid target. Run with --help for a list of available targets.");
  }
  return target_names.at(str);
}

// We need a global pointer to the runner for the SIGINT handler
BaseRunner *runner_ptr = nullptr;
void HandleSigInt(int)
{
  if (runner_ptr != nullptr)
  {
    Print("Interrupted. Execution will stop after current image.");
    runner_ptr->ScheduleInterruption();
    runner_ptr = nullptr;
  }
  else
  {
    exit(1);
  }
}

int main(int argc, char *argv[])
try
{
  namespace po = boost::program_options;
  po::options_description desc("Run a model on multiple binary images and print"
                               " statistics");
  // clang-format off
  desc.add_options()
    ("help", "print this message and quit")
    ("model", po::value<std::string>()->default_value(kDefaultModelFile), "tflite file")
    ("input", po::value<std::string>()->default_value(kDefaultImagesDir), "directory with input images")
    ("offset", po::value<int>()->default_value(1), "labels offset")
    ("target", po::value<std::string>()->default_value("nnfw-delegate"),
      "how the model will be run (available targets: tflite-cpu, tflite-delegate, nnfw-delegate)")
    ("imgsize", po::value<unsigned>()->default_value(224), "the width and height of the image");
  // clang-fomrat on
  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, desc), vm);
  if (vm.count("help"))
  {
    std::cerr << desc << std::endl;
    return 0;
  }

  auto runner = MakeRunner(vm["model"].as<std::string>(), vm["imgsize"].as<unsigned>(),
                           GetTarget(vm["target"].as<std::string>()));
  runner_ptr = runner.get();

  struct sigaction sigint_handler;
  sigint_handler.sa_handler = HandleSigInt;
  sigemptyset(&sigint_handler.sa_mask);
  sigint_handler.sa_flags = 0;
  sigaction(SIGINT, &sigint_handler, nullptr);

  Print("Running TensorFlow Lite...");
  runner->IterateInDirectory(vm["input"].as<std::string>(), vm["offset"].as<int>());
  Print("Done.");
  return 0;
}
catch (std::exception &e)
{
  Print("%s: %s", argv[0], e.what());
  return 1;
}