/*
 Copyright (C) 2016 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

#include <boost/make_shared.hpp>
#include <ored/portfolio/builders/fxforward.hpp>
#include <ored/portfolio/enginefactory.hpp>
#include <ored/portfolio/fxforward.hpp>
#include <ored/utilities/log.hpp>
#include <ored/utilities/parsers.hpp>
#include <ored/utilities/xmlutils.hpp>
#include <ql/cashflows/simplecashflow.hpp>
#include <qle/instruments/fxforward.hpp>

namespace ore {
namespace data {

void FxForward::build(const boost::shared_ptr<EngineFactory>& engineFactory) {
    // If you Buy EURUSD forward, then you buy EUR and sell USD.
    // EUR = foreign, USD = Domestic.
    // You pay in USD, so the Domestic / Sold ccy is the "payer" currency
    Currency boughtCcy = parseCurrency(boughtCurrency_);
    Currency soldCcy = parseCurrency(soldCurrency_);
    Date maturityDate = parseDate(maturityDate_);

    // Derive settlement date from payment data parameters
    Date payDate;
    if (payDate_.empty()) {
        //LOG("Attempting paydate advance");
        Period payLag = payLag_.empty() ? 0 * Days : parsePeriod(payLag_);
        Calendar payCalendar = payCalendar_.empty() ? NullCalendar() : parseCalendar(payCalendar_);
        BusinessDayConvention payConvention =
            payConvention_.empty() ? Unadjusted : parseBusinessDayConvention(payConvention_);
        payDate = payCalendar.advance(maturityDate, payLag, payConvention);
    } else {
        payDate = parseDate(payDate_);
        QL_REQUIRE(payDate >= maturityDate, "FX Forward settlement date should equal or exceed the maturity date.");
    }

    // Add required FX fixing for Cash settlement. If fixingDate == payDate, no fixing is actually required.
    // Currently, fxIndexCalendar_ is "" (NullCalendar) and fxIndexDays_ is 0, so fixingDate = maturityDate always.
    Date fixingDate;
    Calendar fxIndexCalendar = fxIndexCalendar_.empty() ? NullCalendar() : parseCalendar(fxIndexCalendar_);
    fixingDate = fxIndexCalendar.advance(maturityDate, -static_cast<Integer>(fxIndexDays_), Days);

    boost::shared_ptr<QuantExt::FxIndex> fxIndex;
    Currency payCcy;

    if (payCurrency_.empty()) {
        // If settlement currency is not set, set it to the domestic currency.
        //LOG("Settlement currency was not specified, defaulting to " << soldCcy.code());
        payCcy = soldCcy;
    } else {
        payCcy = parseCurrency(payCurrency_);
        QL_REQUIRE(payCcy == boughtCcy || payCcy == soldCcy,
                   "Settlement currency must be either " << boughtCcy.code() << " or " << soldCcy.code() << ".");
    }

    bool fixingRequired = (settlement_ == "Cash") && (payDate > fixingDate);
    if (fixingRequired) {
        QL_REQUIRE(!fxIndex_.empty(), "FX settlement index must be specified for non-deliverable forwards");
        Currency nonPayCcy = payCcy == boughtCcy ? soldCcy : boughtCcy;
        fxIndex = buildFxIndex(fxIndex_, payCcy.code(), nonPayCcy.code(), engineFactory->market(),
                               engineFactory->configuration(MarketContext::pricing), fxIndexCalendar_, fxIndexDays_);
        requiredFixings_.addFixingDate(fixingDate, fxIndex_, payDate);
    }

    QL_REQUIRE(tradeActions().empty(), "TradeActions not supported for FxForward");

    DLOG("Build FxForward with maturity date " << QuantLib::io::iso_date(maturityDate) << " and pay date "
                                               << QuantLib::io::iso_date(payDate));

    boost::shared_ptr<QuantLib::Instrument> instrument =
        boost::make_shared<QuantExt::FxForward>(boughtAmount_, boughtCcy, soldAmount_, soldCcy, maturityDate, false,
                                                settlement_ == "Physical", payDate, payCcy, fixingDate, fxIndex);
    instrument_.reset(new VanillaInstrument(instrument));

    npvCurrency_ = soldCurrency_;
    notional_ = Null<Real>(); // soldAmount_;
    notionalCurrency_ = "";   // soldCurrency_;
    maturity_ = std::max(payDate, maturityDate);

    // Set up Legs
    legs_ = {{boost::make_shared<SimpleCashFlow>(boughtAmount_, payDate)},
             {boost::make_shared<SimpleCashFlow>(soldAmount_, payDate)}};
    legCurrencies_ = {boughtCurrency_, soldCurrency_};
    legPayers_ = {false, true};

    // set Pricing engine
    boost::shared_ptr<EngineBuilder> builder = engineFactory->builder(tradeType_);
    QL_REQUIRE(builder, "No builder found for " << tradeType_);
    boost::shared_ptr<FxForwardEngineBuilderBase> fxBuilder =
        boost::dynamic_pointer_cast<FxForwardEngineBuilderBase>(builder);

    instrument_->qlInstrument()->setPricingEngine(fxBuilder->engine(boughtCcy, soldCcy));

    DLOG("FxForward leg 0: " << legs_[0][0]->date() << " " << legs_[0][0]->amount());
    DLOG("FxForward leg 1: " << legs_[1][0]->date() << " " << legs_[1][0]->amount());

    additionalData_["soldCurrency"] = soldCurrency_;
    additionalData_["boughtCurrency"] = boughtCurrency_;
    additionalData_["soldAmount"] = soldAmount_;
    additionalData_["boughtAmount"] = boughtAmount_;
}

QuantLib::Real FxForward::notional() const {
    // try to get the notional from the additional results of the instrument
    try {
        return instrument_->qlInstrument()->result<Real>("currentNotional");
    } catch (const std::exception& e) {
        if (strcmp(e.what(), "currentNotional not provided"))
            ALOG("error when retrieving notional: " << e.what());
    }
    // if not provided, return null
    return Null<Real>();
}

std::string FxForward::notionalCurrency() const {
    // try to get the notional ccy from the additional results of the instrument
    try {
        return instrument_->qlInstrument()->result<std::string>("notionalCurrency");
    } catch (const std::exception& e) {
        if (strcmp(e.what(), "notionalCurrency not provided"))
            ALOG("error when retrieving notional ccy: " << e.what());
    }
    // if not provided, return an empty string
    return "";
}

void FxForward::fromXML(XMLNode* node) {
    Trade::fromXML(node);
    XMLNode* fxNode = XMLUtils::getChildNode(node, "FxForwardData");
    QL_REQUIRE(fxNode, "No FxForwardData Node");
    maturityDate_ = XMLUtils::getChildValue(fxNode, "ValueDate", true);
    boughtCurrency_ = XMLUtils::getChildValue(fxNode, "BoughtCurrency", true);
    soldCurrency_ = XMLUtils::getChildValue(fxNode, "SoldCurrency", true);
    boughtAmount_ = XMLUtils::getChildValueAsDouble(fxNode, "BoughtAmount", true);
    soldAmount_ = XMLUtils::getChildValueAsDouble(fxNode, "SoldAmount", true);
    settlement_ = XMLUtils::getChildValue(fxNode, "Settlement", false);
    if (settlement_ == "")
        settlement_ = "Physical";

    if (XMLNode* settlementDataNode = XMLUtils::getChildNode(fxNode, "SettlementData")) {
        payCurrency_ = XMLUtils::getChildValue(settlementDataNode, "Currency", false);
        fxIndex_ = XMLUtils::getChildValue(settlementDataNode, "FXIndex", false);
        payDate_ = XMLUtils::getChildValue(settlementDataNode, "Date", false);

        if (payDate_.empty()) {
            if (XMLNode* rulesNode = XMLUtils::getChildNode(settlementDataNode, "Rules")) {
                payLag_ = XMLUtils::getChildValue(rulesNode, "PaymentLag", false);
                payCalendar_ = XMLUtils::getChildValue(rulesNode, "PaymentCalendar", false);
                payConvention_ = XMLUtils::getChildValue(rulesNode, "PaymentConvention", false);
            }
        }
    }
}

XMLNode* FxForward::toXML(XMLDocument& doc) {
    XMLNode* node = Trade::toXML(doc);
    XMLNode* fxNode = doc.allocNode("FxForwardData");
    XMLUtils::appendNode(node, fxNode);
    XMLUtils::addChild(doc, fxNode, "ValueDate", maturityDate_);
    XMLUtils::addChild(doc, fxNode, "BoughtCurrency", boughtCurrency_);
    XMLUtils::addChild(doc, fxNode, "BoughtAmount", boughtAmount_);
    XMLUtils::addChild(doc, fxNode, "SoldCurrency", soldCurrency_);
    XMLUtils::addChild(doc, fxNode, "SoldAmount", soldAmount_);
    XMLUtils::addChild(doc, fxNode, "Settlement", settlement_);

    XMLNode* settlementDataNode = doc.allocNode("SettlementData");
    XMLUtils::appendNode(fxNode, settlementDataNode);

    if (!payCurrency_.empty())
        XMLUtils::addChild(doc, settlementDataNode, "Currency", payCurrency_);
    if (!fxIndex_.empty())
        XMLUtils::addChild(doc, settlementDataNode, "FXIndex", fxIndex_);
    if (!payDate_.empty()) {
        XMLUtils::addChild(doc, settlementDataNode, "Date", payDate_);
    } else {
        XMLNode* rulesNode = doc.allocNode("Rules");
        XMLUtils::appendNode(settlementDataNode, rulesNode);
        if (!payLag_.empty())
            XMLUtils::addChild(doc, rulesNode, "PaymentLag", payLag_);
        if (!payCalendar_.empty())
            XMLUtils::addChild(doc, rulesNode, "PaymentCalendar", payCalendar_);
        if (!payConvention_.empty())
            XMLUtils::addChild(doc, rulesNode, "PaymentConvention", payConvention_);
    }

    return node;
}
} // namespace data
} // namespace ore


#include <flann/mpi/queries.h>
#include <flann/mpi/index.h>

#include <stdio.h>
#include <sys/time.h>
#include <boost/thread/thread.hpp>

#define IF_RANK0 if (world.rank()==0)

timeval start_time_;
void start_timer(const std::string& message = "")
{
	if (!message.empty()) {
		printf("%s", message.c_str());
		fflush(stdout);
	}
    gettimeofday(&start_time_,NULL);
}

double stop_timer()
{
    timeval end_time;
    gettimeofday(&end_time,NULL);

	return double(end_time.tv_sec-start_time_.tv_sec)+ double(end_time.tv_usec-start_time_.tv_usec)/1000000;
}

float compute_precision(const flann::Matrix<int>& match, const flann::Matrix<int>& indices)
{
	int count = 0;

	assert(match.rows == indices.rows);
	size_t nn = std::min(match.cols, indices.cols);

	for(size_t i=0; i<match.rows; ++i) {
		for (size_t j=0;j<nn;++j) {
			for (size_t k=0;k<nn;++k) {
				if (match[i][j]==indices[i][k]) {
					count ++;
				}
			}
		}
	}

	return float(count)/(nn*match.rows);
}

void search(flann::mpi::Index<flann::L2<float> >* index)
{
	boost::mpi::communicator world;

	int nn = 1;

	flann::Matrix<float> query;
		flann::Matrix<int> match;
	//	flann::Matrix<float> gt_dists;
		IF_RANK0 {
			flann::load_from_file(query, "sift100K.h5","query");
			flann::load_from_file(match, "sift100K.h5","match");
			//	flann::load_from_file(gt_dists, "sift100K.h5","dists");
		}

		boost::mpi::broadcast(world, query, 0);
		boost::mpi::broadcast(world, match, 0);

		flann::Matrix<int> indices(new int[query.rows*nn], query.rows, nn);
		flann::Matrix<float> dists(new float[query.rows*nn], query.rows, nn);

		IF_RANK0 {
			indices = flann::Matrix<int>(new int[query.rows*nn], query.rows, nn);
			dists = flann::Matrix<float>(new float[query.rows*nn], query.rows, nn);
		}

		// do a knn search, using 128 checks0
		IF_RANK0 start_timer("Performing search...\n");
		index->knnSearch(query, indices, dists, nn, flann::SearchParams(128));
		IF_RANK0 {
			printf("Search done (%g seconds)\n", stop_timer());
			printf("Indices size: (%d,%d)\n", (int)indices.rows, (int)indices.cols);
			printf("Checking results\n");
			float precision = compute_precision(match, indices);
			printf("Precision is: %g\n", precision);
		}
		delete[] query.ptr();
		delete[] match.ptr();

		IF_RANK0 {
			delete[] indices.ptr();
			delete[] dists.ptr();
		}

}

int main(int argc, char** argv)
{
	boost::mpi::environment env(argc, argv);
	boost::mpi::communicator world;

	//flann::Matrix<float> dataset;

	IF_RANK0 start_timer("Loading data...\n");
	// construct an randomized kd-tree index using 4 kd-trees
	flann::mpi::Index<flann::L2<float> > index("sift100K.h5", "dataset", flann::KDTreeIndexParams(4));
	//flann::load_from_file(dataset, "sift100K.h5","dataset");
	//flann::Index<flann::L2<float>	> index( dataset, flann::KDTreeIndexParams(4));
	world.barrier();
	IF_RANK0 printf("Loading data done (%g seconds)\n", stop_timer());
	IF_RANK0 printf("Index size: (%d,%d)\n", index.size(), index.veclen());

	start_timer("Building index...\n");
	index.buildIndex();
	printf("Building index done (%g seconds)\n", stop_timer());
    world.barrier();

    printf("Searching...\n");

    boost::thread t(boost::bind(search, &index));
    t.join();
    boost::thread t2(boost::bind(search, &index));

    for(;;){};

	return 0;
}


/*
 * Struck: Structured Output Tracking with Kernels
 *
 * Code to accompany the paper:
 *   Struck: Structured Output Tracking with Kernels
 *   Sam Hare, Amir Saffari, Philip H. S. Torr
 *   International Conference on Computer Vision (ICCV), 2011
 *
 * Copyright (C) 2011 Sam Hare, Oxford Brookes University, Oxford, UK
 *
 * This file is part of Struck.
 *
 * Struck 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.
 *
 * Struck 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 Struck.  If not, see <http://www.gnu.org/licenses/>.
 *
 */

