// ======================================================================

#include "Projection.h"
#include "Config.h"
#include "Hash.h"
#include <boost/algorithm/string/predicate.hpp>
#include <boost/filesystem/operations.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/numeric/conversion/cast.hpp>
#include <engines/gis/Engine.h>
#include <fmt/format.h>
#include <gis/Box.h>
#include <gis/CoordinateTransformation.h>
#include <grid-files/common/GeneralFunctions.h>
#include <macgyver/Exception.h>
#include <spine/HTTP.h>
#include <cmath>
#include <ogr_geometry.h>
#include <ogr_spatialref.h>
#include <stdexcept>

namespace SmartMet
{
namespace Plugin
{
namespace Dali
{
// ----------------------------------------------------------------------
/*!
 * \brief Initialize the projection from JSON
 */
// ----------------------------------------------------------------------

void Projection::init(const Json::Value& theJson,
                      const State& theState,
                      const Config& /* theConfig */)
{
  try
  {
    if (!theJson.isObject())
      throw Fmi::Exception(BCP, "Projection JSON is not a JSON object (name-value pairs)");

    // For later use
    gisengine = &theState.getGisEngine();

    // Extract all the members

    Json::Value nulljson;
    auto request = theState.getRequest();
    boost::optional<std::string> v;

    auto json = theJson.get("crs", nulljson);
    if (!json.isNull())
      crs = json.asString();

    json = theJson.get("bboxcrs", nulljson);
    if (!json.isNull())
      bboxcrs = json.asString();

    if (crs && *crs == "data")
    {
      json = theJson.get("size", nulljson);
      if (!json.isNull())
        size = json.asDouble();

      v = request.getParameter("size");
      if (v)
        size = toDouble(*v);
    }

    if (!size || *size <= 0)
    {
      json = theJson.get("xsize", nulljson);
      if (!json.isNull())
        xsize = json.asInt();

      json = theJson.get("ysize", nulljson);
      if (!json.isNull())
        ysize = json.asInt();

      v = request.getParameter("xsize");
      if (v)
        xsize = toInt32(*v);

      v = request.getParameter("ysize");
      if (v)
        ysize = toInt32(*v);
    }

    json = theJson.get("x1", nulljson);
    if (!json.isNull())
      x1 = json.asDouble();

    json = theJson.get("y1", nulljson);
    if (!json.isNull())
      y1 = json.asDouble();

    json = theJson.get("x2", nulljson);
    if (!json.isNull())
      x2 = json.asDouble();

    json = theJson.get("y2", nulljson);
    if (!json.isNull())
      y2 = json.asDouble();

    json = theJson.get("cx", nulljson);
    if (!json.isNull())
      cx = json.asDouble();

    json = theJson.get("cy", nulljson);
    if (!json.isNull())
      cy = json.asDouble();

    json = theJson.get("resolution", nulljson);
    if (!json.isNull())
      resolution = json.asDouble();

    // Note: intentionally after cx,cy parsing to enable name overrides

    json = theJson.get("geoid", nulljson);
    if (!json.isNull())
    {
      const auto& engine = theState.getGeoEngine();
      // TODO(mheiskan): Make the language configurable
      int id = json.asInt();
      auto loc = engine.idSearch(id, "fi");
      if (!loc)
        throw Fmi::Exception(BCP,
                             "Unable to find coordinates for geoid '" + Fmi::to_string(id) + "'");
      latlon_center = true;
      cx = loc->longitude;
      cy = loc->latitude;
    }

    json = theJson.get("place", nulljson);
    if (!json.isNull())
    {
      const auto& engine = theState.getGeoEngine();
      // TODO(mheiskan): Make the language configurable
      std::string name = json.asString();
      auto loc = engine.nameSearch(name, "fi");
      if (!loc)
        throw Fmi::Exception(BCP, "Unable to find coordinates for location '" + name + "'");

      latlon_center = true;
      cx = loc->longitude;
      cy = loc->latitude;
    }

    // Note: this must be handled after the CRS variables!

    json = theJson.get("bbox", nulljson);
    if (!json.isNull())
    {
      std::string bbox = json.asString();
      std::vector<std::string> parts;
      boost::algorithm::split(parts, bbox, boost::algorithm::is_any_of(","));
      if (parts.size() != 4)
        throw Fmi::Exception(BCP, "bbox should contain 4 comma separated doubles");

      // for example in EPSG:4326 order of coordinates is changed in WMS
      // for more info: https://trac.osgeo.org/gdal/wiki/rfc20_srs_axes

      OGRSpatialReference spatref;
      if (bboxcrs)
      {
        // SetFromUserInput wants 'EPSGA' - code in order to understand axis ordering
        if (boost::algorithm::starts_with(*bboxcrs, "EPSG:"))
          boost::algorithm::replace_first(*bboxcrs, "EPSG:", "EPSGA:");

        spatref.SetFromUserInput(bboxcrs->c_str());
        spatref.SetAxisMappingStrategy(OAMS_TRADITIONAL_GIS_ORDER);
      }
      else
      {
        // SetFromUserInput wants 'EPSGA' - code in order to understand axis ordering
        if (boost::algorithm::starts_with(*crs, "EPSG:"))
          boost::algorithm::replace_first(*crs, "EPSG:", "EPSGA:");

        spatref.SetFromUserInput(crs->c_str());
      }

      bool latLonOrder = spatref.EPSGTreatsAsLatLong() != 0;

      if (latLonOrder)
      {
        y1 = Fmi::stod(parts[0]);
        x1 = Fmi::stod(parts[1]);
        y2 = Fmi::stod(parts[2]);
        x2 = Fmi::stod(parts[3]);
      }
      else
      {
        x1 = Fmi::stod(parts[0]);
        y1 = Fmi::stod(parts[1]);
        x2 = Fmi::stod(parts[2]);
        y2 = Fmi::stod(parts[3]);
      }
    }
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief Hash value
 */
// ----------------------------------------------------------------------

std::size_t Projection::hash_value(const State& /* theState */) const
{
  try
  {
    auto hash = Fmi::hash_value(crs);
    Fmi::hash_combine(hash, Fmi::hash_value(size));
    Fmi::hash_combine(hash, Fmi::hash_value(xsize));
    Fmi::hash_combine(hash, Fmi::hash_value(ysize));
    Fmi::hash_combine(hash, Fmi::hash_value(x1));
    Fmi::hash_combine(hash, Fmi::hash_value(y1));
    Fmi::hash_combine(hash, Fmi::hash_value(x2));
    Fmi::hash_combine(hash, Fmi::hash_value(y2));
    Fmi::hash_combine(hash, Fmi::hash_value(cx));
    Fmi::hash_combine(hash, Fmi::hash_value(cy));
    Fmi::hash_combine(hash, Fmi::hash_value(resolution));
    Fmi::hash_combine(hash, Fmi::hash_value(bboxcrs));
    Fmi::hash_combine(hash, Fmi::hash_value(size));
    return hash;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief Update area with querydata if necessary
 */
// ----------------------------------------------------------------------

void Projection::update(const Engine::Querydata::Q& theQ)
{
  if (!theQ)
    return;

  try
  {
    if (crs && *crs == "data")
    {
      crs = theQ->SpatialReference().projStr();
      bool no_bbox = (!x1 && !y1 && !x2 && !y2 && !cx && !cy && !resolution && !bboxcrs);

      if (no_bbox)
      {
        auto world1 = theQ->area().XYToWorldXY(theQ->area().BottomLeft());
        auto world2 = theQ->area().XYToWorldXY(theQ->area().TopRight());
        x1 = world1.X();
        y1 = world1.Y();
        x2 = world2.X();
        y2 = world2.Y();
      }

      if (size)
      {
        xsize = C_INT(C_DOUBLE(theQ->info()->GridXNumber()) * (*size));
        ysize = C_INT(C_DOUBLE(theQ->info()->GridYNumber()) * (*size));
      }

      ogr_crs.reset();
      box.reset();
    }
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief Return the spatial reference
 */
// ----------------------------------------------------------------------

const Fmi::SpatialReference& Projection::getCRS() const
{
  try
  {
    prepareCRS();
    return *ogr_crs;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief Return the spatial area
 */
// ----------------------------------------------------------------------

const Fmi::Box& Projection::getBox() const
{
  try
  {
    prepareCRS();
    return *box;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief Create OGR spatial reference from the settings
 */
// ----------------------------------------------------------------------

void Projection::prepareCRS() const
{
  try
  {
    // Done already?
    if (ogr_crs)
      return;

    if (!crs)
      throw Fmi::Exception(BCP, "CRS not set, unable to create projection");

    if (!xsize && !ysize)
    {
      if (size)
        return;

      throw Fmi::Exception(BCP, "CRS xsize and ysize are both missing");
    }

    // Are subdefinitions complete?
    bool full_rect_bbox = (x1 && y1 && x2 && y2);
    bool full_center_bbox = (cx && cy && resolution);

    // Are they partial?
    bool partial_rect_bbox = !full_rect_bbox && (x1 || y1 || x2 || y2);
    bool partial_center_bbox = !full_center_bbox && (cx || cy || resolution);

    // Disallow partial definitions
    if (partial_rect_bbox)
      throw Fmi::Exception(BCP, "Partial CRS bounding box given: x1,y1,x2,y2 are needed");
    if (partial_center_bbox)
      throw Fmi::Exception(BCP, "Partial CRS bounding box given: cx,cy,resolution are needed");

    // bbox definition missing completely?
    if (!full_rect_bbox && !full_center_bbox)
      throw Fmi::Exception(BCP,
                           "CRS bounding box missing: x1,y2,x2,y2 or cx,cy,resolution are needed");

// two conflicting definitions is OK, since we create the corners from centered bbox
#if 0
        if(full_rect_bbox && full_center_bbox)
      throw Fmi::Exception(BCP,"CRS bounding box overdefined: use only x1,y2,x2,y2 or cx,cy,resolution");
#endif

    // must give both width and height if centered bbox is given
    if (!full_rect_bbox && full_center_bbox && (!xsize || !ysize))
      throw Fmi::Exception(BCP,
                           "CRS xsize and ysize are required when a centered bounding box is used");

    // Create the CRS
    ogr_crs = std::make_shared<Fmi::SpatialReference>(*crs);

    if (xsize && *xsize <= 0)
      throw Fmi::Exception(BCP, "Projection xsize must be positive");

    if (ysize && *ysize <= 0)
      throw Fmi::Exception(BCP, "Projection ysize must be positive");

    // World XY bounding box will be calculated during the process

    double XMIN, YMIN, XMAX, YMAX;

    // Create the Contour::Area object

    if (full_rect_bbox)
    {
      // rect bounding box

      XMIN = *x1;
      YMIN = *y1;
      XMAX = *x2;
      YMAX = *y2;

      if (bboxcrs)
      {
        // Reproject corners coordinates from bboxcrs to crs
        Fmi::CoordinateTransformation transformation(*bboxcrs, *ogr_crs);
        transformation.transform(XMIN, YMIN);
        transformation.transform(XMAX, YMAX);
      }

      if (XMIN == XMAX || YMIN == YMAX)
        throw Fmi::Exception(BCP, "Bounding box size is zero!");

      if (!xsize)
      {
        // Preserve aspect by calculating xsize
        int w = boost::numeric_cast<int>((*ysize) * (XMAX - XMIN) / (YMAX - YMIN));
        box = std::make_shared<Fmi::Box>(XMIN, YMIN, XMAX, YMAX, w, *ysize);
      }
      else if (!ysize)
      {
        // Preserve aspect by calculating ysize
        int h = boost::numeric_cast<int>((*xsize) * (YMAX - YMIN) / (XMAX - XMIN));
        box = std::make_shared<Fmi::Box>(XMIN, YMIN, XMAX, YMAX, *xsize, h);
      }
      else
      {
        box = std::make_shared<Fmi::Box>(XMIN, YMIN, XMAX, YMAX, *xsize, *ysize);
      }

      if (ogr_crs->isGeographic() != 0)
      {
        // Substract equations 5 and 4, subsititute equation 3 and solve resolution
        double pi = boost::math::constants::pi<double>();
        double circumference = 2 * pi * 6371.220;
        resolution = (YMAX - YMIN) * circumference / (360 * (*ysize));
      }
      else
      {
        // Substract equations 1 and 2 to eliminate and solve resolution
        resolution = (XMAX - XMIN) / ((*xsize) * 1000);
      }
    }

    else
    {
      // centered bounding box
      if (!xsize || !ysize)
        throw Fmi::Exception(BCP,
                             "xsize and ysize are required when a centered bounding box is used");

      double CX = *cx, CY = *cy;

      if (latlon_center || (bboxcrs && *crs != *bboxcrs))
      {
        // Reproject center coordinates from latlon/bboxcrs to crs
        Fmi::CoordinateTransformation transformation(latlon_center ? "WGS84" : bboxcrs->c_str(),
                                                     *ogr_crs);
        transformation.transform(CX, CY);
      }

      if (ogr_crs->isGeographic() != 0)
      {
        double pi = boost::math::constants::pi<double>();
        double circumference = 2 * pi * 6371.220;

        // This will work well, since we move along an isocircle
        double dy = 360 * (*ysize) / 2.0 * (*resolution) / circumference;  // Equation 3.
        YMIN = CY - dy;                                                    // Equation 4.
        YMAX = CY + dy;                                                    // Equation 5

        // Distances will become distorted the further away we are from the equator
        double dx = 360 * (*xsize) / 2.0 * (*resolution) / circumference / cos(CX * pi / 180.0);
        XMIN = CX - dx;
        XMAX = CX + dx;

        box = std::make_shared<Fmi::Box>(XMIN, YMIN, XMAX, YMAX, *xsize, *ysize);
      }
      else
      {
        XMIN = CX - (*xsize) / 2.0 * (*resolution) * 1000;  // Equation 1.
        XMAX = CX + (*xsize) / 2.0 * (*resolution) * 1000;  // Equation 2.
        YMIN = CY - (*ysize) / 2.0 * (*resolution) * 1000;
        YMAX = CY + (*ysize) / 2.0 * (*resolution) * 1000;
        box = std::make_shared<Fmi::Box>(XMIN, YMIN, XMAX, YMAX, *xsize, *ysize);
      }
    }

    // newbase corners calculated from world xy coordinates

    Fmi::CoordinateTransformation transformation(*ogr_crs, "FMI");

    // Calculate bottom left and top right coordinates

    transformation.transform(XMIN, YMIN);
    itsBottomLeft = NFmiPoint(XMIN, YMIN);

    transformation.transform(XMAX, YMAX);
    itsTopRight = NFmiPoint(XMAX, YMAX);

#if 0
    std::cerr << std::setprecision(8) << "\nProjection:\n\n"
              << "\nxsize=" << (xsize ? *xsize : -1) << "\nysize=" << (ysize ? *ysize : -1)
              << "\nx1=" << (x1 ? *x1 : -1) << "\ny1=" << (y1 ? *y1 : -1)
              << "\nx2=" << (x2 ? *x2 : -1) << "\ny2=" << (y2 ? *y2 : -1)
              << "\ncx=" << (cx ? *cx : -1) << "\ncy=" << (cy ? *cy : -1)
              << "\nres=" << (resolution ? *resolution : -1) << "\nbl=" << itsBottomLeft.X() << ","
              << itsBottomLeft.Y() << "\ntr=" << itsTopRight.X() << "," << itsTopRight.Y() << "\n"
              << "\nxmin=" << box->xmin() << "\nxmax=" << box->xmax() << "\nymin=" << box->ymin()
              << "\nymax=" << box->ymax() << "\nwidth=" << box->width()
              << "\nheight=" << box->height() << "\n"
              << std::endl;
#endif
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

}  // namespace Dali
}  // namespace Plugin
}  // namespace SmartMet


// ====================================================================
// This file is part of FlexibleSUSY.
//
// FlexibleSUSY 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.
//
// FlexibleSUSY 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 FlexibleSUSY.  If not, see
// <http://www.gnu.org/licenses/>.
// ====================================================================

#ifndef SUM_H
#define SUM_H

#include <type_traits>
#include <cstddef>
#include <Eigen/Core>

namespace flexiblesusy {

#define SUM(...) (get_sum(__VA_ARGS__)(__VA_ARGS__))

#define get_sum(...) get_sum_macro(__VA_ARGS__, sum_user_t, sum_ptrdiff_t,)

#define get_sum_macro(_1, _2, _3, _4, _5, name, ...) name

#define sum_ptrdiff_t(idx, ini, fin, expr)      \
    sum<std::ptrdiff_t>((ini), (fin), [&](std::ptrdiff_t (idx)) { return (expr); })

#define sum_user_t(type, idx, ini, fin, expr)	\
    sum<type>((ini), (fin), [&](type (idx)) { return (expr); })

template<typename T>
struct is_eigen_type
{
    static constexpr auto value =
	std::is_base_of<Eigen::EigenBase<T>, T>::value;
};

template<typename Idx, typename Function, bool isEigenType>
struct EvalEigenXprImpl {
    static auto eval(Idx i, Function f) -> decltype(f(i)) {
	return f(i);
    }
};

template<typename Idx, typename Function>
struct EvalEigenXprImpl<Idx, Function, true> {
    static auto eval(Idx i, Function f) ->
	typename std::remove_reference<decltype(f(i).eval())>::type
    {
	return f(i).eval();
    }
};

template<typename Idx, typename Function>
auto EvalEigenXpr(Idx i, Function f) ->
    decltype(
	EvalEigenXprImpl<Idx, Function, is_eigen_type<decltype(f(i))>::value>::
	eval(i, f))
{
    return
	EvalEigenXprImpl<Idx, Function, is_eigen_type<decltype(f(i))>::value>::
	eval(i, f);
}

template<typename T, bool isEigenType>
struct create_zero {
    static const T zero() {
	return T();
    }
};

template<typename T>
struct create_zero<T, true> {
    static const T zero() {
	T z;
	z.setZero();
	return z;
    }
};

template<class Idx, class Function>
auto sum(Idx ini, Idx fin, Function f) -> decltype(EvalEigenXpr<Idx>(ini, f))
{
    using Evaled = decltype(EvalEigenXpr<Idx>(ini, f));
    using Acc = typename std::remove_cv<Evaled>::type;
    Acc s = create_zero<Acc, is_eigen_type<Evaled>::value>::zero();
    for (Idx i = ini; i <= fin; i++) s += f(i);
    return s;
}

} // namespace flexiblesusy

#endif // sum_hpp


#ifndef _TYPE_DEF_H_
#define _TYPE_DEF_H_

#include <armadillo>

//#define ALL_USING_DOUBLE_TYPE

//#define PORT_ADIO_USING_STD_MEMCPY

#define MATH_USING_FFTW3

#define FFTW3_FLAG FFTW_ESTIMATE // FFTW_MEASURE or FFTW_ESTIMATE
//#define FFTW3_USING_THREADS
//#define FFTW3_NUMBER_OF_THREADS 2

namespace type {
    using sint8        = int8_t;
    using sint16       = int16_t;
    using sint32       = int32_t;
    using sint64       = int64_t;
    using uint8        = uint8_t;
    using uint16       = uint16_t;
    using uint32       = uint32_t;
    using uint64       = uint64_t;
    using sint         = sint32;
    using uint         = uint32;
    #if not defined(ALL_USING_DOUBLE_TYPE)
    using val          = float;
    #else
    using val          = double;
    #endif
    using cx_val       = std::complex<val>;
    using sint_vec     = arma::Col<sint>;
    using uint_vec     = arma::Col<uint>;
    using vec          = arma::Col<val>;
    using cx_vec       = arma::Col<cx_val>;
    using sint_row_vec = arma::Row<sint>;
    using uint_row_vec = arma::Row<uint>;
    using row_vec      = arma::Row<val>;
    using cx_row_vec   = arma::Row<cx_val>;
    using sint_mat     = arma::Mat<sint>;
    using uint_mat     = arma::Mat<uint>;
    using mat          = arma::Mat<val>;
    using cx_mat       = arma::Mat<cx_val>;
    using sint_cub     = arma::Cube<sint>;
    using uint_cub     = arma::Cube<uint>;
    using cub          = arma::Cube<val>;
    using cx_cub       = arma::Cube<cx_val>;
    template <typename t>
    using abs_vec      = arma::Col<t>;
    template <typename t>
    using abs_row_vec  = arma::Row<t>;
    template <typename t>
    using abs_mat      = arma::Mat<t>;
    template <typename t>
    using abs_cub      = arma::Cube<t>;
    using idx_vec      = arma::uvec;
}

namespace cnst {
    using mat = arma::Datum<type::val>;
    const type::cx_val i = type::cx_val(0, 1);
    // reduce precision
    #if not defined(ALL_USING_DOUBLE_TYPE)
    const type::val tol = mat::eps * (type::val) 1E+2;
    #else
    const type::val tol = mat::eps * (type::val) 1E+2;
    #endif
}

#endif


/*
 Copyright (c) 2010, The Barbarian Group
 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.

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

#include "cinder/Surface.h"
#include "cinder/ip/Resize.h"
#include "cinder/Filter.h"
#include "cinder/Rect.h"
#include "cinder/ChanTraits.h"

#include <math.h>
#include <vector>
using std::vector;
using std::pair;
#include <limits>
#include <fstream>
#include <algorithm>
#include <boost/preprocessor/seq.hpp>

namespace cinder { namespace ip {

template<typename T>
struct SCALETRAIT {
	static const uint8_t dataType;
};

template<>
struct SCALETRAIT<uint8_t> {
	typedef int32_t SUMT;
	static const int32_t WEIGHTBITS = 14;					// # bits in filter coefficients
	static const int32_t FINALSHIFT = 2 * WEIGHTBITS - 8;	// shift after x&y filter passes
	static const int32_t HALFFINALSHIFT = 1 << ( FINALSHIFT - 1 );
	static const int32_t WEIGHTONE = 1 << WEIGHTBITS;		// filter weight of one
	static uint8_t ACCUMTOCHANNEL( const int32_t in ) {
		int32_t result = (in + HALFFINALSHIFT) >> FINALSHIFT;
		if ( result < 0 )
			result = 0;
		else if ( result > 255 )
			result = 255;
		return static_cast<uint8_t>( result );
	}
	static int32_t CHANNELTOBUFFER( const int32_t in ) { return in >> 8; }
};

template<>
struct SCALETRAIT<float> {
	typedef float SUMT;
	static const float WEIGHTONE;		// filter weight of one
	static float ACCUMTOCHANNEL( const float in ) { return in; }
	static float CHANNELTOBUFFER( const float in ) { return in; }
};

const float SCALETRAIT<float>::WEIGHTONE = 1.0f;

// the mapping from discrete dest coordinates b to continuous source coordinates:
#define MAP(b, scale, offset)  (((b)+(offset))/(scale))

typedef struct {	/* ZOOM-SPECIFIC FILTER PARAMETERS */
    float scale;	/* filter scale (spacing between centers in a space) */
    float supp;	/* scaled filter support radius */
    int32_t width;		/* filter width: max number of nonzero samples */
} FilterParams;

typedef struct {	// SOURCE TO DEST COORDINATE MAPPING
    float sx, sy;	// x and y scales
    float tx, ty;	// x and y translations
    float ux, uy;	// x and y offset used by MAP, private fields
} Mapping;

template<typename T>
class WeightTable {		/* SAMPLED FILTER WEIGHT TABLE */
 public:
    int32_t start, end;		/* range of samples is [start..end-1] */
    T		*weight;		/* weight[i] goes with pixel at start+i */
};

template<typename LT, typename AT>
void scanlineAccumulate( LT weight, LT *lineBuffer, int32_t lineBufferWidth, AT *accum );
template<typename T, typename WT>
void makeWeightTable( int32_t b, float cen, const FilterBase &filter, const FilterParams *params, int32_t len, bool trimzeros, WeightTable<WT> *wtab );

template<typename AT, typename T>
void scanlineShiftAccumToChannel( AT *accum, int32_t x1, int32_t y, int32_t width, ChannelT<T> *channel )
{
	AT result;
	T *dst;

	dst = channel->getData( x1, y );
	int8_t pixelStride = channel->getIncrement();

	for( int32_t i = 0; i < width; i++ ) {
		result = SCALETRAIT<T>::ACCUMTOCHANNEL( *accum++ );
		*dst = static_cast<T>( result );
		dst += pixelStride;
	}
}

template<typename T, typename WT, typename AT>
void scanlineFilterChannelToBuffer( WeightTable<WT> *weights, int32_t x, int32_t y, const ChannelT<T> &channel, AT *lineBuffer, int32_t width )
{
	int32_t b, af;
	AT sum;
	AT *wp;
	const T *srcLine, *src;

	srcLine = channel.getData( x, y );

	int8_t pixelStride = channel.getIncrement();
	for ( b = 0; b < width; b++ ) {
		if( std::numeric_limits<AT>::is_integer )
			sum = 1 << 7;
		else
			sum = 0;
		src = srcLine + weights->start * pixelStride;
		wp = weights->weight;
		for ( af = weights->start; af < weights->end; af++ ) {
			sum += *wp++ * *src;
			src += pixelStride;
		}
		*lineBuffer++ = SCALETRAIT<T>::CHANNELTOBUFFER( sum );
		weights++;
	}
}

// assumes channels are of same dimensions
template<typename T>
void resample( const vector<const ChannelT<T>*> &srcChannels, const FilterBase &filter, const Area &srcArea, const Area &dstArea, const vector<ChannelT<T>*> &dstChannels )
{
	Rectf clippedSrcRect;
	Area clippedDstArea;
	getClippedScaledRects( srcChannels[0]->getBounds(), Rectf( srcArea ), dstChannels[0]->getBounds(), dstArea, &clippedSrcRect, &clippedDstArea );

	if ( ( clippedSrcRect.getWidth() <= 0 ) || ( clippedDstArea.getWidth() <= 0 )
		|| ( clippedSrcRect.getHeight() <= 0 ) || ( clippedDstArea.getHeight() <= 0 ) )
		return;

	FilterParams filterParamsX, filterParamsY;
	Mapping m;
	std::shared_ptr<typename SCALETRAIT<T>::SUMT> accum;
	int32_t dstWidth = (int32_t)clippedDstArea.getWidth(), dstHeight = (int32_t)clippedDstArea.getHeight();
	int32_t srcWidth = (int32_t)clippedSrcRect.getWidth(), srcHeight = (int32_t)clippedSrcRect.getHeight();
	int32_t srcOffsetX = static_cast<int32_t>( floor( clippedSrcRect.getX1() ) );
	int32_t srcOffsetY = static_cast<int32_t>( floor( clippedSrcRect.getY1() ) );
	vector<pair<int32_t,std::shared_ptr<typename SCALETRAIT<T>::SUMT> > > linesBuffer;

	m.sx = dstWidth / (float)srcWidth;
	m.sy = dstHeight / (float)srcHeight;
	m.tx = clippedDstArea.getX1() - 0.5f - m.sx * ( clippedSrcRect.getX1() - 0.5f );
	m.ty = clippedDstArea.getY1() - 0.5f - m.sy * ( clippedSrcRect.getY1() - 0.5f );
	m.ux = clippedDstArea.getX1() - m.sx * ( clippedSrcRect.getX1()- 0.5f ) - m.tx;
	m.uy = clippedDstArea.getY1() - m.sy * ( clippedSrcRect.getY1()- 0.5f ) - m.ty;

	filterParamsX.scale = std::max( 1.0f, 1.0f / m.sx );
	filterParamsX.supp = std::max( 0.5f, filterParamsX.scale * filter.getSupport() );
	filterParamsX.width = (int32_t)ceil( 2.0f * filterParamsX.supp );

	filterParamsY.scale = std::max( 1.0f, 1.0f / m.sy );
	filterParamsY.supp = std::max( 0.5f, filterParamsY.scale * filter.getSupport() );
	filterParamsY.width = (int32_t)ceil( 2.0f * filterParamsY.supp );

	for( int32_t i = 0; i < filterParamsY.width; i++ )
		linesBuffer.push_back( std::make_pair( -1, std::shared_ptr<typename SCALETRAIT<T>::SUMT>( new typename SCALETRAIT<T>::SUMT[dstWidth], checked_array_deleter<typename SCALETRAIT<T>::SUMT>() ) ) );

	WeightTable<typename SCALETRAIT<T>::SUMT> *xWeights, yWeights;
	typename SCALETRAIT<T>::SUMT *xWeightBuffer, *xWeightPtr;
	xWeights = (WeightTable<typename SCALETRAIT<T>::SUMT>*)malloc( sizeof(WeightTable<int32_t>) * dstWidth );
	xWeightBuffer = (typename SCALETRAIT<T>::SUMT*)malloc( sizeof(typename SCALETRAIT<T>::SUMT) * dstWidth * filterParamsX.width );
	yWeights.weight = (typename SCALETRAIT<T>::SUMT*)malloc( sizeof(typename SCALETRAIT<T>::SUMT) * filterParamsY.width );
	accum = std::shared_ptr<typename SCALETRAIT<T>::SUMT>( new typename SCALETRAIT<T>::SUMT[dstWidth], checked_array_deleter<typename SCALETRAIT<T>::SUMT>() );

	xWeightPtr = xWeightBuffer;
	for ( int32_t bx = 0; bx < dstWidth; bx++, xWeightPtr += filterParamsX.width ) {
		xWeights[bx].weight = xWeightPtr;
		makeWeightTable<T,typename SCALETRAIT<T>::SUMT>( bx, MAP(bx, m.sx, m.ux), filter, &filterParamsX, srcWidth, true, &xWeights[bx] );
	}

	for( size_t chan = 0; chan < srcChannels.size(); ++chan ) {
		for ( int32_t dstY = 0; dstY < dstHeight; ++dstY ) {     // loop over dest scanlines
			// prepare a weight table for dest y position by
			makeWeightTable<T,typename SCALETRAIT<T>::SUMT>( dstY, MAP(dstY, m.sy, m.uy), filter, &filterParamsY, srcHeight, false, &yWeights );

			memset( accum.get(), 0, sizeof(int32_t) * dstWidth );

			// loop over source scanlines that influence this dest scanline
			for ( int32_t ayf = yWeights.start; ayf < yWeights.end; ayf++ ) {
				typename SCALETRAIT<T>::SUMT *line = linesBuffer[ayf % filterParamsY.width].second.get();
				if( linesBuffer[ayf % filterParamsY.width].first != ayf ) {
					scanlineFilterChannelToBuffer( xWeights, srcOffsetX, srcOffsetY + ayf, *(srcChannels[chan]), line, dstWidth );
					linesBuffer[ayf % filterParamsY.width].first = ayf;
				}
				scanlineAccumulate<typename SCALETRAIT<T>::SUMT,typename SCALETRAIT<T>::SUMT>( yWeights.weight[ayf - yWeights.start], line, dstWidth, accum.get() );
			}

			scanlineShiftAccumToChannel( accum.get(), clippedDstArea.getX1(), clippedDstArea.getY1() + dstY, dstWidth, dstChannels[chan] );
		}
	}

	free( xWeights );
	free( xWeightBuffer );
	free( yWeights.weight );
}

template<typename LT, typename AT>
void scanlineAccumulate( LT weight, LT *lineBuffer, int32_t width, AT *accum )
{
	AT *dest = accum;
	int32_t x;

	for ( x = 0; x < width; x++ )
		*dest++ += *lineBuffer++ * weight;
}

template<typename T, typename WT>
void makeWeightTable( int32_t b, float cen, const FilterBase &filter, const FilterParams *params, int32_t len, bool trimzeros, WeightTable<WT> *wtab )
{
	int32_t start, end, i, stillzero, lastnonzero, nz;
	WT *wp, t, sum;
	float den, sc, tr;

	// find the source coord range of this positioned filter: [start..end-1]
	start = (int32_t)( cen - params->supp + 0.5f );
	end = (int32_t)( cen + params->supp + 0.5f );
	if ( start < 0 )
		start = 0;
	if ( end > len )
		end = len;

	// the range of source samples to buffer:
	wtab->start = start;
	wtab->end = end;

	// find scale factor sc to normalize the filter
	for ( den = 0, i=start; i < end; i++ )
		den += filter( ( i + 0.5f - cen ) / params->scale );

	// set sc so that sum of sc*func() is approximately WEIGHTONE
	sc = ( den == 0.0f ) ? ( SCALETRAIT<T>::WEIGHTONE ) : ( SCALETRAIT<T>::WEIGHTONE / den );

	// compute the discrete, sampled filter coefficients
	stillzero = trimzeros;
	for ( sum = 0, wp = wtab->weight, i = start; i < end; i++ ) {
		// evaluate the filter function:
		tr = sc * filter( ( i + 0.5f - cen ) / params->scale );

		if( std::numeric_limits<WT>::is_integer )
			t = (WT)floor( tr + 0.5f );
		else
			t = (WT)tr;
		if ( stillzero && ( t == 0 ) )
			start++;	// find first nonzero
		else {
			stillzero = 0;
			*wp++ = t;			// add weight to table
			sum += t;
			if ( t != 0 )
				lastnonzero = i;	// find last nonzero
		}
	}

	if ( sum == 0 ) {
		nz = wtab->end-wtab->start;
		wtab->start = (wtab->start+wtab->end) >> 1;
		wtab->end = wtab->start+1;
		wtab->weight[0] = SCALETRAIT<T>::WEIGHTONE;
	}
	else {
		if ( trimzeros ) {		/* skip leading and trailing zeros */
			/* set wtab->start and ->end to the nonzero support of the filter */
			nz = wtab->end-wtab->start-(lastnonzero-start+1);
			wtab->start = start;
			wtab->end = end = lastnonzero+1;
		}
		else				/* keep leading and trailing zeros */
			nz = 0;

		if ( sum != SCALETRAIT<T>::WEIGHTONE ) {
			/*
				* Fudge the center slightly to make sum=WEIGHTONE exactly.
				* Is this the best way to normalize a discretely sampled
				* continuous filter?
			*/
			i = (int32_t)( cen + 0.5f );
			if ( i < start )
				i = start;
			else if ( i >= end )
				i = end - 1;
			t = SCALETRAIT<T>::WEIGHTONE - sum;
			wtab->weight[i - start] += t;	/* fudge center sample */
		}
	}
}

template<typename T>
void resize( const SurfaceT<T> &srcSurface, const Area &srcArea, SurfaceT<T> *dstSurface, const Area &dstArea, const FilterBase &filter )
{
	vector<const ChannelT<T>*> srcChannels;
	vector<ChannelT<T>*> dstChannels;

	srcChannels.push_back( &srcSurface.getChannelRed() );
	dstChannels.push_back( &dstSurface->getChannelRed() );
	srcChannels.push_back( &srcSurface.getChannelGreen() );
	dstChannels.push_back( &dstSurface->getChannelGreen() );
	srcChannels.push_back( &srcSurface.getChannelBlue() );
	dstChannels.push_back( &dstSurface->getChannelBlue() );
	if ( srcSurface.hasAlpha() && dstSurface->hasAlpha() ) {
		srcChannels.push_back( &srcSurface.getChannelAlpha() );
		dstChannels.push_back( &dstSurface->getChannelAlpha() );
	}

	resample( srcChannels, filter, srcArea, dstArea, dstChannels );
}

template<typename T>
void resize( const ChannelT<T> &srcChannel, const Area &srcArea, ChannelT<T> *dstChannel, const Area &dstArea, const FilterBase &filter )
{
	vector<const ChannelT<T>*> srcChannels;
	vector<ChannelT<T>*> dstChannels;

	srcChannels.push_back( &srcChannel );
	dstChannels.push_back( dstChannel );

	resample( srcChannels, filter, srcArea, dstArea, dstChannels );
}

template<typename T>
void resize( const SurfaceT<T> &srcSurface, SurfaceT<T> *dstSurface, const FilterBase &filter )
{
	resize( srcSurface, srcSurface.getBounds(), dstSurface, dstSurface->getBounds(), filter );
}

template<typename T>
SurfaceT<T> resizeCopy( const SurfaceT<T> &srcSurface, const Area &srcArea, const Vec2i &dstSize, const FilterBase &filter )
{
	SurfaceT<T> result( dstSize.x, dstSize.y, srcSurface.hasAlpha(), srcSurface.getChannelOrder() );
	resize( srcSurface, srcArea, &result, result.getBounds(), filter );
	return result;
}

template<typename T>
void resize( const ChannelT<T> &srcChannel, ChannelT<T> *dstChannel, const FilterBase &filter )
{
	resize( srcChannel, srcChannel.getBounds(), dstChannel, dstChannel->getBounds(), filter );
}

#define resize_PROTOTYPES(r,data,T)\
	template void resize( const SurfaceT<T> &srcSurface, SurfaceT<T> *dstSurface, const FilterBase &filter ); \
	template void resize( const SurfaceT<T> &srcSurface, const Area &srcArea, SurfaceT<T> *dstSurface, const Area &dstArea, const FilterBase &filter ); \
	template void resize( const ChannelT<T> &srcChannel, ChannelT<T> *dstChannel, const FilterBase &filter ); \
	template SurfaceT<T> resizeCopy( const SurfaceT<T> &srcSurface, const Area &srcArea, const Vec2i &dstSize, const FilterBase &filter ); \
	template void resize( const ChannelT<T> &srcChannel, const Area &srcArea, ChannelT<T> *dstChannel, const Area &dstArea, const FilterBase &filter );

BOOST_PP_SEQ_FOR_EACH( resize_PROTOTYPES, ~, CHANNEL_TYPES )

} } // namespace cinder::ip


// Copyright (c) 2019 Graphcore Ltd. All rights reserved.
#define BOOST_TEST_MODULE Final0SubgraphTest

// tests of final greedy selection of sub-graphs

#include "blip.hpp"
#include <algorithm>
#include <boost/test/unit_test.hpp>
#include <iostream>
#include <vector>
#include <popart/logging.hpp>

using namespace fwtools::subgraph;
using namespace blip;

BOOST_AUTO_TEST_CASE(Final0_Subgraph) {

  auto test = [](const std::vector<blip::Type> &types,
                 const std::map<blip::Type, blip::Value> &value_map,
                 const std::vector<Edge> &edges,
                 std::vector<Match> expected_matches,
                 int algo,
                 float threshold) {
    // prepare the schedule from the input parameters
    std::vector<const Blip *> sched;
    std::vector<std::unique_ptr<Blip>> blips;
    for (auto &t : types) {
      blips.emplace_back(
          std::unique_ptr<Blip>(new Blip(t, value_map.at(t), {})));
    }

    for (auto edge : edges) {
      blips[edge.destId]->addIn(
          edge.inIndex, blips[edge.sourceId].get(), edge.outIndex);

      blips[edge.sourceId]->addOut(blips[edge.destId].get(), edge.outIndex);
    }
    for (int i = 0; i < blips.size(); ++i) {
      sched.push_back(blips[i].get());
    }

    std::vector<std::pair<size_t, size_t>> sequences(sched.size());
    float sequenceBreakCost = 0.0f;

    // get the matches
    std::vector<Match> matches;
    if (algo == 1) {
      matches = getRinseMatches<const Blip>(sched,
                                            sequences,
                                            threshold,
                                            sequenceBreakCost,
                                            OutlinerAlgorithm::ALGO1);
    } else if (algo == 0) {
      matches = getRinseMatches<const Blip>(sched,
                                            sequences,
                                            threshold,
                                            sequenceBreakCost,
                                            OutlinerAlgorithm::ALGO0);
    } else {
      throw std::runtime_error("invalid algo");
    }

    // compare to the expected matches
    std::stringstream ss;
    ss << "\nExpected matches:";
    for (auto &x : expected_matches) {
      ss << "\n" << x;
    }
    ss << "\nComputed matches:";
    for (auto &x : matches) {
      ss << "\n" << x;
    }
    ss << "\n\n\n";
    popart::logging::debug(ss.str());
    BOOST_CHECK(matches == expected_matches);
  };

  // -------------------------------------------
  //                            0  1  2  3  4  5  6  7  8  9
  std::vector<blip::Type> types{0, 1, 1, 0, 1, 1, 0, 1, 1, 0};
  //                            x  x  x  x        x  x  x  x
  //                               w  w     w  w     w  w
  //                              [*][*]   [*][*]   [*][*]
  //                            ^        ^        ^        ^
  // TODO: T7255 check for sub-sequences which are saturated by smaller
  // sequences, as in the case above. Match "w" should be removed

  // 1's are worth slightly more that 0's
  std::map<blip::Type, blip::Value> value_map{{0, 10.0f}, {1, 11.0f}};

  std::vector<Match> expected_matches = {
      {{0, 6}, 4},             // 0110
      {{1, 4, 7}, 2},          // 11
      {{1, 2, 4, 5, 7, 8}, 1}, // 1
      {{0, 3, 6, 9}, 1}        // 0

  };
  std::vector<Edge> edges;

  for (int algo : {0, 1}) {
    popart::logging::info(
        "full test, saturated large sequence, threshold -1.0, algo {}", algo);
    test(types, value_map, edges, expected_matches, algo, -1.0);
  }

  expected_matches = {
      {{1, 2, 4, 5, 7, 8}, 1}, // 1
      {{0, 3, 6, 9}, 1}        // 0
  };

  for (int algo : {0, 1}) {
    popart::logging::info(
        "full test, saturated large sequence, threshold 5.0, algo {}", algo);
    test(types, value_map, edges, expected_matches, algo, 5.0);
  }

  // -------------------------------------------
  //       0  1  2  3  4  5  6  7  8  9  10 11 12 13 14 15
  types = {6, 1, 2, 5, 1, 2, 6, 6, 1, 2, 7, 1, 2, 8, 1, 2};
  //       0--0
  //       0-----0
  //       0-----1
  //       0-----------------------0
  //       0--------------------------0
  //                               0--1
  //       0-----------0
  //       0--------------1  note here this is 0->1, not 0->0
  //                   0--0  as in the first 2. This third edge changed too
  //       0--------------------------------0
  //       0-----------------------------------1
  //                                        0--0
  //
  //       0------------------------------------------0
  //       0---------------------------------------------1
  //                                                  0--0

  edges = {{0, 1, 0, 0}, // 0
           {0, 2, 0, 0}, // 0
           {0, 2, 0, 1}, // 0
           {0, 8, 0, 0},
           {0, 9, 0, 0},
           {8, 9, 0, 1},
           {0, 4, 0, 0}, // 0
           {0, 5, 0, 1}, // 1
           {4, 5, 0, 0}, // 0 i.e. different input indices => not isomorphism
           {0, 11, 0, 0},
           {0, 12, 0, 1},
           {11, 12, 0, 0},
           {0, 14, 0, 0},
           {0, 15, 0, 1},
           {14, 15, 0, 0}

  };

  // 1's are worth slightly more that 0's
  value_map = {{1, 11.0f},
               {2, 10.0f},
               {3, 1.0f},
               {4, 1.0f},
               {5, 1.0f},
               {6, 1.0f},
               {7, 1.0f},
               {8, 1.0f}};

  auto metatest = [types, value_map, edges, &test](
                      float threshold, std::vector<Match> expected_matches) {
    for (int algo : {0, 1}) {
      test(types, value_map, edges, expected_matches, algo, threshold);
    }
  };

  expected_matches = {
      // 12 first isomorphic type
      // The other 2 isomorphic types (starting at 1 and 8) are singletons
      // Note, that with a non-negative threshold, this match is removed
      {{4, 11, 14}, 2},
      // 1: it is not subsumed
      {{4, 8, 11, 14}, 1},
      // 2: it is not subsumed. The 2 at index 2 has the same input tensor
      {{5, 9, 12, 15}, 1},
      // 6: index 0 has it's output consumed, so not isomorphic here
      {{6, 7}, 1}};

  popart::logging::info(
      "full test, non-isomorphic double take, threshold -1.0f");
  metatest(-1.0f, expected_matches);

  popart::logging::info("metatest, threshold 0.0f");
  metatest(0.0f, {{{4, 8, 11, 14}, 1}, {{5, 9, 12, 15}, 1}, {{6, 7}, 1}});

  popart::logging::info("metatest, threshold 9.0f");
  metatest(9.0f, {{{4, 8, 11, 14}, 1}, {{5, 9, 12, 15}, 1}});

  popart::logging::info("metatest, threshold 100.0f");
  metatest(100.0f, {});
}


//
//  Simulator.cpp
//  proteintools
//
//  Created by Salik Syed on 10/5/17.
//  Copyright © 2017 N/A. All rights reserved.
//

#include "Simulator.hpp"
#include "PDBGeometry.hpp"
#include "Residue.hpp"
#include <iostream>
#include <Eigen/Geometry>
using namespace std;

#define TORSION_EPSILON 0.00001f

Simulator::Simulator(Chain& chain, ForceField& forcefield) {
    size_t atom_count = 0;
    ResidueTypeIterator it = chain.first_residue();

    while (it != chain.last_residue()) {
        const Residue * r = forcefield.getResidue(*it,
                                            it==chain.first_residue(),
                                            it==chain.last_residue() - 1);
        _residues.push_back(r);
        atom_count += r->numAtoms();
        it++;
    }

    _numAtoms = atom_count;

    _evaluator = (EnergyEvaluator*) new CPUEvaluator((unsigned int)_numAtoms, forcefield.getLj14scale(), forcefield.getC14scale());

    // initialize the conformation
    _conformation = new Conformation(_residues.size());

    // init torsion params
    for(size_t i = 0; i < _conformation->numTorsionParameters(); i++) {
        _conformation->setTorsion(i, 0.0f);
    }

    // Allocate space for original and transformed atom positions
    _atoms = new Eigen::Matrix4Xf(4, atom_count);
    _atomsTransformed = new Eigen::Matrix4Xf(4, atom_count);

    // Allocate space for the charge parameters:
    _atomParams = new Eigen::Matrix4Xf(4, atom_count);

    it = chain.first_residue();
    size_t curr_atom = 0;

    for(size_t i = 0; i < _residues.size(); i++) {
        const Residue * r = _residues[i];
        ResidueType type = r->geometry_name();
        PDBGeometry & geom = PDBGeometry::get(type);
        for(size_t j = 0; j < r->numAtoms(); j++) {
            pair<AtomName,AtomType> a = r->getAtom(j);
            _atomParams->col(curr_atom)<<
            -1.0f,
            a.second.charge,
            a.second.sigma,
            a.second.epsilon;
            AtomName name(a.first);
            if (geom.hasGeometry(name)) {
                _atomParams->col(curr_atom)<<
                1.0f,
                a.second.charge,
                a.second.sigma,
                a.second.epsilon;
                Eigen::Vector4f pos = geom.position(name);
                _atoms->col(curr_atom)<<pos.x(), pos.y(), pos.z(), 1.0;
                _atomsTransformed->col(curr_atom)<<pos.x(), pos.y(), pos.z(), 1.0;
            } else {
                _atoms->col(curr_atom)<<0.0, 0.0, 0.0, 0.0;
                _atomsTransformed->col(curr_atom)<<0.0, 0.0, 0.0, 0.0;
            }
            curr_atom++;
        }
        if (i != 0) {
            Eigen::Matrix4f m = _residues[i - 1]->getTransformForChild(r);
            _translationTransforms.push_back(m);
            _matrixStack.push_back(m*_matrixStack[_matrixStack.size() -1]);
        } else {
            _translationTransforms.push_back(Eigen::Matrix4f::Identity(4, 4));
            _matrixStack.push_back(Eigen::Matrix4f::Identity(4, 4));
        }
    }
    setConformation(*_conformation, true);
}

void Simulator::setConformation(Conformation &conformation, bool forceUpdate) {

    // update the transformed atom data
    size_t curr = _residues[0]->numAtoms();
    for(size_t i = 1; i < _residues.size(); i++) {
        if (conformation.getTorsion(i) > TORSION_EPSILON) {
            Eigen::Matrix4f rot;
            rot.setIdentity();
            Eigen::Vector3f transformedAxis = (_matrixStack[i-1] * _residues[i]->getBondAxis()).head<3>();
            transformedAxis.normalize();
            rot.block<3,3>(0,0) = Eigen::AngleAxisf(conformation.getTorsion(i),
                                                    transformedAxis).matrix();
            rot.rightCols(1) = Eigen::Vector4f(0.0, 0.0, 0.0, 1.0);

            _matrixStack[i] = _translationTransforms[i] *
                            rot *
                            _matrixStack[i - 1];
        } else {
            _matrixStack[i] = _translationTransforms[i] *
                            _matrixStack[i - 1];
        }
        for(size_t j = 0; j < _residues[i]->numAtoms(); j++) {
            _atomsTransformed->col(curr + j) << _matrixStack[i] * _atoms->col(curr + j);
        }
        curr += _residues[i]->numAtoms();
    }
}

Conformation Simulator::getConformation() const {
    Conformation conformation(_conformation->numTorsionParameters());
    for(size_t i = 0; i < _conformation->numTorsionParameters(); i++) {
        conformation.setTorsion(i, _conformation->getTorsion(i));
    }
    return conformation;
}

void Simulator::getAtoms(vector<AtomInfo>& atomList) const{
    size_t idx = 0;
    for(size_t i = 0; i < _residues.size(); i++) {
        const Residue * r = _residues[i];
        ResidueType type = r->name();
        for(size_t j = 0; j < r->numAtoms(); j++){
            pair<AtomName, AtomType> a = r->getAtom(j);
            AtomInfo info;
            AtomName name(a.first);
            PDBGeometry & geom = PDBGeometry::get(r->geometry_name().c_str());
            info.geometryValid = geom.hasGeometry(name);
            info.atom = a.second;
            info.isBackboneChild = j == r->parentAtomIndex();
            info.isBackboneChild = j == r->childAtomIndex();
            info.residue = type;
            info.position = _atomsTransformed->col(idx);
            atomList.push_back(info);
            idx++;
        }
    }
}
void Simulator::getBonds(vector<pair<int, int>> & bondList) const{
    size_t offset = 0;
    for(size_t i = 0; i < _residues.size(); i++) {
        const Residue * r = _residues[i];
        BondIterator a_it = r->first_bond();
        while(a_it != r->last_bond()) {
            size_t a1 = a_it->atom1_idx + offset;
            size_t a2 = a_it->atom2_idx + offset;
            bondList.push_back(pair<int, int>((int)a1, (int)a2));
            a_it++;
        }
        offset += r->numAtoms();
    }
}

float Simulator::getEnergy() const {
    return _evaluator->getEnergy(_atomParams, _atomsTransformed);
}

Simulator::~Simulator() {
    delete _evaluator;
    delete _atoms;
    delete _atomsTransformed;
    delete _atomParams;
}


/*
 * Copyright 2015-2019 Autoware Foundation. 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 <stdio.h>
#include <stdlib.h>
#include <iostream>
#include <fstream>
#include <time.h>

#include <math.h>
#include "ros/ros.h"
#include <libxml/xmlreader.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/CompressedImage.h>
#include <boost/array.hpp>
#include "chessboard.h"
#include "scan_window.h"
#include "trans.h"
#include "common_2d_calib.h"
#include "image_window.h"
#include "data_struct.h"
#include <opencv/highgui.h>
#include "opencv2/highgui/highgui.hpp"

#define XSTR(x) #x
#define STR(x) XSTR(x)

#define WINDOW_NAME_SCAN "LRF"
#define WINDOW_NAME_IMAGE "CAMERA"

static int window_scan_width = 1240;
static int window_scan_height = 960;
static int scale = 300;
static int line_division_num = 10;
static double judge_margin = 20.0;
static int pat_row =7 ;
static int pat_col = 11;
static int pat_size = pat_row * pat_col;
static double chess_size = 20.0;
static double paper_width_margin = 30.0;

static bool g_chess_scan_flag = false;
static bool g_chess_camera_flag = false;
static IplImage *image_camera;
static bool g_calib_flag = false;
static bool g_fusion_flag = false;
static bool g_save_flag = false;
static bool g_on_calib_flag = false;
static bool g_on_save_flag = false;
static double g_scan_z;
static Three_dimensional_vector g_3d_scan;

static void on_mouse(int event, int x, int y, int flags, void *param = NULL)
{
    if (50 < x && x < 320 && 150< y && y < 220) { // calib
        g_on_calib_flag = true;
        g_on_save_flag = false;
        switch(event) {
        case cv::EVENT_LBUTTONDOWN:
            g_calib_flag = true;
            break;

        default:
            break;
        }
    } else if ( 50 < x && x < 320 && 200< y && y < 270) { //save
        g_on_save_flag = true;
        g_on_calib_flag = false;
        switch(event) {
        case cv::EVENT_LBUTTONDOWN:
            g_save_flag = true;
            break;

        default:
            break;
        }
    } else {
        g_on_calib_flag = false;
        g_on_save_flag = false;
    }
}

static void print_param(CvMat *matrix)
{
    for (int i = 0; i < matrix->rows; i++) {
        for (int j = 0; j < matrix->cols; j++) {
                printf ("% lf\t", cvmGet (matrix, i, j));
            }
        printf ("\n");
    }
}

static void imageCallback(const sensor_msgs::Image& image_raw) {
    cv_bridge::CvImagePtr cv_image = cv_bridge::toCvCopy(image_raw, sensor_msgs::image_encodings::BGR8);
    IplImage temp = cv_image->image;
    image_camera = &temp;

    CvMat image_points;
    CvMat object_points;
    CvPoint3D32f objects[pat_row*pat_col];

    if (!g_fusion_flag) {
        int corner_count;
        CvPoint2D32f *corners = (CvPoint2D32f *) cvAlloc (sizeof (CvPoint2D32f) * pat_size);
        if(findChessboard(image_camera, pat_col, pat_row, chess_size, &object_points, &corner_count, corners, &objects[0])){
            g_chess_camera_flag = true;
            drawChessboard(image_camera, pat_col, pat_row, corner_count, corners, &image_points);
        } else {
            g_chess_camera_flag = false;
        }
    }
    if (g_calib_flag) {
        g_fusion_flag = true;
        g_calib_flag = false;

        /* Get external parameter */
        CvMat sub_image_points, sub_object_points;
        cvGetRows(&image_points, &sub_image_points, 0, pat_size);
        cvGetRows(&object_points, &sub_object_points, 0, pat_size);
        cvFindExtrinsicCameraParams2 (&sub_object_points, &sub_image_points, m_intrinsic, m_dist, v_rotation, v_g2c);
        //Chnage vector to matrix
        cvRodrigues2(v_rotation, m_rotation);

        //Get global to scan parameter
        cvmSet(v_g2l, 0, 0, chess_size * (pat_col - 1) / 2);
        cvmSet(v_g2l, 0, 1, chess_size * (pat_row - 1) / 2);
        cvmSet(v_g2l, 0, 2, g_scan_z * 1000);
        set_rotation(m_rotation);
        set_g2c(v_g2c);
    }

    else if (g_fusion_flag) {
        Two_dimensional_vector scan_image;
        scan_image.x.resize(g_3d_scan.x.size());
        scan_image.y.resize(g_3d_scan.x.size());
        scan_image = trans_scan2scan_image(&g_3d_scan, v_g2l, v_g2c, m_rotation, m_intrinsic);
        plot_scan_image(image_camera, &scan_image);
    }

    if (g_save_flag) {
        // Write parameter
        ros::NodeHandle n;
        std::string camera_yaml;
        n.param<std::string>("/camera_lidar2d/camera_yaml", camera_yaml, STR(CAMERA_YAML));
        cv::FileStorage fs(camera_yaml.c_str(), cv::FileStorage::WRITE);
        if(!fs.isOpened()){
            fprintf(stderr, "%s : cannot open file\n", camera_yaml.c_str());
            exit(EXIT_FAILURE);
        }

        CvMat *affine_l2g = cvCreateMat(4, 4, CV_64FC1);
        CvMat *affine_g2c = cvCreateMat(4, 4, CV_64FC1);
        CvMat *affine_l2c = cvCreateMat(4, 4, CV_64FC1);
        cvSetZero(affine_l2g);
        cvSetZero(affine_g2c);
        cvSetZero(affine_l2c);

        cvmSet(affine_l2g, 0, 1, 1.0);
        cvmSet(affine_l2g, 1, 0, -1.0);
        cvmSet(affine_l2g, 2, 2, -1.0);
        cvmSet(affine_l2g, 0, 3, cvmGet(v_g2l, 0, 1));
        cvmSet(affine_l2g, 1, 3, cvmGet(v_g2l, 0, 0));
        cvmSet(affine_l2g, 2, 3, cvmGet(v_g2l, 0, 2));
        cvmSet(affine_l2g, 3, 3, 1);

        cvmSet(affine_g2c, 0, 0, cvmGet(m_rotation, 0, 0));
        cvmSet(affine_g2c, 0, 1, cvmGet(m_rotation, 0, 1));
        cvmSet(affine_g2c, 0, 2, cvmGet(m_rotation, 0, 2));
        cvmSet(affine_g2c, 1, 0, cvmGet(m_rotation, 1, 0));
        cvmSet(affine_g2c, 1, 1, cvmGet(m_rotation, 1, 1));
        cvmSet(affine_g2c, 1, 2, cvmGet(m_rotation, 1, 2));
        cvmSet(affine_g2c, 2, 0, cvmGet(m_rotation, 2, 0));
        cvmSet(affine_g2c, 2, 1, cvmGet(m_rotation, 2, 1));
        cvmSet(affine_g2c, 2, 2, cvmGet(m_rotation, 2, 2));
        cvmSet(affine_g2c, 0, 3, cvmGet(v_g2c, 0, 0));
        cvmSet(affine_g2c, 1, 3, cvmGet(v_g2c, 0, 1));
        cvmSet(affine_g2c, 2, 3, cvmGet(v_g2c, 0, 2));
        cvmSet(affine_g2c, 3, 3, 1);

        cvMatMulAdd(affine_g2c, affine_l2g, 0, affine_l2c);

        printf("--transration v_g2l\n");
        print_param(v_g2l);
        printf("--transration v_g2c\n");
        print_param(v_g2c);
        printf("--transration m_rotation\n");
        print_param(m_rotation);
        printf("--transration l2g\n");
        print_param(affine_l2g);
        printf("--transration g2c\n");
        print_param(affine_g2c);
        printf("--transration l2c\n");
        print_param(affine_l2c);
        printf("--intrinsic\n");
        print_param(m_intrinsic);

        cv::Size image_size(image_raw.width, image_raw.height);

	cv::Mat affine_l2c_mat=cv::cvarrToMat(affine_l2c);
	cv::Mat m_intrinsic_mat=cv::cvarrToMat(m_intrinsic);
	cv::Mat m_dist_mat=cv::cvarrToMat(m_dist);

        fs << "CameraExtrinsicMat" << affine_l2c_mat;
        fs << "CameraMat" << m_intrinsic_mat;
        fs << "DistCoeff" << m_dist_mat;
        fs << "ImageSize" << image_size;
        fs << "ReprojectionError" << 0;

        cvReleaseMat (&affine_g2c);
        cvReleaseMat (&affine_l2g);
        cvReleaseMat (&affine_l2c);

        fs.release();
        printf("saved\n");
        g_fusion_flag = true;
        g_save_flag = false;
    }
    cvShowImage (WINDOW_NAME_IMAGE, image_camera);
    cvWaitKey (2);
}

static void scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan_msg)
{
    IplImage *image_lrf;
    CvPoint pt, center_pt;

    image_lrf = cvCreateImage(cvSize(window_scan_width, window_scan_height), IPL_DEPTH_8U, 3);
    cvSetZero(image_lrf);

    /* Convert to 3d-axis */
    g_3d_scan.x.resize(scan_msg->ranges.size());
    g_3d_scan.y.resize(scan_msg->ranges.size());
    g_3d_scan.z.resize(scan_msg->ranges.size());
    for(int i = 0; i < (int)scan_msg->ranges.size(); i++) {
        g_3d_scan.x[i] = scan_msg->ranges[i] * sin(scan_msg->angle_min + scan_msg->angle_increment * i);
        g_3d_scan.y[i] = 0;
        g_3d_scan.z[i] = scan_msg->ranges[i] * cos(scan_msg->angle_min + scan_msg->angle_increment * i);
    }

    /* Plot window line */
    plot_vertical_line(image_lrf, line_division_num, window_scan_width, window_scan_height);
    plot_horizontal_line(image_lrf, window_scan_width, window_scan_height);

    // Plot chessboard line
    center_pt = get_center_pt(window_scan_width, &g_3d_scan, scale);
    plot_center_pt_line(image_lrf, center_pt, chess_size, pat_col, paper_width_margin, window_scan_width, window_scan_height, scale);

    /* Plot scan points on image */
    g_chess_scan_flag = true;
    g_scan_z = g_3d_scan.z[scan_msg->ranges.size()/2];
    for (int i = 0; i < (int)scan_msg->ranges.size(); i++) {
        if(0 > g_3d_scan.x[i]*scale+(window_scan_width/2) || g_3d_scan.x[i]*scale+(window_scan_width/2) > window_scan_width) {
            continue;
        }
        if(0 > g_3d_scan.z[i]*scale || g_3d_scan.z[i]*scale > window_scan_height) {
            continue;
        }
        // scan point on image
        pt.x = g_3d_scan.x[i]*scale + (window_scan_width/2);
        pt.y = g_3d_scan.z[i]*scale;

        // Plot point
        int half_paper_col = ((chess_size/1000)*(pat_col+1)+(paper_width_margin/1000))*scale/2;
        if(center_pt.x - half_paper_col <= pt.x && pt.x <= center_pt.x + half_paper_col) {
            // Judge point in chessboard
            if(g_scan_z - (judge_margin/1000) <= g_3d_scan.z[i]
               && g_3d_scan.z[i] <= g_scan_z + (judge_margin/1000)) {
                // red point
                cvCircle(image_lrf, pt, 1, CV_RGB (255, 0, 0), CV_FILLED, 8, 0);
            } else {
                // green point
                cvCircle(image_lrf, pt, 1, CV_RGB (0, 255, 0), CV_FILLED, 8, 0);
                g_chess_scan_flag = false;
            }
        } else {
            //green point
            cvCircle(image_lrf, pt, 1, CV_RGB (0, 255, 0), CV_FILLED, 8, 0);
        }
    }

    /* Plot center point */
    // yellow point
    cvCircle(image_lrf, center_pt, 6, CV_RGB (255, 255, 0), CV_FILLED, 8, 0);

    /* Plot info */
    // Plot center point's distance
    {
        char text[32];
        sprintf(text, "%.3fm", g_scan_z);
        plot_string(image_lrf, text, 1, 50, 50, CV_RGB(255, 0, 0));
    }
    // Plot String
    if(g_chess_scan_flag) {
        plot_string(image_lrf, "LRF OK", 4, 50, 100, CV_RGB(255, 0, 0));
    }
    if(g_chess_camera_flag) {
        plot_string(image_lrf, "CAMERA OK", 4, 50, 150, CV_RGB(255, 0, 0));
    }
    cvSetMouseCallback (WINDOW_NAME_SCAN, on_mouse);
    plot_string_on_buttun(image_lrf, "CALIBRATE(click)", 4, 50, 200, g_on_calib_flag);
    if(g_fusion_flag) {
        plot_string_on_buttun(image_lrf, "SAVE(click)", 4, 50, 250, g_on_save_flag);
    }

    cvShowImage(WINDOW_NAME_SCAN,image_lrf);
    cvWaitKey(2);
    cvReleaseImage (&image_lrf);
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "camera_lidar2d_offline_calib");
    ros::NodeHandle n;
    ros::NodeHandle private_nh("~");
    std::string image_topic_name;
    private_nh.param<std::string>("image_raw_topic", image_topic_name, "image_raw");

    /* Read parameter xml */
    std::string param_yaml;
    n.param<std::string>("/camera_lidar2d/param_yaml", param_yaml, STR(PARAM_YAML));

    cv::FileStorage fs(param_yaml.c_str(), cv::FileStorage::READ);
    if(!fs.isOpened()){
        fprintf(stderr, "%s : cannot open file\n", param_yaml.c_str());
        exit(EXIT_FAILURE);
    }

    cv::FileNode tm = fs["checkerboard"];
    cv::Mat m_intrinsic_opencv2to1;
    cv::Mat m_dist_opencv2to1;
    CV_Assert(tm.type() == cv::FileNode::MAP && tm.size() == 4);
    pat_row = static_cast<int>(tm["pat_row"]);
    pat_col = static_cast<int>(tm["pat_col"]);
    chess_size =static_cast<double>(tm["chess_size"]);
    paper_width_margin = static_cast<double>(tm["paper_width_margin"]);
    pat_size = pat_col * pat_row;
    tm = fs["window_lrf"];
    CV_Assert(tm.type() == cv::FileNode::MAP && tm.size() == 4);
    window_scan_width = static_cast<int>(tm["width"]);
    window_scan_height = static_cast<int>(tm["height"]);
    line_division_num = static_cast<int>(tm["line_division_num"]);
    judge_margin = static_cast<double>(tm["judge_margin"]);
    fs["intrinsic_matrix"] >> m_intrinsic_opencv2to1;
    fs["distrotion_matrix"] >> m_dist_opencv2to1;
    *m_intrinsic = m_intrinsic_opencv2to1;
    *m_dist = m_dist_opencv2to1;

    /* Create windows */
    cvNamedWindow(WINDOW_NAME_SCAN, CV_WINDOW_AUTOSIZE);
    cvNamedWindow(WINDOW_NAME_IMAGE, CV_WINDOW_AUTOSIZE);

    /* Create bar in window*/
    int default_vector_trackbar = 1000;
    int default_rad_trackbar = 180;
    cvCreateTrackbar ("vector_X", WINDOW_NAME_IMAGE, &default_vector_trackbar, 2000, vector_x_bar);
    cvCreateTrackbar ("vector_Y", WINDOW_NAME_IMAGE, &default_vector_trackbar, 2000, vector_y_bar);
    cvCreateTrackbar ("vector_Z", WINDOW_NAME_IMAGE, &default_vector_trackbar, 2000, vector_z_bar);
    cvCreateTrackbar ("rad_X", WINDOW_NAME_IMAGE, &default_rad_trackbar, 360, rad_x_bar);
    cvCreateTrackbar ("rad_Y", WINDOW_NAME_IMAGE, &default_rad_trackbar, 360, rad_y_bar);
    cvCreateTrackbar ("rad_Z", WINDOW_NAME_IMAGE, &default_rad_trackbar, 360, rad_z_bar);

    ros::Subscriber scan_sub = n.subscribe("scan", 1, scanCallback);
    ros::Subscriber image_sub = n.subscribe(image_topic_name, 1, imageCallback);

    ros::spin();

    cvDestroyWindow(WINDOW_NAME_SCAN);
    cvDestroyWindow(WINDOW_NAME_IMAGE);
    release_common();
    release_image_window();

    return 0;
}


/*
 Copyright (C) 2017 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.
*/

/*
 Copyright (C) 2008, 2009 Jose Aparicio
 Copyright (C) 2008 Chris Kenyon
 Copyright (C) 2008 Roland Lichters
 Copyright (C) 2008 StatPro Italia srl

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

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

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

#include <qle/instruments/creditdefaultswap.hpp>
#include <qle/pricingengines/midpointcdsengine.hpp>
#include <qle/termstructures/defaultprobabilityhelpers.hpp>

#include <ql/utilities/null_deleter.hpp>
#include <ql/time/daycounters/actual360.hpp>

#include <boost/make_shared.hpp>

namespace QuantExt {

CdsHelper::CdsHelper(const Handle<Quote>& quote, const Period& tenor, Integer settlementDays, const Calendar& calendar,
                     Frequency frequency, BusinessDayConvention paymentConvention, DateGeneration::Rule rule,
                     const DayCounter& dayCounter, Real recoveryRate, const Handle<YieldTermStructure>& discountCurve,
                     const Date& startDate, bool settlesAccrual, bool paysAtDefaultTime)
    : RelativeDateDefaultProbabilityHelper(quote), tenor_(tenor), settlementDays_(settlementDays), calendar_(calendar),
      frequency_(frequency), paymentConvention_(paymentConvention), rule_(rule), dayCounter_(dayCounter),
      recoveryRate_(recoveryRate), discountCurve_(discountCurve), settlesAccrual_(settlesAccrual),
      paysAtDefaultTime_(paysAtDefaultTime), startDate_(startDate) {

    initializeDates();

    registerWith(discountCurve);
}

CdsHelper::CdsHelper(Rate quote, const Period& tenor, Integer settlementDays, const Calendar& calendar,
                     Frequency frequency, BusinessDayConvention paymentConvention, DateGeneration::Rule rule,
                     const DayCounter& dayCounter, Real recoveryRate, const Handle<YieldTermStructure>& discountCurve,
                     const Date& startDate, bool settlesAccrual, bool paysAtDefaultTime)
    : RelativeDateDefaultProbabilityHelper(quote), tenor_(tenor), settlementDays_(settlementDays), calendar_(calendar),
      frequency_(frequency), paymentConvention_(paymentConvention), rule_(rule), dayCounter_(dayCounter),
      recoveryRate_(recoveryRate), discountCurve_(discountCurve), settlesAccrual_(settlesAccrual),
      paysAtDefaultTime_(paysAtDefaultTime), startDate_(startDate) {

    initializeDates();

    registerWith(discountCurve);
}

void CdsHelper::setTermStructure(DefaultProbabilityTermStructure* ts) {
    RelativeDateDefaultProbabilityHelper::setTermStructure(ts);

    probability_.linkTo(boost::shared_ptr<DefaultProbabilityTermStructure>(ts, null_deleter()), false);

    resetEngine();
}

void CdsHelper::update() {
    RelativeDateDefaultProbabilityHelper::update();
    resetEngine();
}

void CdsHelper::initializeDates() {
    protectionStart_ = evaluationDate_ + settlementDays_;

    Date startDate, endDate;
    if (startDate_ == Date()) {
        startDate = calendar_.adjust(protectionStart_, paymentConvention_);
        if (rule_ == DateGeneration::CDS || rule_ == DateGeneration::CDS2015) { // for standard CDS ..
            // .. the start date is not adjusted
            startDate = protectionStart_;
        }
        endDate = protectionStart_ + tenor_;
    } else {
        if (!schedule_.empty())
            return; // no need to update schedule
        startDate = calendar_.adjust(startDate_, paymentConvention_);
        endDate = startDate_ + settlementDays_ + tenor_;
    }

    schedule_ = MakeSchedule()
                    .from(startDate)
                    .to(endDate)
                    .withFrequency(frequency_)
                    .withCalendar(calendar_)
                    .withConvention(paymentConvention_)
                    .withTerminationDateConvention(Unadjusted)
                    .withRule(rule_);
    earliestDate_ = schedule_.dates().front();
    latestDate_ = calendar_.adjust(schedule_.dates().back(), paymentConvention_);
}

SpreadCdsHelper::SpreadCdsHelper(const Handle<Quote>& runningSpread, const Period& tenor, Integer settlementDays,
                                 const Calendar& calendar, Frequency frequency, BusinessDayConvention paymentConvention,
                                 DateGeneration::Rule rule, const DayCounter& dayCounter, Real recoveryRate,
                                 const Handle<YieldTermStructure>& discountCurve, const Date& startDate,
                                 bool settlesAccrual, bool paysAtDefaultTime)
    : CdsHelper(runningSpread, tenor, settlementDays, calendar, frequency, paymentConvention, rule, dayCounter,
                recoveryRate, discountCurve, startDate, settlesAccrual, paysAtDefaultTime) {}

SpreadCdsHelper::SpreadCdsHelper(Rate runningSpread, const Period& tenor, Integer settlementDays,
                                 const Calendar& calendar, Frequency frequency, BusinessDayConvention paymentConvention,
                                 DateGeneration::Rule rule, const DayCounter& dayCounter, Real recoveryRate,
                                 const Handle<YieldTermStructure>& discountCurve, const Date& startDate,
                                 bool settlesAccrual, bool paysAtDefaultTime)
    : CdsHelper(runningSpread, tenor, settlementDays, calendar, frequency, paymentConvention, rule, dayCounter,
                recoveryRate, discountCurve, startDate, settlesAccrual, paysAtDefaultTime) {}

Real SpreadCdsHelper::impliedQuote() const {
    swap_->recalculate();
    return swap_->fairSpread();
}

void SpreadCdsHelper::resetEngine() {

    // For CDS, the standard day counter is Actual/360 and the final period coupon accrual includes the maturity date.
    Actual360 standardDayCounter;
    DayCounter lastPeriodDayCounter = dayCounter_ == standardDayCounter ? Actual360(true) : dayCounter_;

    swap_ = boost::shared_ptr<QuantExt::CreditDefaultSwap>(new QuantExt::CreditDefaultSwap(
        Protection::Buyer, 100.0, 0.01, schedule_, paymentConvention_, dayCounter_, settlesAccrual_,
        paysAtDefaultTime_, protectionStart_, boost::shared_ptr<Claim>(), lastPeriodDayCounter));

    swap_->setPricingEngine(
        boost::make_shared<QuantExt::MidPointCdsEngine>(probability_, recoveryRate_, discountCurve_));
}

UpfrontCdsHelper::UpfrontCdsHelper(const Handle<Quote>& upfront, Rate runningSpread, const Period& tenor,
                                   Integer settlementDays, const Calendar& calendar, Frequency frequency,
                                   BusinessDayConvention paymentConvention, DateGeneration::Rule rule,
                                   const DayCounter& dayCounter, Real recoveryRate,
                                   const Handle<YieldTermStructure>& discountCurve, const Date& startDate,
                                   Natural upfrontSettlementDays, bool settlesAccrual, bool paysAtDefaultTime)
    : CdsHelper(upfront, tenor, settlementDays, calendar, frequency, paymentConvention, rule, dayCounter, recoveryRate,
                discountCurve, startDate, settlesAccrual, paysAtDefaultTime),
      upfrontSettlementDays_(upfrontSettlementDays), runningSpread_(runningSpread) {
    initializeDates();
}

UpfrontCdsHelper::UpfrontCdsHelper(Rate upfrontSpread, Rate runningSpread, const Period& tenor, Integer settlementDays,
                                   const Calendar& calendar, Frequency frequency,
                                   BusinessDayConvention paymentConvention, DateGeneration::Rule rule,
                                   const DayCounter& dayCounter, Real recoveryRate,
                                   const Handle<YieldTermStructure>& discountCurve, const Date& startDate,
                                   Natural upfrontSettlementDays, bool settlesAccrual, bool paysAtDefaultTime)
    : CdsHelper(upfrontSpread, tenor, settlementDays, calendar, frequency, paymentConvention, rule, dayCounter,
                recoveryRate, discountCurve, startDate, settlesAccrual, paysAtDefaultTime),
      upfrontSettlementDays_(upfrontSettlementDays), runningSpread_(runningSpread) {
    initializeDates();
}

void UpfrontCdsHelper::initializeDates() {
    CdsHelper::initializeDates();
    upfrontDate_ = calendar_.advance(evaluationDate_, upfrontSettlementDays_, Days, paymentConvention_);
}

Real UpfrontCdsHelper::impliedQuote() const {
    SavedSettings backup;
    Settings::instance().includeTodaysCashFlows() = true;
    swap_->recalculate();
    return swap_->fairUpfront();
}

void UpfrontCdsHelper::resetEngine() {
    swap_ = boost::shared_ptr<CreditDefaultSwap>(new CreditDefaultSwap(
        Protection::Buyer, 100.0, 0.01, runningSpread_, schedule_, paymentConvention_, dayCounter_, settlesAccrual_,
        paysAtDefaultTime_, protectionStart_, upfrontDate_, boost::shared_ptr<Claim>()));
    swap_->setPricingEngine(
        boost::make_shared<QuantExt::MidPointCdsEngine>(probability_, recoveryRate_, discountCurve_, true));
}
} // namespace QuantExt


// Copyright (C) 2021 Christian Brommer, Control of Networked Systems, University of Klagenfurt, Austria.
//
// All rights reserved.
//
// This software is licensed under the terms of the BSD-2-Clause-License with
// no commercial use allowed, the full terms of which are made available
// in the LICENSE file. No license in patents is granted.
//
// You can contact the author at <christian.brommer@ieee.org>

#include "mars_wrapper_gps.h"
#include <mars/core_logic.h>
#include <mars/core_state.h>
#include <mars/sensors/gps/gps_measurement_type.h>
#include <mars/sensors/imu/imu_measurement_type.h>
#include <mars/sensors/imu/imu_sensor_class.h>
#include <mars/type_definitions/buffer_data_type.h>
#include <mars/type_definitions/buffer_entry_type.h>
#include <mars_msg_conv.h>
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/NavSatFix.h>
#include <Eigen/Dense>
#include <iostream>
#include <string>
#include "mars_msg_conv.h"

using namespace mars;

MarsWrapperGps::MarsWrapperGps(ros::NodeHandle nh)
  : reconfigure_cb_(boost::bind(&MarsWrapperGps::configCallback, this, _1, _2))
  , p_wi_init_(0, 0, 0)
  , q_wi_init_(Eigen::Quaterniond::Identity())
{
  reconfigure_srv_.setCallback(reconfigure_cb_);

  initialization_service_ = nh.advertiseService("init_service", &MarsWrapperGps::initServiceCallback, this);

  // Read parameter
  publish_on_propagation_ = nh.param<bool>("pub_on_prop", publish_on_propagation_);
  use_ros_time_now_ = nh.param<bool>("use_ros_time_now", use_ros_time_now_);
  verbose_output_ = nh.param<bool>("verbose", verbose_output_);
  verbose_ooo_ = nh.param<bool>("verbose_out_of_order", verbose_ooo_);
  discard_ooo_prop_meas_ = nh.param<bool>("discard_ooo_prop_meas", discard_ooo_prop_meas_);

  pub_cb_buffer_size_ = uint32_t(nh.param<int>("pub_cb_buffer_size", int(pub_cb_buffer_size_)));
  sub_imu_cb_buffer_size_ = uint32_t(nh.param<int>("sub_imu_cb_buffer_size", int(sub_imu_cb_buffer_size_)));
  sub_sensor_cb_buffer_size_ = uint32_t(nh.param<int>("sub_sensor_cb_buffer_size", int(sub_sensor_cb_buffer_size_)));

  std::cout << "Setup framework components" << std::endl;

  // Framework components
  imu_sensor_sptr_ = std::make_shared<mars::ImuSensorClass>("IMU");
  core_states_sptr_ = std::make_shared<mars::CoreState>();
  core_states_sptr_.get()->set_propagation_sensor(imu_sensor_sptr_);
  core_logic_ = mars::CoreLogic(core_states_sptr_);
  core_logic_.buffer_.set_max_buffer_size(800);

  core_logic_.verbose_ = verbose_output_;
  core_logic_.verbose_out_of_order_ = verbose_ooo_;
  core_logic_.discard_ooo_prop_meas_ = discard_ooo_prop_meas_;

  core_states_sptr_->set_noise_std(Eigen::Vector3d(0.013, 0.013, 0.013), Eigen::Vector3d(0.0013, 0.0013, 0.0013),
                                   Eigen::Vector3d(0.083, 0.083, 0.083), Eigen::Vector3d(0.0083, 0.0083, 0.0083));

  // Sensors
  gps1_sensor_sptr_ = std::make_shared<mars::GpsSensorClass>("Gps1", core_states_sptr_);

  {  // Limit scope of temp variables
    Eigen::Matrix<double, 3, 1> gps_meas_std;
    gps_meas_std << 2.0, 2.0, 2.0;
    gps1_sensor_sptr_->R_ = gps_meas_std.cwiseProduct(gps_meas_std);

    GpsSensorData gps_calibration;
    gps_calibration.state_.p_ig_ = Eigen::Vector3d(0, 0, 0.10);
    Eigen::Matrix<double, 9, 9> gps_cov;
    gps_cov.setZero();
    gps_cov.diagonal() << 1e-2, 1e-2, 1e-2, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001;
    gps_calibration.sensor_cov_ = gps_cov;
    gps1_sensor_sptr_->set_initial_calib(std::make_shared<GpsSensorData>(gps_calibration));

    // TODO is set here for now, but will be managed by core logic in later versions
    gps1_sensor_sptr_->const_ref_to_nav_ = true;
  }

  // Subscriber
  sub_imu_measurement_ = nh.subscribe("imu_in", sub_imu_cb_buffer_size_, &MarsWrapperGps::ImuMeasurementCallback, this);
  sub_gps1_measurement_ =
      nh.subscribe("gps_in", sub_sensor_cb_buffer_size_, &MarsWrapperGps::GpsMeasurementCallback, this);

  // Publisher
  pub_ext_core_state_ = nh.advertise<mars_ros::ExtCoreState>("core_ext_state_out", pub_cb_buffer_size_);
  pub_ext_core_state_lite_ = nh.advertise<mars_ros::ExtCoreStateLite>("core_ext_state_lite_out", pub_cb_buffer_size_);
  pub_core_pose_state_ = nh.advertise<geometry_msgs::PoseStamped>("core_pose_state_out", pub_cb_buffer_size_);
  pub_gps_state_ = nh.advertise<geometry_msgs::PoseWithCovarianceStamped>("gps_cal_state_out", pub_cb_buffer_size_);
}

bool MarsWrapperGps::init()
{
  core_logic_.core_is_initialized_ = false;
  core_logic_.buffer_.ResetBufferData();
  gps1_sensor_sptr_->is_initialized_ = false;
  common_gps_ref_is_set_ = false;

  return true;
}

bool MarsWrapperGps::initServiceCallback(std_srvs::SetBool::Request& /*request*/,
                                         std_srvs::SetBool::Response& /*response*/)
{
  init();
  ROS_INFO_STREAM("Initialized filter trough ROS Service");

  return true;
}

void MarsWrapperGps::configCallback(mars_ros::marsConfig& config, uint32_t /*level*/)
{
  // Config parameter overwrite
  publish_on_propagation_ = config.pub_on_prop;
  core_logic_.verbose_ = config.verbose;
  verbose_output_ = config.verbose;
  use_ros_time_now_ = config.use_ros_time_now;

  if (config.initialize)
  {
    init();
    ROS_INFO_STREAM("Initialized filter trough Reconfigure GUI");
  }

  config.initialize = false;
}

void MarsWrapperGps::set_common_gps_reference(const GpsCoordinates& reference)
{
  if (!common_gps_ref_is_set_)
  {
    std::cout << "Setting the common GPS reference to: \n" << reference << std::endl;

    gps1_sensor_sptr_->set_gps_reference_coordinates(reference);

    common_gps_ref_is_set_ = true;
  }
}

void MarsWrapperGps::ImuMeasurementCallback(const sensor_msgs::ImuConstPtr& meas)
{
  // Map the measutement to the mars type
  Time timestamp(meas->header.stamp.toSec());

  // Generate a measurement data block
  BufferDataType data;
  data.set_sensor_data(std::make_shared<IMUMeasurementType>(MarsMsgConv::ImuMsgToImuMeas(*meas)));

  // Call process measurement
  core_logic_.ProcessMeasurement(imu_sensor_sptr_, timestamp, data);

  // Initialize the first time at which the propagation sensor occures
  if (!core_logic_.core_is_initialized_)
  {
    core_logic_.Initialize(p_wi_init_, q_wi_init_);
  }

  if (publish_on_propagation_)
  {
    mars::BufferEntryType latest_state;
    core_logic_.buffer_.get_latest_state(&latest_state);
    mars::CoreStateType latest_core_state = static_cast<mars::CoreType*>(latest_state.data_.core_.get())->state_;

    pub_ext_core_state_.publish(
        MarsMsgConv::ExtCoreStateToMsg(latest_state.timestamp_.get_seconds(), latest_core_state));
  }
}

void MarsWrapperGps::GpsMeasurementCallback(const sensor_msgs::NavSatFixConstPtr& meas)
{
  Time timestamp;

  if (use_ros_time_now_)
  {
    timestamp = Time(ros::Time::now().toSec());
  }
  else
  {
    timestamp = Time(meas->header.stamp.toSec());
  }

  // Map the measurement to the mars sensor type
  GpsMeasurementType gps_meas = MarsMsgConv::NavSatFixMsgToGpsMeas(*meas);
  set_common_gps_reference(gps_meas.coordinates_);

  GpsMeasurementUpdate(gps1_sensor_sptr_, gps_meas, timestamp);
}

void MarsWrapperGps::RunCoreStatePublisher()
{
  mars::BufferEntryType latest_state;
  core_logic_.buffer_.get_latest_state(&latest_state);
  mars::CoreStateType latest_core_state = static_cast<mars::CoreType*>(latest_state.data_.core_.get())->state_;

  pub_ext_core_state_.publish(MarsMsgConv::ExtCoreStateToMsg(latest_state.timestamp_.get_seconds(), latest_core_state));
  pub_ext_core_state_lite_.publish(
      MarsMsgConv::ExtCoreStateLiteToMsg(latest_state.timestamp_.get_seconds(), latest_core_state));

  pub_core_pose_state_.publish(
      MarsMsgConv::ExtCoreStateToPoseMsg(latest_state.timestamp_.get_seconds(), latest_core_state));
}

void MarsWrapperGps::GpsMeasurementUpdate(std::shared_ptr<mars::GpsSensorClass> sensor_sptr,
                                          const GpsMeasurementType& gps_meas, const Time& timestamp)
{
  // TMP feedback init pose
  // p_wi_init_ = pose_meas.position_;
  // q_wi_init_ = pose_meas.orientation_;

  // Generate a measurement data block
  BufferDataType data;
  data.set_sensor_data(std::make_shared<GpsMeasurementType>(gps_meas));

  // Call process measurement
  if (!core_logic_.ProcessMeasurement(sensor_sptr, timestamp, data))
  {
    return;
  }

  // Publish the latest core state
  this->RunCoreStatePublisher();

  // Publish latest sensor state
  mars::BufferEntryType latest_sensor_state;
  core_logic_.buffer_.get_latest_sensor_handle_state(sensor_sptr, &latest_sensor_state);

  mars::GpsSensorStateType gps_sensor_state = sensor_sptr.get()->get_state(latest_sensor_state.data_.sensor_);
  pub_gps_state_.publish(MarsMsgConv::GpsStateToMsg(latest_sensor_state.timestamp_.get_seconds(), gps_sensor_state));
}