#include "mtf/ThirdParty/Struck/Tracker.h"
#include "mtf/ThirdParty/Struck/Config.h"
#include "mtf/ThirdParty/Struck/ImageRep.h"
#include "mtf/ThirdParty/Struck/Sampler.h"
#include "mtf/ThirdParty/Struck/Sample.h"
#include "mtf/ThirdParty/Struck/GraphUtils/GraphUtils.h"

#include "mtf/ThirdParty/Struck/HaarFeatures.h"
#include "mtf/ThirdParty/Struck/RawFeatures.h"
#include "mtf/ThirdParty/Struck/HistogramFeatures.h"
#include "mtf/ThirdParty/Struck/MultiFeatures.h"

#include "mtf/ThirdParty/Struck/Kernels.h"

#include "mtf/ThirdParty/Struck/LaRank.h"

#include <opencv/cv.h>
#include <opencv2/highgui/highgui.hpp>

#include <Eigen/Core>

#include <vector>
#include <algorithm>

using namespace cv;
using namespace std;
using namespace Eigen;
namespace struck{
	Tracker::Tracker(const Config& conf) :
		m_config(conf),
		m_initialised(false),
		m_pLearner(0),
		m_debugImage(2 * conf.searchRadius + 1, 2 * conf.searchRadius + 1, CV_32FC1),
		m_needsIntegralImage(false)
	{
		Reset();
	}

	Tracker::~Tracker()
	{
		delete m_pLearner;
		for(int i = 0; i < (int)m_features.size(); ++i)
		{
			delete m_features[i];
			delete m_kernels[i];
		}
	}

	void Tracker::Reset()
	{
		m_initialised = false;
		m_debugImage.setTo(0);
		if(m_pLearner) delete m_pLearner;
		for(int i = 0; i < (int)m_features.size(); ++i)
		{
			delete m_features[i];
			delete m_kernels[i];
		}
		m_features.clear();
		m_kernels.clear();

		m_needsIntegralImage = false;
		m_needsIntegralHist = false;

		int numFeatures = m_config.features.size();
		vector<int> featureCounts;
		for(int i = 0; i < numFeatures; ++i)
		{
			switch(m_config.features[i].feature)
			{
			case Config::kFeatureTypeHaar:
				m_features.push_back(new HaarFeatures(m_config));
				m_needsIntegralImage = true;
				break;
			case Config::kFeatureTypeRaw:
				m_features.push_back(new RawFeatures(m_config));
				break;
			case Config::kFeatureTypeHistogram:
				m_features.push_back(new HistogramFeatures(m_config));
				m_needsIntegralHist = true;
				break;
			}
			featureCounts.push_back(m_features.back()->GetCount());

			switch(m_config.features[i].kernel)
			{
			case Config::kKernelTypeLinear:
				m_kernels.push_back(new LinearKernel());
				break;
			case Config::kKernelTypeGaussian:
				m_kernels.push_back(new GaussianKernel(m_config.features[i].params[0]));
				break;
			case Config::kKernelTypeIntersection:
				m_kernels.push_back(new IntersectionKernel());
				break;
			case Config::kKernelTypeChi2:
				m_kernels.push_back(new Chi2Kernel());
				break;
			}
		}

		if(numFeatures > 1)
		{
			MultiFeatures* f = new MultiFeatures(m_features);
			m_features.push_back(f);

			MultiKernel* k = new MultiKernel(m_kernels, featureCounts);
			m_kernels.push_back(k);
		}

		m_pLearner = new LaRank(m_config, *m_features.back(), *m_kernels.back());
	}

	void Tracker::Initialise(const cv::Mat& frame, FloatRect bb)
	{
		m_bb = IntRect(bb);
		ImageRep image(frame, m_needsIntegralImage, m_needsIntegralHist);
		for(int i = 0; i < 1; ++i)
		{
			UpdateLearner(image);
		}
		m_initialised = true;
	}

	void Tracker::Track(const cv::Mat& frame)
	{
		assert(m_initialised);

		ImageRep image(frame, m_needsIntegralImage, m_needsIntegralHist);

		vector<FloatRect> rects = Sampler::PixelSamples(m_bb, m_config.searchRadius);

		vector<FloatRect> keptRects;
		keptRects.reserve(rects.size());
		for(int i = 0; i < (int)rects.size(); ++i)
		{
			if(!rects[i].IsInside(image.GetRect())) continue;
			keptRects.push_back(rects[i]);
		}

		MultiSample sample(image, keptRects);

		vector<double> scores;
		m_pLearner->Eval(sample, scores);

		double bestScore = -DBL_MAX;
		int bestInd = -1;
		for(int i = 0; i < (int)keptRects.size(); ++i)
		{
			if(scores[i] > bestScore)
			{
				bestScore = scores[i];
				bestInd = i;
			}
		}

		UpdateDebugImage(keptRects, m_bb, scores);

		if(bestInd != -1)
		{
			m_bb = keptRects[bestInd];
			UpdateLearner(image);
#if VERBOSE
			cout << "track score: " << bestScore << endl;
#endif
		}
	}

	void Tracker::UpdateDebugImage(const vector<FloatRect>& samples, const FloatRect& centre, const vector<double>& scores)
	{
		double mn = VectorXd::Map(&scores[0], scores.size()).minCoeff();
		double mx = VectorXd::Map(&scores[0], scores.size()).maxCoeff();
		m_debugImage.setTo(0);
		for(int i = 0; i < (int)samples.size(); ++i)
		{
			int x = (int)(samples[i].XMin() - centre.XMin());
			int y = (int)(samples[i].YMin() - centre.YMin());
			m_debugImage.at<float>(m_config.searchRadius + y, m_config.searchRadius + x) = (float)((scores[i] - mn) / (mx - mn));
		}
	}

	void Tracker::Debug()
	{
		imshow("tracker", m_debugImage);
		m_pLearner->Debug();
	}

	void Tracker::UpdateLearner(const ImageRep& image)
	{
		// note these return the centre sample at index 0
		vector<FloatRect> rects = Sampler::RadialSamples(m_bb, 2 * m_config.searchRadius, 5, 16);
		//vector<FloatRect> rects = Sampler::PixelSamples(m_bb, 2*m_config.searchRadius, true);

		vector<FloatRect> keptRects;
		keptRects.push_back(rects[0]); // the true sample
		for(int i = 1; i < (int)rects.size(); ++i)
		{
			if(!rects[i].IsInside(image.GetRect())) continue;
			keptRects.push_back(rects[i]);
		}

#if VERBOSE
		cout << keptRects.size() << " samples" << endl;
#endif

		MultiSample sample(image, keptRects);
		m_pLearner->Update(sample, 0);
	}
}


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

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

#ifndef BOOST_SIMD_FUNCTION_SCALAR_CSCD_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_SCALAR_CSCD_HPP_INCLUDED

#include <boost/simd/function/definition/cscd.hpp>
#include <boost/simd/arch/common/scalar/function/cscd.hpp>

#endif


#ifndef SOLVER_HPP_
#define SOLVER_HPP_

#include <algorithm>

#include <Eigen/Core>
#include <Eigen/SparseCore>

namespace sdm {
class Solver {
public:
  virtual ~Solver(void) {}

  // virtual Eigen::VectorXd train() = 0;
  virtual void set_regularized_parameter(const double &c) = 0;
  virtual int get_train_l(void) = 0;
  virtual int get_valid_l(void) = 0;
  virtual Eigen::ArrayXd train_warm_start(const Eigen::ArrayXd &alpha) = 0;
  virtual Eigen::VectorXd train_warm_start(const Eigen::VectorXd &alpha) = 0;

  virtual std::vector<double>
  get_c_set_right_opt(const Eigen::ArrayXd &alpha_opt, const double &c_now,
                      double &valid_err) = 0;

  // virtual Eigen::VectorXd train_warm_start_inexact(const Eigen::VectorXd
  // &alpha,
  //                                                  const double
  //                                                  inexact_level,
  //                                                  double &ub_validerr,
  //                                                  double &lb_validerr) = 0;

  // virtual std::vector<double>
  // get_c_set_right_opt(const Eigen::ArrayXd &alpha_opt, const double &c_now,
  //                     double &valid_err) = 0;

  // virtual std::vector<double>
  // get_c_set_right_subopt(const Eigen::VectorXd &w,
  //                        const Eigen::VectorXd &grad_w,
  //                        const double &c_now) = 0;

  // virtual std::vector<double>
  // get_c_set_left_subopt(const Eigen::VectorXd &w, const Eigen::VectorXd
  // &grad_w,
  //                       const double &c_now) = 0;
};
} // namespace sdm

#endif // PRIMAL_SOLVER_HPP_


/*
 *            Copyright 2009-2020 The VOTCA Development Team
 *                       (http://www.votca.org)
 *
 *      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.
 *
 */

// Third party includes
#include <boost/format.hpp>

// VOTCA includes
#include <votca/tools/elements.h>

// Local VOTCA includes
#include "votca/xtp/atom.h"
#include "votca/xtp/forces.h"
#include "votca/xtp/statetracker.h"

namespace votca {
namespace xtp {

using std::flush;
void Forces::Initialize(tools::Property& options) {

  std::vector<std::string> choices = {"forward", "central"};
  _force_method =
      options.ifExistsAndinListReturnElseThrowRuntimeError<std::string>(
          ".method", choices);

  _displacement = options.ifExistsReturnElseReturnDefault<double>(
      ".displacement", 0.001);  // Angstrom
  _displacement *= tools::conv::ang2bohr;

  _remove_total_force = options.ifExistsReturnElseReturnDefault<bool>(
      ".CoMforce_removal", _remove_total_force);
  return;
}

void Forces::Calculate(const Orbitals& orbitals) {

  Index natoms = orbitals.QMAtoms().size();
  _forces = Eigen::MatrixX3d::Zero(natoms, 3);

  for (Index atom_index = 0; atom_index < natoms; atom_index++) {

    XTP_LOG(Log::debug, *_pLog)
        << "FORCES--DEBUG working on atom " << atom_index << flush;
    Eigen::Vector3d atom_force = Eigen::Vector3d::Zero();
    // Calculate Force on this atom
    if (_force_method == "forward") {
      atom_force = NumForceForward(orbitals, atom_index);
    }
    if (_force_method == "central") {
      atom_force = NumForceCentral(orbitals, atom_index);
    }
    _forces.row(atom_index) = atom_force.transpose();
  }
  if (_remove_total_force) {
    RemoveTotalForce();
  }
  return;
}

void Forces::Report() const {

  XTP_LOG(Log::error, *_pLog)
      << (boost::format(" ---- FORCES (Hartree/Bohr)   ")).str() << flush;
  XTP_LOG(Log::error, *_pLog)
      << (boost::format("      %1$s differences   ") % _force_method).str()
      << flush;
  XTP_LOG(Log::error, *_pLog)
      << (boost::format("      displacement %1$1.4f Angstrom   ") %
          (_displacement * tools::conv::bohr2ang))
             .str()
      << flush;
  XTP_LOG(Log::error, *_pLog)
      << (boost::format(" Atom\t x\t  y\t  z ")).str() << flush;

  for (Index i = 0; i < _forces.rows(); i++) {
    XTP_LOG(Log::error, *_pLog)
        << (boost::format("%1$4d    %2$+1.4f  %3$+1.4f  %4$+1.4f") % i %
            _forces(i, 0) % _forces(i, 1) % _forces(i, 2))
               .str()
        << flush;
  }
  return;
}

Eigen::Vector3d Forces::NumForceForward(Orbitals orbitals, Index atom_index) {
  Eigen::Vector3d force = Eigen::Vector3d::Zero();
  // get this atoms's current coordinates
  double energy_center =
      orbitals.getTotalStateEnergy(_tracker.CalcState(orbitals));
  const Eigen::Vector3d current_pos = orbitals.QMAtoms()[atom_index].getPos();
  for (Index i_cart = 0; i_cart < 3; i_cart++) {
    Eigen::Vector3d displacement_vec = Eigen::Vector3d::Zero();
    displacement_vec[i_cart] = _displacement;
    // update the coordinate
    Eigen::Vector3d pos_displaced = current_pos + displacement_vec;
    orbitals.QMAtoms()[atom_index].setPos(pos_displaced);
    _gwbse_engine.ExcitationEnergies(orbitals);
    double energy_displaced =
        orbitals.getTotalStateEnergy(_tracker.CalcState(orbitals));
    force(i_cart) = (energy_center - energy_displaced) / _displacement;
    orbitals.QMAtoms()[atom_index].setPos(
        current_pos);  // restore original coordinate into segment
  }                    // Cartesian directions
  return force;
}

Eigen::Vector3d Forces::NumForceCentral(Orbitals orbitals, Index atom_index) {
  Eigen::Vector3d force = Eigen::Vector3d::Zero();
  const Eigen::Vector3d current_pos = orbitals.QMAtoms()[atom_index].getPos();
  for (Index i_cart = 0; i_cart < 3; i_cart++) {
    XTP_LOG(Log::debug, *_pLog)
        << "FORCES--DEBUG           Cartesian component " << i_cart << flush;
    Eigen::Vector3d displacement_vec = Eigen::Vector3d::Zero();
    displacement_vec[i_cart] = _displacement;
    // update the coordinate
    Eigen::Vector3d pos_displaced = current_pos + displacement_vec;
    orbitals.QMAtoms()[atom_index].setPos(pos_displaced);
    _gwbse_engine.ExcitationEnergies(orbitals);
    double energy_displaced_plus =
        orbitals.getTotalStateEnergy(_tracker.CalcState(orbitals));
    // update the coordinate
    pos_displaced = current_pos - displacement_vec;
    orbitals.QMAtoms()[atom_index].setPos(pos_displaced);
    _gwbse_engine.ExcitationEnergies(orbitals);
    double energy_displaced_minus =
        orbitals.getTotalStateEnergy(_tracker.CalcState(orbitals));
    force(i_cart) =
        0.5 * (energy_displaced_minus - energy_displaced_plus) / _displacement;
    orbitals.QMAtoms()[atom_index].setPos(
        current_pos);  // restore original coordinate into orbital
  }
  return force;
}

void Forces::RemoveTotalForce() {
  Eigen::Vector3d avgtotal_force =
      _forces.colwise().sum() / double(_forces.rows());
  for (Index i_atom = 0; i_atom < _forces.rows(); i_atom++) {
    _forces.row(i_atom) -= avgtotal_force;
  }
  return;
}

}  // namespace xtp
}  // namespace votca


#include <iostream>
#include <iomanip>
#include <cmath>
#include <array>
#include <cfloat>
#include <bitset>
#include <vector>
#include <boost/functional/hash.hpp>
#include <boost/timer/timer.hpp>
#include <boost/format.hpp>
#include <boost/predef.h>
#include <random>
#include <fstream>

extern "C" {
  double my_fmod (double x, double y);
  double __ieee754_fmod32 (double x, double y);
};

using namespace std;

typedef union { uint32_t a[2]; uint64_t i; uint8_t b[8]; double x; } mynumber;

constexpr uint64_t random_numbers[] = {
        UINT64_C(0xA2C8B877585CF27B),
        UINT64_C(0xB500C85FF4024153),
        UINT64_C(0x7A99BA5628F8C1C1),
        UINT64_C(0x0D04E05E91147E21),
        UINT64_C(0x12099DE6BC6AFA62),
        UINT64_C(0x5ADF8FD793E00727),
        UINT64_C(0xF7002F54BBF0B21B),
        UINT64_C(0x4755AD393D09ED99),
        UINT64_C(0xCA10A5131AD12E4F),
        UINT64_C(0xC44A2E3FC98CC2DA)
};

class test_fmod_base {

public:
    struct test_result {
        std::size_t hash_output = UINT64_C(0x8370B2F63E16DFB9);
        std::size_t hash_input  = UINT64_C(0x4bA6C5A2EA5458A1);
        std::uint64_t total_calcs = 0;
        std::uint64_t sum_results = 0;
        std::uint64_t dt = 0; //nanoseconds
        test_result() = default;
    };

    static
    std::string report(const test_result &r, int ind = 0) {
        std::string inds(ind, ' ');
        double time = (double(r.dt) * 1E-9) / (double(r.total_calcs) / 1E9);
        auto result =
                boost::format("%1%C++ HASH input values      : 0x%2$016X\n"
                              "%1%C++ HASH output values     : 0x%3$016X\n"
                              "%1%Total calculations         : %4%\n"
                              "%1%Time                       : %5$.3f (s for billion calls)\n"
                ) % inds % r.hash_input % r.hash_output %  r.total_calcs % time;
        return result.str();
    }
};

class test_fmod_main : public test_fmod_base {
private:
    static constexpr int bits_change = 14;
    static constexpr int bits_num = (1 << bits_change);

    vector<uint64_t> masksx, masksy;
    vector<int> fct_x, fct_y;

    mynumber create_num(uint32_t i, uint64_t mask) {
        mynumber result;
        result.i  = uint64_t(i) << (64 - bits_change);
        result.i |= mask & (UINT64_C(0xFFFFFFFFFFFFFFFF) >> bits_change);
        return result;
    }

    static vector<uint64_t> create_masks() {
        vector<uint64_t> result(cbegin(random_numbers), cend(random_numbers));
        for(int i = 0; i < 64 - bits_change; i++) {
            result.emplace_back( (UINT64_C(1) << i) );
            result.emplace_back( (UINT64_C(1) << i) + 1 );
        }
        return result;
    }

    static vector<int> create_factors() {
        vector<int> result(bits_num);
        iota(result.begin(), result.end(), 0);
        return result;
    }

    template<class C> static
    C reduce_size(const C &cont, int r_factor, int seed, int first_store) {
        C result = cont;
        if( r_factor > 1 ) {
            shuffle(result.begin() + first_store, result.end(), std::default_random_engine(seed));
            result.resize(min<int>(result.size(), result.size() / r_factor));
        }
        return result;
    }
public:

    void set_vals(int r_factor) {
        auto m = create_masks();
        auto f = create_factors();
        masksx = reduce_size(m, r_factor, 1, 2);
        masksy = reduce_size(m, r_factor, 2, 2);
        fct_x = reduce_size(f, r_factor, 3, 10);
        fct_y = reduce_size(f, r_factor, 4, 10);
    }

    template<typename F>
    test_result do_test(F f) {
        test_result result;
        boost::timer::cpu_timer at;
        at.start();
        for (int i : fct_x) {
            for (const auto &mi : masksx) {
                mynumber n1 = create_num(i, mi);
                boost::hash_combine(result.hash_input, boost::hash_value(n1.i));
                for (int j : fct_y) {
                    for (const auto &mj : masksy) {
                        mynumber n2 = create_num(j, mj);
                        mynumber r;
                        boost::hash_combine(result.hash_input, boost::hash_value(n2.i));
                        r.x = f(n1.x, n2.x);
                        result.sum_results += r.i;
                        boost::hash_combine(result.hash_output, boost::hash_value(r.i));
                        result.total_calcs++;
                    }
                }
            }
        }
        at.stop();
        result.dt = at.elapsed().wall;
        return result;
    }
};

class test_fmod_wrap : public test_fmod_base {
    double x_val_min;
    double x_val_max;
    uint64_t num_vals;
    double y_val;
public:
    test_fmod_wrap(double _x_val_min, double _x_val_max, uint64_t _num_vals, double _y_val) :
            x_val_min(_x_val_min), x_val_max(_x_val_max), num_vals(_num_vals), y_val(_y_val) {};
    template<typename F>
    test_result do_test(F f) {
        test_result result;
        boost::timer::cpu_timer at;
        at.start();
        for (uint64_t i = 0; i < num_vals; i++) {
            mynumber n1, r;
            n1.x = x_val_min + (x_val_max - x_val_min) * double(i) / double(num_vals);
            boost::hash_combine(result.hash_input, boost::hash_value(n1.i));
            r.x = f(n1.x, y_val);
            result.sum_results += r.i;
            boost::hash_combine(result.hash_output, boost::hash_value(r.i));
            result.total_calcs++;
        }
        at.stop();
        result.dt = at.elapsed().wall;
        return result;
    }
};

class test_fmod_extreme : public test_fmod_base {
    uint64_t num_vals;
public:
    explicit test_fmod_extreme(uint64_t _num_vals) : num_vals(_num_vals) {};
    template<typename F>
    test_result do_test(F f) {
        test_result result;
        boost::timer::cpu_timer at;
        at.start();
        mynumber n1, n2, r;
        for (uint64_t i = 0; i < num_vals; i++) {
            n1.i =  0x7E3A47DC2F33B492UL + i; // huge number
            n2.i = 1 + i; // subnormal
            boost::hash_combine(result.hash_input, boost::hash_value(n1.i));
            boost::hash_combine(result.hash_input, boost::hash_value(n2.i));

            r.x = f(n1.x, n2.x);
            result.sum_results += r.i;
            boost::hash_combine(result.hash_output, boost::hash_value(r.i));
            result.total_calcs++;
        }
        at.stop();
        result.dt = at.elapsed().wall;
        return result;
    }
};

class test_fmod_fixed : public test_fmod_base {
  uint64_t num_vals;
  mynumber n1, n2;
 public:
  explicit test_fmod_fixed(uint64_t _num_vals, double x, double y)
  : num_vals(_num_vals) {
    n1.x = x;
    n2.x = y;
  };
  template<typename F>
  test_result do_test(F f) {
    test_result result;
    boost::timer::cpu_timer at;
    at.start();
    mynumber r;
    boost::hash_combine(result.hash_input, boost::hash_value(n1.i));
    boost::hash_combine(result.hash_input, boost::hash_value(n2.i));
    for (uint64_t i = 0; i < num_vals; i++) {
        r.x = f(n1.x, n2.x);
        result.sum_results += r.i;
        boost::hash_combine(result.hash_output, boost::hash_value(r.i));
        result.total_calcs++;
    }
    at.stop();
    result.dt = at.elapsed().wall;
    return result;
  }
};
// fstream fs("chekv.dat", fstream::out);

inline
double dummy_f(double x, double y) {
    /* if( abs(x) < abs(y) )
      swap(x, y);
    fs << setprecision(18) << x << " " << y << endl; */
    return x + y;
}

/*inline
double fmod_libm(double x, double y) {
    mynumber nx, nr;
    nx.x = x;
    nr.x = fmod(x, y);
    //fs << hex << nx.x << " " << nr.x << " " << nx.i << " " << nr.i << endl;
    //return fmod(x, y);
    return nr.x;
}*/

inline
double fmod_libm(double x, double y) {
    return fmod(x, y);
}

inline
double fmod_libm32(double x, double y) {
  return __ieee754_fmod32(x, y);
}

#if BOOST_ARCH_X86
inline
double fmod_fpu(double x, double y) {
  double res;
  asm ("1:\tfprem\n"
       "fstsw   %%ax\n"
       "sahf\n"
       "jp      1b\n"
       "fstp    %%st(1)"
  : "=t" (res) : "0" (x), "u" (y) : "ax", "st(1)");
  return res;
}
#endif

inline
double my_fmod_wrap(double x, double y)
{
  mynumber xa, ya, ni, nn;
  xa.x = x;
  ya.x = y;
  cout << hex << xa.i << " " << ya.i << dec << endl;
  nn.x = my_fmod(x, y);
  return nn.x;
}

inline
double fmod_check(double x, double y) {
    /*if( !isfinite(x) || !isfinite(y) )
      return 0.0;
    if( abs(x) < 1E-280 || abs(y) < 1E-280 )
      return 0.0;*/
    mynumber xa, ya, ni, nn;
    xa.x = x;
    ya.x = y;
    ni.x = fmod(x, y);
    nn.x = my_fmod(x, y);
    if( nn.i != ni.i )
      {
        cout << hex << xa.i << " " << ya.i << dec << endl;
        cout << ni.x << " " << nn.x << endl;
      }
    return ni.x;
}

template <class C>
uint64_t do_calc_set(C tm, string header) {
  cout << header << endl;
  auto r = tm.do_test(dummy_f);
  cout << "  Dummy function   :" << endl << test_fmod_base::report(r, 4) << endl;
  uint64_t result = r.total_calcs;

  r = tm.do_test(fmod_libm);
  cout << "  Internal function :" << endl << test_fmod_base::report(r, 4) << endl;

  r = tm.do_test(my_fmod);
  cout << "  New function      :" << endl << test_fmod_base::report(r, 4) << endl;
#if BOOST_ARCH_X86
  r = tm.do_test(fmod_fpu);
  cout << "  FPU function      :" << endl << test_fmod_base::report(r, 4) << endl;
#endif
  //r = tm.do_test(fmod_libm32);
  //cout << "  ieee754 32 bit    :" << endl << test_fmod_base::report(r, 4) << endl;

  return result;
}

int main() {
    {
        std::size_t control_hash = boost::hash_value(200);
        boost::hash_combine(control_hash, boost::hash_value(UINT64_MAX - 1000));
        cout << hex << "Test hash: 0x" << control_hash << endl
                    << "Equal to : 0x64061da93a405636" << dec << endl;
    }

  /*{
    mynumber n1, n2;
    n1.i = UINT64_C(0x43ac001000000001);
    n2.i = UINT64_C(0x3360000000040001);
    cout << fmod_check(n1.x, n2.x) << endl;
  }*/

    uint64_t num_vals = 6864400;
    {
        test_fmod_main tm;
        tm.set_vals(45); // Reduce factor default 25
        num_vals = do_calc_set(tm, "Main test:");
    }

    {
        test_fmod_wrap t_pi(-4 * M_PI, 4 * M_PI, 10 * num_vals, 2 * M_PI);
        do_calc_set(t_pi, "Test wrap 2 * pi:");
    }

    {
        test_fmod_wrap t_factor(-1E6, 1E6, 10 * num_vals, 1.0);
        do_calc_set(t_factor, "Fraction part:");
    }

    {
        test_fmod_extreme t_extr(num_vals);
        do_calc_set(t_extr, "fmod from huge and tiny values:");
    }

    for(const auto &p : {std::make_pair(5.0, 3.0),
                         std::make_pair(3.5, 3.0),
                         std::make_pair(5497558138880.0, 3.0), // 5 * 2^40
                         std::make_pair(DBL_MAX, DBL_MIN * 1.3)}) {
        test_fmod_fixed t_fixed(num_vals, p.first, p.second);
        do_calc_set(t_fixed, "fmod(" + std::to_string(p.first) +", "
                                     + std::to_string(p.second) + "):" );
    }

    return 0;
}


#ifndef RAI_QUADRUPEDCONTROLLER_HPP
#define RAI_QUADRUPEDCONTROLLER_HPP

#include <ros/console.h>
#include <Eigen/Core>
#include "string"
#include "cerberus_anymal_utils/SimpleMLPLayer.hpp"
#include "cerberus_anymal_utils/IK.hpp"
#include "cerberus_anymal_utils/GraphLoader.hpp"
# define M_PI       3.14159265358979323846    /* pi */
# define M_PI_2        1.57079632679489661923    /* pi/2 */

namespace RAI {

class QuadrupedController {
  using Dtype = float;
  static constexpr int EstimationDim = 4;
  static constexpr int ObservationDim = 60;
  static constexpr int HistoryLength = 128;
  static constexpr int jointHistoryLength = 20;
  static constexpr unsigned int actionDimension = 16;
  static constexpr int stateDimension = 133;

  typedef typename Eigen::Matrix<double, 3, 1> AngularVelocity;
  typedef typename Eigen::Matrix<double, 3, 1> LinearVelocity;
  typedef typename Eigen::Matrix<double, 4, 1> Quaternion;
  typedef typename Eigen::Matrix<double, 3, 3> RotationMatrix;
  typedef typename Eigen::Matrix<double, 4, 1> Vector4d;
  typedef typename Eigen::Matrix<double, actionDimension, 1> Action;
  typedef typename Eigen::Matrix<Dtype, ObservationDim, 1> Observation;
  typedef typename Eigen::Matrix<Dtype, stateDimension, 1> State;

  typedef typename Eigen::VectorXd VectorXd;
  typedef typename Eigen::Matrix<Dtype, -1, 1> VectorXD;

 public:
  QuadrupedController() {

    footPositionOffset_ <<
                        0.3 + 0.1, 0.2, h0_,
        0.3 + 0.1, -0.2, h0_,
        -0.3 - 0.1, 0.2, h0_,
        -0.3 - 0.1, -0.2, h0_;

    jointNominalConfig_.setZero(12);
    Eigen::Vector3d sol;
    for (int i = 0; i < 4; i++) {
      IK_.solveIK(sol, footPositionOffset_.segment(3 * i, 3), i);
      jointNominalConfig_.segment(3 * i, 3) = sol;
    }

    observationOffset_ << 0.0, 0.0, 0.0,
        0.0, 0.0, 1.0, /// gravity axis
        VectorXD::Constant(6, 0.0),
        jointNominalConfig_.template cast<Dtype>(),
        VectorXD::Constant(12, 0.0),
        VectorXD::Constant(12, 0.0),
        VectorXD::Constant(8, 0.0),
        VectorXD::Constant(4, 0.0);

    observationScale_ <<
                      1.5, 1.5, 1.5, /// command
        5.0, 5.0, 5.0, /// gravity axis
        VectorXD::Constant(3, 2.0),
        VectorXD::Constant(3, 2.0),
        VectorXD::Constant(12, 2.0), /// joint angles
        0.5, 0.4, 0.3,
        0.5, 0.4, 0.3,
        0.5, 0.4, 0.3,
        0.5, 0.4, 0.3,
        6.5, 4.5, 3.5,
        6.5, 4.5, 3.5,
        6.5, 4.5, 3.5,
        6.5, 4.5, 3.5,
        VectorXD::Constant(8, 1.5),
        VectorXD::Constant(4, 2.0 / freqScale_);

    /// state params
    stateOffset_ << 0.0, 0.0, 0.0, /// command
        0.0, 0.0, 1.0, /// gravity axis
        VectorXD::Constant(6, 0.0), /// body lin/ang vel
        jointNominalConfig_.template cast<Dtype>(), /// joint position
        VectorXD::Constant(12, 0.0), /// position error
        VectorXD::Constant(12, 0.0), /// position error
        VectorXD::Constant(4, 0.0),
        VectorXD::Constant(8, 0.0),
        VectorXD::Constant(24, 0.0),
        VectorXD::Constant(24, 0.0),
        jointNominalConfig_.template cast<Dtype>(),
        jointNominalConfig_.template cast<Dtype>(),
        0.0;

    stateScale_ << 1.5, 1.5, 1.5, /// command
        5.0, 5.0, 5.0, /// gravity axis
        VectorXD::Constant(3, 2.0),
        VectorXD::Constant(3, 2.0),
        VectorXD::Constant(12, 2.0), /// joint angles
        0.5, 0.4, 0.3,
        0.5, 0.4, 0.3,
        0.5, 0.4, 0.3,
        0.5, 0.4, 0.3,
        6.5, 4.5, 3.5,
        6.5, 4.5, 3.5,
        6.5, 4.5, 3.5,
        6.5, 4.5, 3.5,
        VectorXD::Constant(4, 2.0 / freqScale_),
        VectorXD::Constant(8, 1.5),
        VectorXD::Constant(24, 5.0), /// joint position errors
        0.5, 0.4, 0.3,
        0.5, 0.4, 0.3,
        0.5, 0.4, 0.3,
        0.5, 0.4, 0.3,
        0.5, 0.4, 0.3,
        0.5, 0.4, 0.3,
        0.5, 0.4, 0.3,
        0.5, 0.4, 0.3,
        VectorXD::Constant(12, 2.0),
        VectorXD::Constant(12, 2.0),
        2.0 / freqScale_;

    double amp1 = 0.5 * freqScale_, amp2 = 0.0;

    baseFreq_ = 1.3 * freqScale_;

    actionScale_ <<
                 amp1, amp1, amp1, amp1,
        VectorXd::Constant(12, 0.2);

    actionOffset_ <<
                  amp2, amp2, amp2, amp2,
        VectorXd::Constant(12, 0.0);

    for (size_t i = 0; i < 4; i++) {
      clearance_[i] = 0.2;
    }

    footPos_Target.resize(4);
    prevfootPos_Target.resize(4);
    prevfootPos_Target2.resize(4);

    //tensor config
    state_dims_inv.AddDim(1);
    state_dims_inv.AddDim(1);
    state_dims_inv.AddDim(stateDimension);
    state_tf_tensor = tensorflow::Tensor(tensorflow::DataType::DT_FLOAT, state_dims_inv);

    for (size_t i = 0; i < 4; i++) {
      piD_[i] = 0.0;
      footPos_Target[i] = footPositionOffset_.template segment<3>(3 * i);
      prevfootPos_Target[i] = footPositionOffset_.template segment<3>(3 * i);
      prevfootPos_Target2[i] = footPositionOffset_.template segment<3>(3 * i);
    }

    pi_[0] = 0.0;
    pi_[1] = M_PI;
    pi_[2] = M_PI;
    pi_[3] = 0.0;

    controlCounter = 0;
    stopCounter = 1000;
    baseFreq_ = 0.0;
    historyUpdateCounter = 0;
    scaledAction_ = actionOffset_;
    jointVelHist_.setZero();
    jointPosHist_.setZero();
    historyBuffer_.setZero();

    jointPositionTarget_ = jointNominalConfig_;
    previousjointPositionTarget_ = jointPositionTarget_;
    previousjointPositionTarget2_ = jointPositionTarget_;

    command_.setZero();
  }

  ~QuadrupedController() {}

  void reset(GraphLoader<Dtype> *graph,
             const Eigen::Matrix<double, 19, 1> &generalizedCoordinates,
             const Eigen::Matrix<double, 18, 1> &generalizedVelocities) { // = init()

    graph_ = graph;

    if (history_dims_inv.dims() != 3) {
      history_dims_inv.AddDim(1);
      history_dims_inv.AddDim(nStack_);
      history_dims_inv.AddDim(ObservationDim);
      history_tf_tensor = tensorflow::Tensor(tensorflow::DataType::DT_FLOAT, history_dims_inv);
      historyBuffer_.resize(ObservationDim, nStack_);
    }

    for (size_t i = 0; i < 4; i++) {
      piD_[i] = 0.0;
      footPos_Target[i] = footPositionOffset_.template segment<3>(3 * i);
      prevfootPos_Target[i] = footPositionOffset_.template segment<3>(3 * i);
      prevfootPos_Target2[i] = footPositionOffset_.template segment<3>(3 * i);
    }

    pi_[0] = 0.0;
    pi_[1] = M_PI;
    pi_[2] = M_PI;
    pi_[3] = 0.0;

    controlCounter = 0;
    stopCounter = 1000;
    baseFreq_ = 0.0;
    historyUpdateCounter = 0;
    scaledAction_ = actionOffset_;
    jointVelHist_.setZero();
    jointPosHist_.setZero();
    historyBuffer_.setZero();
    jointPositionTarget_ = jointNominalConfig_;
    previousjointPositionTarget_ = jointPositionTarget_;
    previousjointPositionTarget2_ = jointPositionTarget_;
    observation_unscaled_.setZero();
    state_unscaled_.setZero();

    command_.setZero();

    ///dummy call for memory allocation
    std::vector<tensorflow::Tensor> outputs;
    graph_->run({std::make_pair("state", state_tf_tensor), std::make_pair("history", history_tf_tensor)},
                {"policy/actionDist"},
                {},
                outputs);

    /// fill in history buffer
    updateHistory(generalizedCoordinates,
                  generalizedVelocities);
    for (int i = 1; i < nStack_; i++) {
      historyBuffer_.col(i) = historyBuffer_.col(0);
    }
  }

  void updateAction(Action action_in) {
    /// update buffer
    for (size_t i = 0; i < 4; i++) {
      prevfootPos_Target2[i] = prevfootPos_Target[i];
      prevfootPos_Target[i] = footPos_Target[i];
    }

    previousjointPositionTarget2_ = previousjointPositionTarget_;
    previousjointPositionTarget_ = jointPositionTarget_;

    scaledAction_ = action_in.cwiseProduct(actionScale_) + actionOffset_;

    /// update cpg
    for (size_t j = 0; j < 4; j++) {
      piD_[j] = scaledAction_[j] + baseFreq_;
      pi_[j] += piD_[j] * decimation;
      pi_[j] = anglemod(pi_[j]);
    }

    y_ << 0.0, e_g[2], -e_g[1];
    y_.normalize();
    x_ = y_.cross(e_g); // e_g cross y_;
    x_.normalize();

    for (size_t i = 0; i < 4; i++) {
      double dh = 0.0;
      if (pi_[i] > 0.0) {
        double t = pi_[i] / M_PI_2;
        if (t < 1.0) {
          double t2 = t * t;
          double t3 = t2 * t;
          dh = (-2 * t3 + 3 * t2);
        } else {
          t = t - 1;
          double t2 = t * t;
          double t3 = t2 * t;
          dh = (2 * t3 - 3 * t2 + 1.0);
        }
        dh *= clearance_[i];
      }

      footPos_Target[i][0] = footPositionOffset_.tail(12)[3 * i];
      footPos_Target[i][1] = footPositionOffset_.tail(12)[3 * i + 1];
      footPos_Target[i][2] = 0.0;
      footPos_Target[i] += e_g * (footPositionOffset_.tail(12)[3 * i + 2] + dh);
      // footPos_Target[i] += e_g * scaledAction_.tail(12)[3 * i + 2];
      // footPos_Target[i] += x_ * scaledAction_.tail(12)[3 * i];
      // footPos_Target[i] += y_ * scaledAction_.tail(12)[3 * i + 1];

      Eigen::Vector3d target;
      Eigen::Vector3d sol;
      target = footPos_Target[i];
//
      IK_.solveIK(sol, target, i);
      jointPositionTarget_.segment<3>(3 * i) = sol;
      jointPositionTarget_(3 * i) += scaledAction_.tail(12)[3 * i];;
      jointPositionTarget_(3 * i + 1) += scaledAction_.tail(12)[3 * i + 1];
      jointPositionTarget_(3 * i + 2) += scaledAction_.tail(12)[3 * i + 2];;
    }
  }

  void getDesJointPos(Eigen::Matrix<double, 12, 1> &output,
                      const Eigen::Matrix<double, 19, 1> &generalizedCoordinates,
                      const Eigen::Matrix<double, 18, 1> &generalizedVelocities,
                      double headingVel, double lateralVel, double yawrate) {
    double norm = std::sqrt(headingVel * headingVel + lateralVel * lateralVel + yawrate * yawrate);

    if (norm < 0.05) {
      command_.setZero();
    } else {
      double linvelNorm = std::sqrt(headingVel * headingVel + lateralVel * lateralVel);
      command_.setZero();
      if (linvelNorm > 0.2) {
        double command_dir = std::atan2(lateralVel, headingVel);
        command_[0] = std::cos(command_dir);
        command_[1] = std::sin(command_dir);
      }

      command_[2] = std::max(std::min(yawrate, 0.8), -0.8);
    }

    if (command_.norm() > 0.1 || generalizedVelocities.head(2).norm() > 0.3 || e_g[2] < 0.5) {
      if (stopCounter > 1200) { // While standin -> reset phase to react faster (unsmooth)
        pi_[0] = 0.0;
        pi_[1] = M_PI;
        pi_[2] = M_PI;
        pi_[3] = 0.0;
      }
      stopCounter = 0;
    } else {
      stopCounter++;
    }

    if (historyUpdateCounter % ObservationStride_ == 0) {
      updateHistory(generalizedCoordinates,
                    generalizedVelocities);
      historyUpdateCounter = 0;
    }

    if (controlCounter % decimation == 0) { // 8: 50 Hz

      // FSM
      if (stopCounter > 75) {
        bool check = true;
        for (int i = 0; i < 4; i++) {// Hack for nice looking stop
          if ((pi_[i] > -0.1 * M_PI) && (pi_[i] < 0.5 * M_PI)) check = false;
        }

        if (check || baseFreq_ < 1.3 * freqScale_) {
//        baseFreq_ = std::max(baseFreq_ - 0.05 * freqScale_, 0.0);
          baseFreq_ = 0.0;
        }
      } else {
//        baseFreq_ = std::min(baseFreq_ + 0.05 * freqScale_, 1.3 * freqScale_);
        baseFreq_ = 1.3 * freqScale_;
      }

      /// Get state
      conversion_GeneralizedState2LearningState(state_,
                                                generalizedCoordinates,
                                                generalizedVelocities);

      Eigen::Matrix<Dtype, actionDimension, 1> action_f_;
      std::memcpy(state_tf_tensor.template flat<Dtype>().data(),
                  state_.data(),
                  sizeof(Dtype) * stateDimension);
      std::memcpy(history_tf_tensor.template flat<Dtype>().data(),
                  historyBuffer_.data(),
                  sizeof(Dtype) * history_tf_tensor.NumElements());

      std::vector<tensorflow::Tensor> outputs;
      graph_->run({std::make_pair("state", state_tf_tensor), std::make_pair("history", history_tf_tensor)},
                  {"policy/actionDist"},
                  {},
                  outputs);

      std::memcpy(action_f_.data(),
                  outputs[0].template flat<Dtype>().data(),
                  sizeof(Dtype) * actionDimension);

      action_ = action_f_.template cast<double>();

      // std::cout << action_ << std::endl;
      /// update action
      updateAction(action_);
      controlCounter = 0;
    }

    // for (size_t i = 0; i < 4; i++) {
    //   Eigen::Vector3d target = (controlCounter + 1) * footPos_Target[i];
    //   target += (decimation - 1 - controlCounter) * prevfootPos_Target[i];
    //   target *= 0.125;

    //   Eigen::Vector3d sol;
    //   IK_.solveIK(sol, target, i);
    //   jointPositionTarget_.segment<3>(3 * i) = sol;
    // }
    output = jointPositionTarget_;

    tempHist_ = jointVelHist_;
    jointVelHist_.head(jointHistoryLength * 12 - 12) = tempHist_.tail(jointHistoryLength * 12 - 12);
    jointVelHist_.tail(12) = generalizedVelocities.tail(12).template cast<Dtype>();

    tempHist_ = jointPosHist_;
    jointPosHist_.head(jointHistoryLength * 12 - 12) = tempHist_.tail(jointHistoryLength * 12 - 12);
    jointPosHist_.tail(12) = (jointPositionTarget_ - generalizedCoordinates.tail(12)).template cast<Dtype>();

    controlCounter++;
    historyUpdateCounter++;
  };

  void updateHistory(const VectorXd &q,
                     const VectorXd &u) {
    Observation observation_scaled;

    Quaternion quat = q.template segment<4>(3);
    RotationMatrix R_b = quatToRotMat(quat);
    e_g = R_b.row(2).transpose();

    /// velocity in body coordinate
    LinearVelocity bodyVel = R_b.transpose() * u.template segment<3>(0);
    AngularVelocity bodyAngVel = u.template segment<3>(3);
    VectorXd jointVel = u.template segment<12>(6);

    observation_unscaled_[0] = command_[0];
    observation_unscaled_[1] = command_[1];
    observation_unscaled_[2] = command_[2];
    observation_unscaled_.template segment<3>(3) = e_g.template cast<Dtype>();;
    observation_unscaled_.template segment<3>(6) = bodyVel.template cast<Dtype>();
    observation_unscaled_.template segment<3>(9) = bodyAngVel.template cast<Dtype>();
    observation_unscaled_.template segment<12>(12) = q.tail(12).
        template cast<Dtype>(); /// position
    observation_unscaled_.template segment<12>(24) = jointVel.template cast<Dtype>();
    observation_unscaled_.template segment<12>(36) = (jointPositionTarget_ - q.tail(12)).template cast<Dtype>();
    for (size_t i = 0; i < 4; i++) {
      observation_unscaled_[48 + 2 * i] = std::sin(pi_[i]);
      observation_unscaled_[49 + 2 * i] = std::cos(pi_[i]);
      observation_unscaled_[56 + i] = piD_[i];
    }

    observation_scaled = (observation_unscaled_ - observationOffset_).cwiseProduct(observationScale_);

    Eigen::Matrix<Dtype, ObservationDim, -1> temp;
    temp = historyBuffer_;

    historyBuffer_.block(0, 1, ObservationDim, nStack_ - 1) = temp.block(0, 0, ObservationDim, nStack_ - 1);
    historyBuffer_.col(0) = observation_scaled.template cast<Dtype>();
  }

  static inline void QuattoEuler(const Quaternion &q, double &roll, double &pitch, double &yaw) {
    double ysqr = q[2] * q[2];

    // roll (x-axis rotation)
    double t0 = +2.0 * (q[0] * q[1] + q[2] * q[3]);
    double t1 = +1.0 - 2.0 * (q[1] * q[1] + ysqr);
    roll = std::atan2(t0, t1);

    // pitch (y-axis rotation)
    double t2 = +2.0 * (q[0] * q[2] - q[3] * q[1]);
    t2 = t2 > 1.0 ? 1.0 : t2;
    t2 = t2 < -1.0 ? -1.0 : t2;
    pitch = std::asin(t2);

    // yaw (z-axis rotation)
    double t3 = +2.0 * (q[0] * q[3] + q[1] * q[2]);
    double t4 = +1.0 - 2.0 * (ysqr + q[3] * q[3]);
    yaw = std::atan2(t3, t4);
  }

 private:
  inline double anglediff(double target, double source) {
    //https://stackoverflow.com/questions/1878907/the-smallest-difference-between-2-angles
    return anglemod(target - source);
  }

  inline double anglemod(double a) {
    return wrapAngle((a + M_PI)) - M_PI;
  }

  inline double wrapAngle(double a) {
    double twopi = 2.0 * M_PI;
    return a - twopi * fastfloor(a / twopi);
  }

  inline int fastfloor(double a) {
    int i = int(a);
    if (i > a) i--;
    return i;
  }

  inline void conversion_GeneralizedState2LearningState(State &state,
                                                        const VectorXd &q,
                                                        const VectorXd &u) {
    Quaternion quat = q.template segment<4>(3);
    RotationMatrix R_b = quatToRotMat(quat);
    e_g = R_b.row(2).transpose();

    /// velocity in body coordinate
    LinearVelocity bodyVel = R_b.transpose() * u.template segment<3>(0);
    AngularVelocity bodyAngVel = u.template segment<3>(3);
    VectorXd jointVel = u.template segment<12>(6);

    /// command slots
    state_unscaled_[0] = command_[0];
    state_unscaled_[1] = command_[1];
    state_unscaled_[2] = command_[2];

    /// gravity vector
    state_unscaled_.template segment<3>(3) = e_g.template cast<Dtype>();

    /// velocities
    state_unscaled_.template segment<3>(6) = bodyVel.template cast<Dtype>();
    state_unscaled_.template segment<3>(9) = bodyAngVel.template cast<Dtype>();

    state_unscaled_.template segment<12>(12) = q.template segment<12>(7).
        template cast<Dtype>();
    state_unscaled_.template segment<12>(24) = jointVel.template cast<Dtype>();
    state_unscaled_.template segment<12>(36) = jointPosHist_.template segment<12>((jointHistoryLength - 1) * 12);

    for (size_t i = 0; i < 4; i++) {
      state_unscaled_[48 + i] = piD_[i];
    }

    int pos = 52;
    for (size_t i = 0; i < 4; i++) {
      state_unscaled_[pos + 2 * i] = std::sin(pi_[i]);
      state_unscaled_[pos + 1 + 2 * i] = std::cos(pi_[i]);
    }
    pos += 8;

    state_unscaled_.template segment<12>(pos) =
        jointPosHist_.template segment<12>((jointHistoryLength - 4) * 12);
    pos += 12;

    state_unscaled_.template segment<12>(pos) =
        jointPosHist_.template segment<12>((jointHistoryLength - 9) * 12);
    pos += 12;

    state_unscaled_.template segment<12>(pos) =
        jointVelHist_.template segment<12>((jointHistoryLength - 4) * 12);
    pos += 12;

    state_unscaled_.template segment<12>(pos) =
        jointVelHist_.template segment<12>((jointHistoryLength - 9) * 12);
    pos += 12;

    state_unscaled_.template segment<12>(pos) = previousjointPositionTarget_.template cast<Dtype>();
    pos += 12;

    state_unscaled_.template segment<12>(pos) = previousjointPositionTarget2_.template cast<Dtype>();
    pos += 12;

    state_unscaled_[pos] = baseFreq_;
    state = (state_unscaled_ - stateOffset_).cwiseProduct(stateScale_);
  }

  inline RotationMatrix quatToRotMat(Quaternion &q) {
    RotationMatrix R;
    R << q(0) * q(0) + q(1) * q(1) - q(2) * q(2) - q(3) * q(3), 2 * q(1) * q(2) - 2 * q(0) * q(3), 2 * q(0) * q(2)
        + 2 * q(1) * q(3),
        2 * q(0) * q(3) + 2 * q(1) * q(2), q(0) * q(0) - q(1) * q(1) + q(2) * q(2) - q(3) * q(3), 2 * q(2) * q(3)
        - 2 * q(0) * q(1),
        2 * q(1) * q(3) - 2 * q(0) * q(2), 2 * q(0) * q(1) + 2 * q(2) * q(3), q(0) * q(0) - q(1) * q(1) - q(2) * q(2)
        + q(3) * q(3);
    return R;
  }

  template<typename T>
  int sgn(T val) {
    return (T(0) < val) - (val < T(0));
  }

  Eigen::Matrix<double, 12, 1> jointNominalConfig_;

  State stateOffset_;
  State stateScale_;
  Action actionOffset_;
  Action actionScale_;
  Action scaledAction_;
  Eigen::Matrix<double, 3, 1> e_g;
  Eigen::Matrix<double, 3, 1> x_, y_;
  Eigen::Matrix<Dtype, 3, 1> command_;
  Eigen::Matrix<Dtype, 12 * jointHistoryLength, 1> jointVelHist_, jointPosHist_, tempHist_;
  Observation observationScale_;
  Observation observationOffset_;
  std::vector<Eigen::Matrix<double, 3, 1>> footPos_Target;
  std::vector<Eigen::Matrix<double, 3, 1>> prevfootPos_Target;
  std::vector<Eigen::Matrix<double, 3, 1>> prevfootPos_Target2;
  Eigen::Matrix<double, 12, 1> footPositionOffset_;
  Eigen::Matrix<float, ObservationDim, -1> historyBuffer_;

  double h0_ = -0.55;
  double clearance_[4];

  double freqScale_ = 0.0025 * 2.0 * M_PI;
  double baseFreq_ = 0.0;

  unsigned long int controlCounter = 0;
  unsigned long int stopCounter = 0;
  unsigned long int historyUpdateCounter = 0;
  unsigned long int decimation = 8;

  InverseKinematics IK_;
  tensorflow::TensorShape state_dims_inv;
  tensorflow::Tensor state_tf_tensor;

  tensorflow::TensorShape history_dims_inv;
  tensorflow::Tensor history_tf_tensor;
  GraphLoader<float> *graph_;

 public:
  double pi_[4];
  double piD_[4];
  Action action_;
  State state_;
  Observation observation_unscaled_;
  State state_unscaled_;

  Eigen::Matrix<double, 12, 1> jointPositionTarget_;

  Eigen::Matrix<double, 12, 1> previousjointPositionTarget_;
  Eigen::Matrix<double, 12, 1> previousjointPositionTarget2_;
  int ObservationStride_ = 8;
  int nStack_ = 100;

};

}

#endif //RAI_QUADRUPEDCONTROLLER_HPP


/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2020,
 *  Max Planck Institute for Intelligent Systems (MPI-IS).
 *  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 MPI-IS 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.
 *********************************************************************/

/* Author: Andreas Orthey, Sohaib Akbar */

#include <ompl/multilevel/planners/qmp/QMPImpl.h>
#include <ompl/tools/config/SelfConfig.h>
#include <boost/foreach.hpp>
#include <ompl/datastructures/NearestNeighbors.h>
#include "ompl/datastructures/PDF.h"
#include <ompl/geometric/PathGeometric.h>
#include <ompl/multilevel/datastructures/graphsampler/GraphSampler.h>

#define foreach BOOST_FOREACH

ompl::multilevel::QMPImpl::QMPImpl(const base::SpaceInformationPtr &si, BundleSpace *parent_) : BaseT(si, parent_)
{
    setName("QMPImpl" + std::to_string(id_));

    setMetric("geodesic");
    setGraphSampler("randomedge");
    setImportance("exponential");

    randomWorkStates_.resize(5);
    getBundle()->allocStates(randomWorkStates_);
}

ompl::multilevel::QMPImpl::~QMPImpl()
{
    getBundle()->freeStates(randomWorkStates_);
}

void ompl::multilevel::QMPImpl::clear()
{
    BaseT::clear();
    pdf.clear();
}

ompl::multilevel::BundleSpaceGraph::Vertex ompl::multilevel::QMPImpl::addConfiguration(Configuration *q)
{
    Vertex m = BaseT::addConfiguration(q);
    PDF_Element *q_element = pdf.add(q, 1.0);
    q->setPDFElement(q_element);
    return m;
}

void ompl::multilevel::QMPImpl::deleteConfiguration(Configuration *q)
{
    q->setPDFElement(nullptr);
    BaseT::deleteConfiguration(q);
}

void ompl::multilevel::QMPImpl::updatePDF(Configuration *q)
{
    unsigned int N = q->total_connection_attempts;
    unsigned int Ns = q->successful_connection_attempts;
    double val = (double)(N - Ns) / (double)N;
    pdf.update(static_cast<PDF_Element *>(q->getPDFElement()), val);
}

unsigned int ompl::multilevel::QMPImpl::computeK()
{
    return k_NearestNeighbors_;
}

void ompl::multilevel::QMPImpl::grow()
{
    if (firstRun_)
    {
        init();
        firstRun_ = false;

        if (qGoal_ == nullptr)
        {
            qGoal_ = new Configuration(getBundle());
            qGoal_->isGoal = true;
        }
        getGoalPtr()->sampleGoal(qGoal_->state);
        addConfiguration(qGoal_);
        addGoalConfiguration(qGoal_);

        findSection();
    }

    //(1) Get Random Sample
    if (!sampleBundleValid(xRandom_->state))
        return;

    //(2) Add Configuration if valid
    Configuration *xNew = new Configuration(getBundle(), xRandom_->state);
    addConfiguration(xNew);

    //(3) Connect to K nearest neighbors
    connectNeighbors(xNew);

    expand();

    if (!hasSolution_)
    {
        if (sameComponent(vStart_, getGoalIndex()))
        {
            hasSolution_ = true;
        }
    }
}

void ompl::multilevel::QMPImpl::connectNeighbors(Configuration *x)
{
    std::vector<Configuration *> nearestNeighbors;

    BaseT::nearestDatastructure_->nearestK(x, computeK(), nearestNeighbors);

    for (unsigned int k = 0; k < nearestNeighbors.size(); k++)
    {
        Configuration *xNear = nearestNeighbors.at(k);

        x->total_connection_attempts++;
        xNear->total_connection_attempts++;

        if (connect(xNear, x))
        {
            x->successful_connection_attempts++;
            xNear->successful_connection_attempts++;
        }
        updatePDF(xNear);
    }
    updatePDF(x);
}

void ompl::multilevel::QMPImpl::expand()
{
    if (pdf.empty())
        return;

    Configuration *q = pdf.sample(rng_.uniform01());

    int s = getBundle()->randomBounceMotion(getBundleSamplerPtr(), q->state, randomWorkStates_.size(),
                                            randomWorkStates_, false);
    if (s > 0)
    {
        Configuration *prev = q;

        Configuration *last = new Configuration(getBundle(), randomWorkStates_[--s]);
        addConfiguration(last);
        connectNeighbors(last);

        for (int i = 0; i < s; i++)
        {
            Configuration *tmp = new Configuration(getBundle(), randomWorkStates_[i]);
            addConfiguration(tmp);

            ompl::multilevel::BundleSpaceGraph::addEdge(prev->index, tmp->index);
            prev = tmp;
        }
        if (!sameComponent(prev->index, last->index))
            ompl::multilevel::BundleSpaceGraph::addEdge(prev->index, last->index);
    }
}


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

#ifndef BOOST_MATH_TOOLS_BIG_CONSTANT_HPP
#define BOOST_MATH_TOOLS_BIG_CONSTANT_HPP

#include <boost/math/tools/config.hpp>
#ifndef BOOST_MATH_NO_LEXICAL_CAST
#include <boost/lexical_cast.hpp>
#endif
#include <boost/type_traits/is_convertible.hpp>

namespace boost{ namespace math{

namespace tools{

template <class T>
inline BOOST_CONSTEXPR_OR_CONST T make_big_value(long double v, const char*, mpl::true_ const&, mpl::false_ const&)
{
   return static_cast<T>(v);
}
template <class T>
inline BOOST_CONSTEXPR_OR_CONST T make_big_value(long double v, const char*, mpl::true_ const&, mpl::true_ const&)
{
   return static_cast<T>(v);
}
#ifndef BOOST_MATH_NO_LEXICAL_CAST
template <class T>
inline T make_big_value(long double, const char* s, mpl::false_ const&, mpl::false_ const&)
{
   return boost::lexical_cast<T>(s);
}
#endif
template <class T>
inline BOOST_CONSTEXPR const char* make_big_value(long double, const char* s, mpl::false_ const&, mpl::true_ const&)
{
   return s;
}

//
// For constants which might fit in a long double (if it's big enough):
//
#define BOOST_MATH_BIG_CONSTANT(T, D, x)\
   boost::math::tools::make_big_value<T>(\
      BOOST_JOIN(x, L), \
      BOOST_STRINGIZE(x), \
      mpl::bool_< (is_convertible<long double, T>::value) && \
         ((D <= std::numeric_limits<long double>::digits) \
          || is_floating_point<T>::value \
          || (std::numeric_limits<T>::is_specialized && \
             (std::numeric_limits<T>::digits10 <= std::numeric_limits<long double>::digits10))) >(), \
      boost::is_convertible<const char*, T>())
//
// For constants too huge for any conceivable long double (and which generate compiler errors if we try and declare them as such):
//
#define BOOST_MATH_HUGE_CONSTANT(T, D, x)\
   boost::math::tools::make_big_value<T>(0.0L, BOOST_STRINGIZE(x), \
   mpl::bool_<is_floating_point<T>::value || (std::numeric_limits<T>::is_specialized && std::numeric_limits<T>::max_exponent <= std::numeric_limits<long double>::max_exponent && std::numeric_limits<T>::digits <= std::numeric_limits<long double>::digits)>(), \
   boost::is_convertible<const char*, T>())

}}} // namespaces

#endif


// Copyright (c) 2012 Christopher Lux <christopherlux@gmail.com>
// Distributed under the Modified BSD License, see license.txt.

#include "height_field_data.h"

#include <algorithm>
#include <cassert>
#include <exception>
#include <stdexcept>
#include <limits>

#include <boost/assign/list_of.hpp>

#include <scm/log.h>
#include <scm/core/log/logger_state.h>

#include <scm/gl_util/data/analysis/transfer_function/build_lookup_table.h>

#include <scm/gl_core/math.h>
#include <scm/gl_core/render_device.h>
#include <scm/gl_core/texture_objects.h>

#include <scm/gl_util/data/imaging/texture_image_data.h>
#include <scm/gl_util/data/imaging/texture_loader.h>
#include <scm/gl_util/primitives/box.h>

#include <renderers/patch_grid_mesh.h>

namespace scm {
namespace {

float
image_sample(const gl::texture_image_data_ptr& image_data,
             const math::vec2i&                sample_pos)
{
    using namespace scm::gl;
    using namespace scm::math;

    const vec2ui image_size  = image_data->mip_level(0).size();
    const vec2i  clamped_pos = clamp(sample_pos, vec2i(0, 0), vec2i(image_size) - vec2i(1, 1));

    if (image_data->format() == FORMAT_R_16) {
        const unsigned short*const data_ptr = reinterpret_cast<const unsigned short*const>(image_data->mip_level(0).data().get());
        const unsigned short       max_val  = (std::numeric_limits<unsigned short>::max)();
        const unsigned short       value    = data_ptr[clamped_pos.y * image_size.x + clamped_pos.x];

        return (static_cast<float>(value) / max_val);
    }
    else if (image_data->format() == FORMAT_R_8) {
        const unsigned char*const data_ptr = reinterpret_cast<const unsigned char*const>(image_data->mip_level(0).data().get());
        const unsigned char       max_val  = (std::numeric_limits<unsigned char>::max)();
        const unsigned char       value    = data_ptr[clamped_pos.y * image_size.x + clamped_pos.x];

        return (static_cast<float>(value) / max_val);
    }
    else if (image_data->format() == FORMAT_R_32F) {
        const float*const data_ptr = reinterpret_cast<const float*const>(image_data->mip_level(0).data().get());
        const float       value    = data_ptr[clamped_pos.y * image_size.x + clamped_pos.x];

        return (value);
    }

    return (0.0f);
}

struct null_deleter
{
    void operator()(void const *) const {
    }
}; // null_deleter

} // namespace

namespace data {

height_field_data::height_field_data(const gl::render_device_ptr& device,
                                     const std::string&           file_name,
                                     const math::vec3f&           height_field_extends)
  : _extends(height_field_extends)
  , _bbox(math::vec3f(0.0f), height_field_extends)
  , _transform(math::mat4f::identity())
{
    using namespace scm::gl;
    using namespace scm::math;
    using boost::assign::list_of;

    const vec2ui            patch_size = vec2ui(64u);

    log::logger_format_saver save_indent(out().associated_logger());

    out() << log::info << "height_field_data::height_field_data()" << log::indent;
    out() << log::info << "loading height map image ('" << file_name << "')." << log::end;

    texture_loader          tex_loader;
    texture_image_data_ptr  image_data = tex_loader.load_image_data(file_name);

    if (!image_data) {
        throw (std::runtime_error("height_field_data::height_field_data(): error loading height field image data from file: " + file_name));
    }

    out() << log::info << "opened height field "
          << "(image size: " << image_data->mip_level(0).size()
          << ", patch size: " << patch_size
          << ", format: " << format_string(image_data->format()) << ")." << log::end;

    //out() << log::info << "padding height map to multiple of patch size "
    //      << "(image size: " << image_data->size()
    //      << ", patch size: " << patch_size
    //      << ", format: " << format_string(image_data->format()) << ")." << log::end;

    //image_data  = pad_to_patch_size(image_data, patch_size - 1);

    out() << log::info << "generating density map." << log::end;

    texture_image_data_ptr  density_data = generate_density_data(image_data, patch_size, height_field_extends);

    if (!density_data) {
        throw (std::runtime_error("height_field_data::height_field_data(): error generating density data."));
    }

    out() << log::info << "generating textures." << log::end;

    _height_map = device->create_texture_2d(image_data->mip_level(0).size(), image_data->format(), 1, 1, 1,
                                            image_data->format(), list_of(image_data->mip_level(0).data().get()));
    device->main_context()->generate_mipmaps(_height_map);

    _density_map = device->create_texture_2d(density_data->mip_level(0).size(), density_data->format(), 1, 1, 1,
                                             density_data->format(), list_of(density_data->mip_level(0).data().get()));

    if (!_height_map) {
        throw (std::runtime_error("height_field_data::height_field_data(): error loading height field texture."));
    }
    if (!_density_map) {
        throw (std::runtime_error("height_field_data::height_field_data(): error loading density texture"));
    }

    const vec2ui texture_size    = _height_map->descriptor()._size;
    const vec2ui patch_grid_res  = vec2ui(ceil(vec2f(texture_size) / vec2f(patch_size)));
    const vec2f  patch_grid_size = vec2f(patch_grid_res * patch_size) / vec2f(texture_size);
    const vec2f  patch_extends   = vec3f(patch_grid_size, height_field_extends.z);

    _transform = make_translation(-_extends / 2.0f);

    out() << log::info << "generation edge density buffers." << log::end;
    render_context_ptr ctx = device->main_context();
    { // tri edge density buffer
        _triangle_edge_density_buffer = device->create_texture_buffer(FORMAT_RGBA_32F, USAGE_STATIC_DRAW,
                                                                      patch_grid_res.x * patch_grid_res.y * 2 * 2 * sizeof(vec4f));// two triangles per grid cell, outer and inner tessellation factors
        if (!_triangle_edge_density_buffer) {
            throw (std::runtime_error("height_field_data::height_field_data(): error creating triangle edge density buffer."));
        }
        vec4f* data = static_cast<vec4f*>(ctx->map_buffer(_triangle_edge_density_buffer->descriptor()._buffer,
                                                          ACCESS_WRITE_INVALIDATE_BUFFER));

        if (!data) {
            throw (std::runtime_error("height_field_data::height_field_data(): unable map triangle edge density buffer."));
        }
        for (unsigned y = 0; y < patch_grid_res.y; ++y) {
            for (unsigned x = 0; x < patch_grid_res.x; ++x) {
                data[4 * (y * patch_grid_res.x + x) + 0] = vec4f(1.0f); // tri 0 outer
                data[4 * (y * patch_grid_res.x + x) + 1] = vec4f(1.0f); // tri 0 inner
                data[4 * (y * patch_grid_res.x + x) + 2] = vec4f(1.0f); // tri 1 outer
                data[4 * (y * patch_grid_res.x + x) + 3] = vec4f(1.0f); // tri 1 inner
            }
        }
        ctx->unmap_buffer(_triangle_edge_density_buffer->descriptor()._buffer);
    }

    out() << log::info << "generation color lookup texture." << log::end;
    _color_transfer.clear();

    _color_transfer.add_stop(0.0f,  vec3f(0.0f, 0.0f, 1.0f));
    _color_transfer.add_stop(0.25f, vec3f(0.0f, 1.0f, 1.0f));
    _color_transfer.add_stop(0.50f, vec3f(0.0f, 1.0f, 0.0f));
    _color_transfer.add_stop(0.75f, vec3f(1.0f, 1.0f, 0.0f));
    _color_transfer.add_stop(1.0f,  vec3f(1.0f, 0.0f, 0.0f));

    _color_map = create_color_map(*device, 512, _color_transfer);
    if (!_color_map) {
        throw std::runtime_error("height_field_data::height_field_data(): error creating color lookup map.");
    }

    out() << log::info << "generating patch and bbox geometries." << log::end;

    try {
        _bbox_geometry = make_shared<box_geometry>(device, _bbox.min_vertex(), _bbox.max_vertex());
        _patch_mesh    = make_shared<patch_grid_mesh>(device, texture_size, patch_size, vec2f(height_field_extends));
    }
    catch (const std::exception& e) {
        throw (std::runtime_error(std::string("height_field_data::height_field_data(): error creating geometry objects: ") + e.what()));
    }
}

height_field_data::~height_field_data()
{
    _bbox_geometry.reset();
    _height_map.reset();
    _density_map.reset();
    _quad_edge_density_buffer.reset();
    _triangle_edge_density_buffer.reset();
    _patch_mesh.reset();
}

const math::vec3f&
height_field_data::extends() const
{
    return (_extends);
}

const math::mat4f&
height_field_data::transform() const
{
    return (_transform);
}

void
height_field_data::transform(const math::mat4f& m)
{
    _transform = m;
}

const gl::box&
height_field_data::bbox() const
{
    return (_bbox);
}

const gl::box_geometry_ptr&
height_field_data::bbox_geometry() const
{
    return (_bbox_geometry);
}

const gl::texture_2d_ptr&
height_field_data::height_map() const
{
    return (_height_map);
}

const gl::texture_2d_ptr&
height_field_data::density_map() const
{
    return (_density_map);
}

const gl::texture_1d_ptr&
height_field_data::color_map() const
{
    return (_color_map);
}

const gl::texture_buffer_ptr&
height_field_data::quad_edge_density_buffer() const
{
    return (_quad_edge_density_buffer);
}

const gl::texture_buffer_ptr&
height_field_data::triangle_edge_density_buffer() const
{
    return (_triangle_edge_density_buffer);
}

const data::patch_grid_mesh_ptr&
height_field_data::patch_mesh() const
{
    return (_patch_mesh);
}

gl::texture_image_data_ptr
height_field_data::generate_density_data(const gl::texture_image_data_ptr& src_image,
                                         const math::vec2ui&               patch_size,
                                         const math::vec3f&                height_field_extends) const
{
    using namespace scm::gl;
    using namespace scm::math;

    out() << log::info << "height_field_data::generate_density_data()" << log::indent;

    scm::size_t         image_data_size = static_cast<size_t>(src_image->mip_level(0).size().x) * src_image->mip_level(0).size().y * sizeof(float);
    shared_array<uint8> image_gradients_array(new uint8[image_data_size]);
    float*              image_gradients = reinterpret_cast<float*>(image_gradients_array.get());

    vec2ui grid_resolution = vec2ui(floor(vec2f(vec2ui(src_image->mip_level(0).size())) / vec2f(patch_size - 1)));
    vec2f  os_patch_size   = vec2f(height_field_extends) / grid_resolution;
    float  max_os_patch_dim = max(os_patch_size.x, os_patch_size.y);

    out() << "grid_resolution " << grid_resolution << log::end;
    out() << "os_patch_size " << os_patch_size << log::end;
#if 0

    vec3f delta = height_field_extends / vec3f(static_cast<float>(src_image->size().x),
                                               static_cast<float>(src_image->size().y),
                                               1.0f);//pow(2.0f, 8 * size_of_channel(src_image->format())));

    float height_scale = 1.0f / pow(2.0f, 8 * size_of_channel(src_image->format()));

    for (int y = 0; y < static_cast<int>(src_image->size().y); ++y) {
        for (int x = 0; x < static_cast<int>(src_image->size().x); ++x) {
            //float c  = image_sample(src_image, vec2i(x,     y   ));
            float l  = image_sample(src_image, vec2i(x - 1, y   ));
            float r  = image_sample(src_image, vec2i(x + 1, y   ));
            float t  = image_sample(src_image, vec2i(x,     y + 1));
            float b  = image_sample(src_image, vec2i(x,     y - 1));
            //float ul = image_sample(src_image, vec2i(x - 1, y + 1));
            //float ll = image_sample(src_image, vec2i(x - 1, y - 1));
            //float ur = image_sample(src_image, vec2i(x + 1, y + 1));
            //float lr = image_sample(src_image, vec2i(x + 1, y - 1));

            //float dx = (r - l) * 0.5f;
            //float dy = (t - b) * 0.5f;
            //image_gradients[y * src_image->size().x + x] = clamp(sqrt(dx*dx + dy*dy), 0.0f, 1.0f);

            float dx = (r - l) * height_field_extends.z;
            float dy = (t - b) * height_field_extends.z;
            image_gradients[y * src_image->size().x + x] = clamp(sqrt(dx*dx + dy*dy), 0.0f, 1.0f) / max_os_patch_dim;

            //vec3f dx = vec3f(delta.x, 0.0f, (r - l) * delta.z);
            //vec3f dy = vec3f(0.0f, delta.y, (t - b) * delta.z);

            //image_gradients[y * src_image->size().x + x] = length(cross(dx, dy));
        }
    }
    texture_image_data_ptr image_gradient_data(new texture_image_data(src_image->size(), FORMAT_R_32F, image_gradients_array));

    vec2ui              density_image_size = vec2ui(floor(vec2f(src_image->size()) / (patch_size - 1)));
    shared_array<uint8> density_array(new uint8[density_image_size.x * density_image_size.y * sizeof(float)]);
    float*              density_image = reinterpret_cast<float*>(density_array.get());

    out() << log::info << "density image size: " << density_image_size << "." << log::end;

    for (int y = 0; y < static_cast<int>(density_image_size.y); ++y) {
        for (int x = 0; x < static_cast<int>(density_image_size.x); ++x) {
            float max_grad = -(std::numeric_limits<float>::max)();

            for (int iy = 0; iy < static_cast<int>(patch_size.y); ++iy) {
                for (int ix = 0; ix < static_cast<int>(patch_size.x); ++ix) {
                    float v = image_sample(image_gradient_data, vec2i(x * (patch_size.x - 1) + ix,
                                                                      y * (patch_size.y - 1) + iy));
                    max_grad = max(max_grad, v);
                }
            }

            density_image[y * density_image_size.x + x] = max_grad;
        }
    }

    texture_image_data_ptr density_data(new texture_image_data(density_image_size, FORMAT_R_32F, density_array));
#else
    texture_image_data::level_vector img_grad_mip_data;
    img_grad_mip_data.push_back(texture_image_data::level(src_image->mip_level(0).size(), image_gradients_array));
    texture_image_data_ptr image_gradient_data(new texture_image_data(texture_image_data::ORIGIN_LOWER_LEFT, FORMAT_R_32F, img_grad_mip_data));

    vec2ui              density_image_size = vec2ui(floor(vec2f(vec2ui(src_image->mip_level(0).size())) / (patch_size - 1)));
    shared_array<uint8> density_array(new uint8[density_image_size.x * density_image_size.y * sizeof(float)]);
    float*              density_image = reinterpret_cast<float*>(density_array.get());

    out() << log::info << "density image size: " << density_image_size << "." << log::end;

    for (int y = 0; y < static_cast<int>(density_image_size.y); ++y) {
        for (int x = 0; x < static_cast<int>(density_image_size.x); ++x) {
            float max_grad = -(std::numeric_limits<float>::max)();

            float min_val = (std::numeric_limits<float>::max)();
            float max_val = -(std::numeric_limits<float>::max)();

            for (int iy = 0; iy < static_cast<int>(patch_size.y); ++iy) {
                for (int ix = 0; ix < static_cast<int>(patch_size.x); ++ix) {

                    float v  = image_sample(src_image, vec2i(x * (patch_size.x - 1) + ix, y * (patch_size.y - 1) + iy));

                    min_val = min(min_val, v);
                    max_val = max(max_val, v);
                }
            }

            density_image[y * density_image_size.x + x] = (max_val - min_val) * height_field_extends.z / max_os_patch_dim;
        }
    }

    texture_image_data::level_vector img_dens_mip_data;
    img_dens_mip_data.push_back(texture_image_data::level(density_image_size, density_array));
    texture_image_data_ptr density_data(new texture_image_data(texture_image_data::ORIGIN_LOWER_LEFT, FORMAT_R_32F, img_dens_mip_data));
#endif
    return (density_data);
}

gl::texture_image_data_ptr
height_field_data::pad_to_patch_size(const gl::texture_image_data_ptr& src_image,
                                     const math::vec2ui&               patch_size) const
{
    using namespace scm::gl;
    using namespace scm::math;

    out() << log::info << "height_field_data::pad_to_patch_size()" << log::indent;

    vec2ui              padded_image_size      = vec2ui(ceil(vec2f(vec2ui(src_image->mip_level(0).size())) / patch_size)) * patch_size;
    scm::size_t         padded_image_data_size = static_cast<size_t>(padded_image_size.x) * padded_image_size.y * size_of_format(src_image->format());
    shared_array<uint8> padded_image_array(new uint8[padded_image_data_size]);

    out() << log::info << "padded image size: " << padded_image_size << "." << log::end;

    if (src_image->format() == FORMAT_R_8) {
        unsigned char* padded_image = reinterpret_cast<unsigned char*>(padded_image_array.get());
        unsigned char* source_image = reinterpret_cast<unsigned char*>(src_image->mip_level(0).data().get());

        // clear data array to 0
        std::fill(padded_image, padded_image + (static_cast<size_t>(padded_image_size.x) * padded_image_size.y), 0);

        for (int y = 0; y < static_cast<int>(src_image->mip_level(0).size().y); ++y) {
            memcpy(padded_image + y * padded_image_size.x,
                   source_image + y * src_image->mip_level(0).size().x,
                   src_image->mip_level(0).size().x * size_of_format(src_image->format()));
        }
    }
    else if (src_image->format() == FORMAT_R_16) {
        unsigned short* padded_image = reinterpret_cast<unsigned short*>(padded_image_array.get());
        unsigned short* source_image = reinterpret_cast<unsigned short*>(src_image->mip_level(0).data().get());

        // clear data array to 0
        std::fill(padded_image, padded_image + (static_cast<size_t>(padded_image_size.x) * padded_image_size.y), 0);

        for (int y = 0; y < static_cast<int>(src_image->mip_level(0).size().y); ++y) {
            memcpy(padded_image + y * padded_image_size.x,
                   source_image + y * src_image->mip_level(0).size().x,
                   src_image->mip_level(0).size().x * size_of_format(src_image->format()));
        }
    }
    else if (src_image->format() == FORMAT_R_32F) {
        float*              padded_image           = reinterpret_cast<float*>(padded_image_array.get());
        float*              source_image           = reinterpret_cast<float*>(src_image->mip_level(0).data().get());

        // clear data array to 0.0f
        std::fill(padded_image, padded_image + (static_cast<size_t>(padded_image_size.x) * padded_image_size.y), 0.0f);

        for (int y = 0; y < static_cast<int>(src_image->mip_level(0).size().y); ++y) {
            memcpy(padded_image + y * padded_image_size.x,
                   source_image + y * src_image->mip_level(0).size().x,
                   src_image->mip_level(0).size().x * size_of_format(src_image->format()));
        }
    }

    texture_image_data::level_vector img_pad_mip_data;
    img_pad_mip_data.push_back(texture_image_data::level(padded_image_size, padded_image_array));
    texture_image_data_ptr padded_data(new texture_image_data(texture_image_data::ORIGIN_LOWER_LEFT, src_image->format(), img_pad_mip_data));

    return (padded_data);
}

gl::texture_1d_ptr
height_field_data::create_color_map(gl::render_device& in_device,
                                    unsigned in_size,
                                    const color_transfer_type& in_color) const
{
    using namespace scm::gl;
    using namespace scm::math;

    log::logger_format_saver save_indent(out().associated_logger());
    out() << log::indent;

    scm::scoped_array<scm::math::vec3f>  color_lut;

    color_lut.reset(new vec3f[in_size]);

    if (   !scm::data::build_lookup_table(color_lut, in_color, in_size)) {
        scm::err() << "height_field_data::create_color_map(): error during lookuptable generation" << log::end;
        return (texture_1d_ptr());
    }
    scm::scoped_array<float> combined_lut;

    combined_lut.reset(new float[in_size * 4]);

    for (unsigned i = 0; i < in_size; ++i) {
        combined_lut[i*4   ] = color_lut[i].x;
        combined_lut[i*4 +1] = color_lut[i].y;
        combined_lut[i*4 +2] = color_lut[i].z;
        combined_lut[i*4 +3] = 1.0f;
    }

    std::vector<void*> in_data;
    in_data.push_back(combined_lut.get());

    texture_1d_ptr new_tex = in_device.create_texture_1d(in_size, FORMAT_RGBA_8, 1, 1, FORMAT_RGBA_32F, in_data);

    if (!new_tex) {
        scm::err() << "height_field_data::create_color_map(): error during color map texture generation." << log::end;
        return (texture_1d_ptr());
    }

    return (new_tex);
}

} // namespace data
} // namespace scm