// General ROS functionality
#include <ros/ros.h>

// Simulator services
#include <sim/Insert.h>
#include <sim/Resume.h>
#include <sim/Pause.h>

// PI
#include "pi/PIController.h"
#include "pi/global.h"
#include <boost/property_tree/xml_parser.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/foreach.hpp>
// END PI

// Quadrotor services
#include <hal_quadrotor/State.h>

// Quadrotor controllers
#include <hal_quadrotor/control/Takeoff.h>
#include <hal_quadrotor/control/VelocityHeight.h>
//#include <hal_quadrotor/control/Waypoint.h>
//#include <hal_quadrotor/control/Hover.h>

#define EPS_POS			5e-1	// epsilon position
#define EPS_VEL			1e-3	// epsilon position

typedef enum { IDLE, TAKEOFF, VELHEIGHT, HOVER, WAYPOINT, OTHER } FSM;

vector<ros::ServiceClient> srvTakeOff;
vector<ros::ServiceClient> srvVelocityHeight;

using namespace std;

vector<FSM> state_FSM; // Finite State Machine state (TODO: get the controller from the msg)
vector<double> jstate;

// PI
PIController pi;
// END PI

vector<double> heights;
vector<string> idunits;
int units;

// compares two states
bool sameState(const hal_quadrotor::State::ConstPtr& s1, const hal_quadrotor::State &s2)
{
	double dpos = sqrt((s1->x-s2.x)*(s1->x-s2.x) + (s1->y-s2.y)*(s1->y-s2.y) + (s1->z-s2.z)*(s1->z-s2.z));
	double dvel = sqrt((s1->u-s2.u)*(s1->u-s2.u) + (s1->v-s2.v)*(s1->v-s2.v));
 	ROS_INFO("dpos: [%f %f]", dpos, dvel);
	return ((dpos < EPS_POS) && (dvel < EPS_VEL));
}

bool sameHeight(const hal_quadrotor::State::ConstPtr& s1, const hal_quadrotor::State &s2)
{
	double dpos = sqrt((s1->z-s2.z)*(s1->z-s2.z));
	double dvel = sqrt((s1->u-s2.u)*(s1->u-s2.u) + (s1->v-s2.v)*(s1->v-s2.v));
	return ((dpos < EPS_POS) && (dvel < EPS_VEL));
}

// Callback for quadrotor state
void StateCallback(int unit, const hal_quadrotor::State::ConstPtr& msg)
{
	switch (state_FSM[unit]) {
		case IDLE: {
  		ROS_INFO("IDLE, trying to take off");
			hal_quadrotor::Takeoff msgTakeoff;
			msgTakeoff.request.altitude = heights[unit];
			if(!srvTakeOff[unit].call(msgTakeoff)){
				ROS_FATAL("NO TAKE OFF!!");
			} else {
				ROS_INFO("Taking OFF!!");
				state_FSM[unit] = TAKEOFF;
			}
		}	break;
		case TAKEOFF: {
			hal_quadrotor::State s;
			s.x = 5.; s.y = 5.; s.z = heights[unit];
			s.u = 0.; s.v = 0.;
			if (sameHeight(msg, s)) {
				ROS_INFO("TAKEN OFF, switch to HOVER");
				state_FSM[unit] = HOVER;
			}
		} break;
		case HOVER: {
			// in HOVER we will actually do PI control
  		ROS_INFO("HOVER pos: [%f,%f,%f]", msg->x, msg->y, msg->z);
			jstate[unit*4] = msg->x;
			jstate[unit*4+1] = msg->y;
			jstate[unit*4+2] = msg->u;
			jstate[unit*4+3] = msg->v;
		} break;
		default: {
  		ROS_INFO("pos: [%f,%f,%f]", msg->x, msg->y, msg->z);
		} break;
	}

}

// Main entry point of application
int main(int argc, char **argv)
{
	// PI
	if (argc!=2) {
		cout << "requires xml_file as argument" << endl;
		exit(-1);
	}

	try {
		ifstream is(argv[1]);
		// read parameters from xml file
		using boost::property_tree::ptree;
		ptree pt;
		if (!is.is_open()) {
			cout << "file not found" << endl;
			exit(-1);
		}
		read_xml(is, pt);

//	// task parameters
//	double dt = pt.get<double>("dt");
//	double R = pt.get<double>("R");
//	int dtperstep = pt.get<int>("dtperstep");
//	int H = pt.get<int>("H");
//	double nu = pt.get<double>("nu");
//	double T = pt.get<double>("T");
//	int seed = pt.get<int>("seed");
//	int N = pt.get<int>("N");
//	units = pt.get<int>("units");
//	int dimUAVx = pt.get<int>("dimUAVx");
//	int dimUAVu = pt.get<int>("dimUAVu");
//	BOOST_FOREACH(ptree::value_type &v,	pt.get_child("takeoffz")) {
//		heights.push_back(v.second.get<double>(""));
//	}
//	BOOST_FOREACH(ptree::value_type &v,	pt.get_child("idunits")) {
//		idunits.push_back(v.second.get<string>(""));
//	}

//	jstate.resize( units*dimUAVx );
//
//	double dt_qrsim = 1.; // CHANGE
//	double dS = dt*dtperstep;
//	int dtperstep_qrsim = int(dS/dt_qrsim);
//	double dS_qrsim = dt_qrsim*dtperstep_qrsim;

// 	// initialize static variables
//	PIController::dt = dt;
//	PIController::units = units;
//	PIController::R = R;
//	PIController::nu = nu;
//	PIController::lambda = R*nu;
//	PIController::dtperstep = dtperstep;
//	PIController::H = H;
//	PIController::dS = dt*dtperstep;
//	PIController::stdv = sqrt(nu/dS);

		// Initial state X should be input as well (don't forget tpo convert)
		// create instance of PIController
		//pi.Init(dimUAVx, dimUAVu,	seed,	N);
		// Initial state X should be input as well (don't forget tpo convert)
		// create instance of PIController
		pi.Init(pt);//prop);

		double T = pt.get<double>("T");

	} catch (int e) {
		cout << "Exception" << e << endl;
	}

	// Initialise the ROS client
	string nodestr = string("example");
  ros::init(argc, argv, nodestr.c_str());

  // Create a node handle, which this C code will use to bind to the ROS server
  ros::NodeHandle n;

  // Create a client for interacting with the Simulator insert and Resume services
  ros::ServiceClient srvInsert = n.serviceClient<sim::Insert>("/simulator/Insert");
  ros::ServiceClient srvResume = n.serviceClient<sim::Resume>("/simulator/Resume");
  ros::ServiceClient srvPause  = n.serviceClient<sim::Pause> ("/simulator/Pause");

	// insert UVAs
	vector<ros::Subscriber> top_state;
	for (int i=0; i<units; i++) {
		// Subscribe to the states of the quadrotors (requires explicit)
		string event = string("hal/") + idunits[i] + string("/Estimate");
		top_state.push_back( n.subscribe<hal_quadrotor::State>(
				event,
				1000,
				boost::bind(StateCallback, i, _1)
			)
		);

		// init Finite State Machine
		state_FSM.push_back(IDLE);

		// Create a resume message, which takes no arguments
		sim::Pause msgPause;

		// Call the client with this message
		if (!srvPause.call(msgPause))
		{
		  ROS_FATAL("Failed to pause the simulator");
		  return 1;
		}

		// Create a message instructing a quadrotor UAV0 to be inserted in to the world
		sim::Insert msgInsert;
		msgInsert.request.model_name = idunits[i];
		msgInsert.request.model_type = "model://hummingbird";

		// Call the client with this message
		if (!srvInsert.call(msgInsert))
		{
		  ROS_FATAL("Failed to insert a hummingbird quadrotor into the simulator");
		  return 1;
		}

		// You should now see the quadrotor
		cout << idunits[i] << endl;
		ROS_INFO("Successfully inserted the model into the world");

		// Create a resume message, which takes no arguments
	  sim::Resume msgResume;

	  // Call the client with this message
	  if (!srvResume.call(msgResume))
	  {
	    ROS_FATAL("Failed to resume the simulator");
	    return 1;
	  }
	}

  // rosservice call /hal/UAV0/controller/Takeoff 5
	for (int i=0; i<units; i++) {
		string takeoff_cmd = string("hal/") + idunits[i] + string("/controller/Takeoff");
		srvTakeOff.push_back( n.serviceClient<hal_quadrotor::Takeoff>(takeoff_cmd.c_str()) );

		string velheight_cmd = string("hal/") + idunits[i] + string("/controller/VelocityHeight");
		srvVelocityHeight.push_back( n.serviceClient<hal_quadrotor::VelocityHeight>(velheight_cmd.c_str()) );
	}

  //ros::Rate loop_rate(1);

  while (ros::ok())
  {
  	// Instead of sleep we use our PI controller
    //loop_rate.sleep();

		// During sleep (or PI-control) all messages get buffered. A spin will cause the Callbacks to work through the buffered messages.
    ros::spinOnce();

		// pi controller (based on VelocityHeight)
		// convert state to pi state

		bool ready = true;
		for (int i=0; i<units; i++) {
			ready = ready && (state_FSM[i] == HOVER);
		}

		if (ready) {
			vec X = jstate;
			vec action = pi.computeControl(X);
			vec Xp = pi.predictState(X, action);

			for (int i=0; i<units; i++) {

				// convert pi control to VelocityHeight control
				hal_quadrotor::VelocityHeight msgVelocityHeight;
				msgVelocityHeight.request.dx = Xp[i*4 + 2];
				msgVelocityHeight.request.dy = Xp[i*4 + 3];
				msgVelocityHeight.request.z = heights[i];
				msgVelocityHeight.request.yaw = 0.;

				if(!srvVelocityHeight[i].call(msgVelocityHeight)){
					ROS_FATAL("NO VELHEIGHT!!");
				}
			}
		}

  }

  // Success!
  return 0;
}


//
// Created by Alex Beccaro on 16/01/2020.
//

#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include "../../src/problems/101-150/145/problem145.hpp"

BOOST_AUTO_TEST_SUITE( Problem145 )

    BOOST_AUTO_TEST_CASE( Example ) {
        auto res = problems::problem145::solve(3);
        BOOST_CHECK_EQUAL(res, 120);
    }

    BOOST_AUTO_TEST_CASE( Solution ) {
        auto res = problems::problem145::solve();
        BOOST_CHECK_EQUAL(res, 608720);
    }

BOOST_AUTO_TEST_SUITE_END()


/*******************************************************************************
* nemesis. an experimental finite element code.                                *
* Copyright (C) 2004-2011 F.E.Karaoulanis [http://www.nemesis-project.org]     *
*                                                                              *
* This program is free software; you can redistribute it and/or modify         *
* it under the terms of the GNU General Public License version 3, as           *
* published by the Free Software Foundation.                                   *
*                                                                              *
* This program is distributed in the hope that it will be useful,              *
* but WITHOUT ANY WARRANTY; without even the implied warranty of               *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the                *
* GNU General Public License for more details.                                 *
*                                                                              *
* You should have received a copy of the GNU General Public License            *
* along with this program.  If not, see < http://www.gnu.org/licenses/>.       *
*******************************************************************************/

// *****************************************************************************
// $LastChangedDate$
// $LastChangedRevision$
// $LastChangedBy$
// $HeadURL$
// Author(s): F.E. Karaoulanis (fkar@nemesis-project.org)
// *****************************************************************************

#include "reorderer/reverse_cuthill_mckee.h"
#include <boost/graph/cuthill_mckee_ordering.hpp>
#include <stdio.h>
#include "analysis/analysis.h"
#include "model/model.h"

ReverseCuthillMckee::ReverseCuthillMckee() {
  myTag = TAG_REORDERER_REVERSE_CUTHILL_MCKEE;
}
ReverseCuthillMckee::~ReverseCuthillMckee() {
}
int ReverseCuthillMckee::get_perm(std::vector<int>& perm) {
  // Create the Graph and additional vectors
  UndirectedGraph G(pA->get_model()->get_num_eqns());
  pA->get_model()->get_undirected_graph(&G);
  property_map < UndirectedGraph, vertex_index_t>::type
    index_map = get(vertex_index, G);
  std::vector<int> inv_perm(num_vertices(G));
  perm.resize(num_vertices(G));

  // Find old bandwidth and profile
  int oldBandwidth = bandwidth(G);
  int oldProfile = profile(G);

  // Call Cuthill McKee algorithm
  cuthill_mckee_ordering(G, inv_perm.rbegin(), get(vertex_color, G),
                         make_degree_map(G));
  for (unsigned k = 0; k != inv_perm.size(); k++)
    perm[index_map[inv_perm[k]]]=k;

  // Find new bandwidth and profile
  int newBandwidth = bandwidth(G,
                      make_iterator_property_map(&perm[0], index_map, perm[0]));
  int newProfile = profile(G, make_iterator_property_map(&perm[0], index_map));

  // Print optimized sizes
  printf("reo: Optimized (original) bandwidth : %d (%d)\n",
         newBandwidth, oldBandwidth);
  printf("reo: Optimized (original) profile   : %d (%d)\n",
         newProfile, oldProfile);
  // Check if optimization is needed
  if (newBandwidth > oldBandwidth) return -1;
  return 1;
}


///////////////////////////////////////////////////////////////////////////////
// random::include.hpp                                                       //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_RANDOM_INCLUDE_HPP_ER_2009
#define BOOST_RANDOM_INCLUDE_HPP_ER_2009

#include <boost/random/chi_squared.hpp>
#include <boost/random/gen_to_random.hpp>
#include <boost/random/students_t.hpp>
#include <boost/random/location_scale.hpp>
#include <boost/random/ref_distribution.hpp>

#endif


#include "NuiPointCloudShape.h"

#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>

#include <boost/make_shared.hpp>

NuiPointCloudShape::NuiPointCloudShape(SgVec3f* vertices, SgVec4f* colors, int pointSize)
{
#ifdef WITH_OPENMP
#pragma omp parallel for
#endif
	for (int i = 0; i < pointSize; i++)
	{
		pcl::PointXYZRGB pt;
		pt.x = vertices[i][0];
		pt.y = vertices[i][1];
		pt.z = vertices[i][2];
		pt.r = colors[i][0];
		pt.g = colors[i][1];
		pt.b = colors[i][2];
		pt.a = colors[i][3];
		m_cloud.push_back(pt);
	}
}

NuiPointCloudShape::~NuiPointCloudShape()
{
	clear();
}

void	NuiPointCloudShape::clear() {
	m_cloud.clear();
	m_processedCloud.clear();
}

void	NuiPointCloudShape::processCloud(float leafSize)
{
	pcl::VoxelGrid<pcl::PointXYZRGB> sor;
	sor.setInputCloud(boost::make_shared<pcl::PointCloud<pcl::PointXYZRGB>>(m_cloud));
	sor.setLeafSize(leafSize, leafSize, leafSize);

	pcl::PointCloud<pcl::PointXYZRGB> tempCloud;
	sor.filter(tempCloud);

	pcl::NormalEstimation<pcl::PointXYZRGB, pcl::Normal> n;
	pcl::PointCloud<pcl::Normal> normals;
	pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZRGB>);

	tree->setInputCloud(boost::make_shared<pcl::PointCloud<pcl::PointXYZRGB>>(tempCloud));

	n.setInputCloud(boost::make_shared<pcl::PointCloud<pcl::PointXYZRGB>>(tempCloud));
	n.setSearchMethod(tree);
	n.setKSearch(20);
	n.compute(normals);

	pcl::concatenateFields(tempCloud, normals, m_processedCloud);
}


#ifndef WALI_UTIL_DETAILS_PARTITION_HPP
#define WALI_UTIL_DETAILS_PARTITION_HPP

#include <ostream>
#include <set>
#include <map>
#include <sstream>
#include <cassert>
#include <list>

#include <boost/pending/disjoint_sets.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/unordered_set.hpp>
#include <boost/unordered_map.hpp>

namespace wali
{
namespace util
{
namespace details
{
  class Partition;

  extern
  bool
  equal_partitions(const Partition &first, const Partition &second);

  // Implementation of Equivalence partition using disjoint-sets.
  class Partition
  {
    typedef boost::unordered_map<int, std::size_t> rank_t;
    typedef boost::unordered_map<int, int> parent_t;

    typedef boost::unordered_set<int> elements_t;

    typedef boost::associative_property_map<rank_t> rank_p_t;
    typedef boost::associative_property_map<parent_t> parent_p_t;

    typedef boost::disjoint_sets<rank_p_t, parent_p_t> DisjointSets;

    typedef boost::unordered_map<int, std::list<int> > root_set_map_t;

  private:
    mutable rank_t rank_; // Data structure to store rank.
    mutable parent_t parent_; // Data structure to store the parent info.
    mutable elements_t elements_; // The set of all elements in the partition.
    mutable DisjointSets disjoint_sets_; // The disjoint set data structure initialized with rank_ and parent_.
    mutable root_set_map_t rootSetMap_; // Map from roots of the partitions to the set of elements in that partition.

  public:

    // Constructor
    Partition(): disjoint_sets_(rank_p_t(rank_),
				parent_p_t(parent_))
    {}

    // Copy constructor
    // copy constructor
    Partition(const Partition &other)
      : rank_(other.rank_)
      , parent_(other.parent_)
      , elements_(other.elements_)
      ,	disjoint_sets_(rank_p_t(rank_), parent_p_t(parent_))
      , rootSetMap_(other.rootSetMap_)
      {
	//disjoint_sets_ = DisjointSets(rank_p_t(rank_), parent_p_t(parent_));
      }

    Partition &operator=(const Partition &rhs)
    {
      if (this != &rhs) {
	rank_ = rhs.rank_;
	parent_ = rhs.parent_;
	elements_ = rhs.elements_;
	disjoint_sets_ = DisjointSets(rank_p_t(rank_), parent_p_t(parent_));
	rootSetMap_ = rhs.rootSetMap_;
      }
      return *this;
    }

    // Clear the partition
    void Clear()
    {
      rank_.clear();
      parent_.clear();
      disjoint_sets_ = DisjointSets(rank_p_t(rank_), parent_p_t(parent_));
      elements_.clear();
      rootSetMap_.clear();
    }

    // Return the set of elements in partition
    boost::unordered_set<int> Elements() const
    {
      return elements_;
    }

    // Return the total number of elements in all partitions.
    std::size_t NumElements() const
    {
      return elements_.size();
    }

    // Normalize the partition so that the smallest element
    // is the root.
    // TODO this would be easy to maintain incrementally.
    //      But I don't think boost does not.
    void Normalize() const
    {
      disjoint_sets_.normalize_sets(elements_.begin(), elements_.end());
    }

    // Return true if the elements in the partition are unconstrained
    // that is, each disjoint set contains a single element
    // equivalently, the number of disjoint sets equals the number of elements.
    bool IsUnconstrained() const
    {
      return  CountSets() == elements_.size();
    }

    bool operator==(const Partition &rhs) const
    {
      return equal_partitions(*this, rhs);
    }

    bool operator!=(const Partition &rhs) const
    {
      return !equal_partitions(*this, rhs);
    }

    // The essential functionality.

    // Add the fact that v1 and v2 are equivalent
    // return true if v1 was not already equivalent to v2
    // and false if v1 was already equivalent to v2
    bool SetEquivalent(int v1, int v2)
    {
      bool const already_equiv = false;
      if (v1 == v2)
	return already_equiv;
      ensureElementExists(v1);
      ensureElementExists(v2);

      const int r1 = disjoint_sets_.find_set(v1);
      const int r2 = disjoint_sets_.find_set(v2);
      if (r1 == r2)
	return already_equiv;
      root_set_map_t::const_iterator it1 = rootSetMap_.find(r1);
      assert(it1 != rootSetMap_.end());
      std::list<int> s1 = it1->second;

      root_set_map_t::const_iterator  it2 = rootSetMap_.find(r2);
      assert(it2 != rootSetMap_.end());
      std::list<int> s2 = it2->second;

      s1.splice(s1.begin(), s2); // union the related sets

      disjoint_sets_.link(r1, r2); // union the disjoint sets

      // associate the combined related set with the new root
      int const new_root = disjoint_sets_.find_set(v1);
      if (new_root != r1) {
	rootSetMap_.erase(it1);
      } else {
	rootSetMap_.erase(it2);
      }
      rootSetMap_[new_root] = s1;
      return !already_equiv;
    }

    // Return true if v1 and v2 are equivalent
    bool AreEquivalent(int v1, int v2) const
    {
      if (v1 == v2) {
	return true;
      }

      // The element does not exist, so we do not
      // know any equivalence information,
      // hence, v1 and v2 are not equivalent.
      if (!elementExists(v1) || !elementExists(v2)) {
	return false;
      }

      return disjoint_sets_.find_set(v1) == disjoint_sets_.find_set(v2);
    }

    // Return the representative for the node v.
    int FindSet(int v) const
    {
      // If element does not exist
      if (!elementExists(v)) {
	// Then it is its own root.
	// This avoids adding singleton sets to the partition.
	return v;
      }

      return disjoint_sets_.find_set(v);
    }

    // Return the number of disjoint sets in the partition.
    std::size_t CountSets() const
    {
      return disjoint_sets_.count_sets(elements_.begin(), elements_.end());
    }

    // Return a list of elements that are in the same disjoint set
    // as v.
    std::list<int> GetRelatedSet(int v) const
    {
      std::list<int> ret;
      if (!elementExists(v)) {
	ret.push_back(v);
	return ret;
      }
      int root = disjoint_sets_.find_set(v);
      root_set_map_t::const_iterator it = rootSetMap_.find(root);
      assert(it!=rootSetMap_.end());
      return it->second;
    }

    // Return the current set of roots (or representatives) for
    // each of the partition
    std::set<int> GetRoots() const
    {
      std::set<int> roots;
      for (elements_t::const_iterator it=elements_.begin(); it!=elements_.end(); ++it) {
	if (FindSet(*it) == *it) {
	  roots.insert(*it);
	}
      }
      return roots;
    }

    // Stringify the data-structure
    std::string ToString() const
    {
      std::stringstream ss;
      std::set<int> roots = GetRoots();
      ss << "{ ";
      for (std::set<int>::const_iterator rit = roots.begin();
	                                 rit != roots.end(); ++rit) {
	ss << "[ " << *rit ;
	for (elements_t::const_iterator eit=elements_.begin(); eit!=elements_.end();++eit) {
	  if (*eit != *rit && FindSet(*eit) == *rit) {
	    ss << ", " << *eit;
	  }
	}
	ss << " ] ";
      }
      ss << "}";
      return ss.str();
    }

    void Insert(int x)
    {
      ensureElementExists(x);
    }

    bool elementExists(int v) const
    {
      elements_t::const_iterator it = elements_.find(v);
      return it != elements_.end();
    }

  private:
    // helper functions

    // If v does not exist, then add it to the disjoint set.
    void ensureElementExists(int v) const
    {
      elements_t::const_iterator it = elements_.find(v);
      if (it == elements_.end()) {
	disjoint_sets_.make_set(v);
	elements_.insert(v);

	std::list<int> li;
	li.push_front(v);
	rootSetMap_[v] = li;
      }
      return;
    }

  };

}
}
}

// Yo, Emacs!
// Local Variables:
//   c-file-style: "ellemtel"
//   c-basic-offset: 2
// End:

#endif


// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test

// Copyright (c) 2015, Oracle and/or its affiliates.

// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#ifndef BOOST_TEST_MODULE
#define BOOST_TEST_MODULE test_maximum_gap
#endif

#include <boost/test/included/unit_test.hpp>

#include <cstddef>

#include <iostream>
#include <sstream>
#include <vector>

#include <boost/geometry/algorithms/detail/max_interval_gap.hpp>

namespace bg = boost::geometry;

class uint_interval
{
public:
    typedef unsigned value_type;
    typedef int difference_type;

    uint_interval(unsigned left, unsigned right)
        : m_left(left)
        , m_right(right)
    {}

    template <std::size_t Index>
    value_type get() const
    {
        return (Index == 0) ? m_left : m_right;
    }

    difference_type length() const
    {
        return static_cast<int>(m_right) - static_cast<int>(m_left);
    }

private:
    unsigned m_left, m_right;
};

struct uint_intervals
    : public std::vector<uint_interval>
{
    uint_intervals()
    {}

    uint_intervals(unsigned left, unsigned right)
    {
        this->push_back(uint_interval(left, right));
    }

    uint_intervals & operator()(unsigned left, unsigned right)
    {
        this->push_back(uint_interval(left, right));
        return *this;
    }
};

std::ostream& operator<<(std::ostream& os, uint_interval const& interval)
{
    os << "[" << interval.get<0>() << ", " << interval.get<1>() << "]";
    return os;
}

template <typename RangeOfIntervals>
inline void test_one(std::string const& case_id,
                     RangeOfIntervals const& intervals,
                     typename boost::range_value
                         <
                             RangeOfIntervals
                         >::type::difference_type expected_gap)
{
    typedef typename boost::range_value<RangeOfIntervals>::type interval_type;
    typedef typename interval_type::difference_type gap_type;

    gap_type gap = bg::maximum_gap(intervals);

    std::ostringstream stream;
    for (typename boost::range_const_iterator<RangeOfIntervals>::type
            it = boost::const_begin(intervals);
            it != boost::const_end(intervals);
            ++it)
    {
        stream << " " << *it;
    }

#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << "intervals:" << stream.str() << std::endl;
    std::cout << "gap found? " << ((gap > 0) ? "yes" : "no") << std::endl;
    std::cout << "max gap length: " << gap << std::endl;
    std::cout << std::endl << std::endl;
#endif

    BOOST_CHECK_MESSAGE(gap == expected_gap,
                        case_id << "; intervals: "
                        << stream.str()
                        << "; expected: " << expected_gap
                        << ", detected: " << gap);
}

BOOST_AUTO_TEST_CASE( test_maximum_gap )
{
    uint_intervals intervals;

    intervals = uint_intervals(3,4)(1,10)(5,11)(20,35)(12,14)(36,40)(39,41)(35,36)(37,37)(50,50)(50,51);
    test_one("case_01", intervals, 9);

    intervals = uint_intervals(3,4)(1,10)(5,11)(20,35)(52,60)(12,14)(36,40)(39,41)(35,36)(37,37)(55,56);
    test_one("case_02", intervals, 11);

    intervals = uint_intervals(3,4);
    test_one("case_03", intervals, 0);

    intervals = uint_intervals(3,4)(15,15);
    test_one("case_04", intervals, 11);

    intervals = uint_intervals(3,14)(5,5)(5,6);
    test_one("case_05", intervals, 0);

    intervals = uint_intervals(3,10)(15,15)(15,18)(15,16);
    test_one("case_06", intervals, 5);

    intervals = uint_intervals(38,41)(3,10)(15,15)(15,18)(15,16)(20,30)(22,30)(23,30);
    test_one("case_07", intervals, 8);
}


///////////////////////////////////////////////////////////////////////////////
// statistics::survival::data::random::include.hpp                           //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_SURVIVAL_DATA_RANDOM_INCLUDE_HPP_ER_2009
#define BOOST_STATISTICS_SURVIVAL_DATA_RANDOM_INCLUDE_HPP_ER_2009

#include <boost/statistics/survival/data/random/batch.hpp>
#include <boost/statistics/survival/data/random/default_batch.hpp>
#include <boost/statistics/survival/data/random/failure_time.hpp>
#include <boost/statistics/survival/data/random/simulate_batches.hpp>

#endif


// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

// Use, modification and distribution is 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)

#include <geometry_test_common.hpp>

#include <boost/geometry/strategies/cartesian/distance_projected_point.hpp>
#include <boost/geometry/strategies/concepts/distance_concept.hpp>

#include <boost/geometry/io/wkt/read.hpp>

#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/adapted/c_array.hpp>
#include <boost/geometry/geometries/adapted/boost_tuple.hpp>
#include <test_common/test_point.hpp>

#ifdef HAVE_TTMATH
#  include <boost/geometry/extensions/contrib/ttmath_stub.hpp>
#endif

BOOST_GEOMETRY_REGISTER_C_ARRAY_CS(cs::cartesian)
BOOST_GEOMETRY_REGISTER_BOOST_TUPLE_CS(cs::cartesian)

template <typename P, typename PS, typename CalculationType>
void test_services()
{
    PS p1, p2;
    bg::assign_values(p1, 0, 0);
    bg::assign_values(p2, 0, 4);

    P p;
    bg::assign_values(p, 2, 0);

    CalculationType const sqr_expected = 4;
    CalculationType const expected = 2;

    namespace bgsd = bg::strategy::distance;
    namespace services = bg::strategy::distance::services;
    // 1: normal, calculate distance:

    typedef bgsd::projected_point<P, PS, CalculationType> strategy_type;

    BOOST_CONCEPT_ASSERT( (bg::concept::PointSegmentDistanceStrategy<strategy_type>) );

    typedef typename services::return_type<strategy_type>::type return_type;

    strategy_type strategy;
    return_type result = strategy.apply(p, p1, p2);
    BOOST_CHECK_CLOSE(result, return_type(expected), 0.001);

    // 2: "similar" to construct a similar strategy (similar but with other template-parameters) for, e.g., the reverse P2/P1
    // 2a: similar_type:
    typedef typename services::similar_type<strategy_type, P, PS>::type similar_type;
    // 2b: get_similar
    similar_type similar = services::get_similar<strategy_type, P, PS>::apply(strategy);

    result = similar.apply(p, p1, p2);
    BOOST_CHECK_CLOSE(result, return_type(expected), 0.001);

    // 3: "comparable" to construct a "comparable strategy" for P1/P2
    //    a "comparable strategy" is a strategy which does not calculate the exact distance, but
    //    which returns results which can be mutually compared (e.g. avoid sqrt)

    // 3a: "comparable_type"
    typedef typename services::comparable_type<strategy_type>::type comparable_type;

    // 3b: "get_comparable"
    comparable_type comparable = bgsd::services::get_comparable<strategy_type>::apply(strategy);

    return_type c_result = comparable.apply(p, p1, p2);
    BOOST_CHECK_CLOSE(c_result, return_type(sqr_expected), 0.001);
}

template <typename P1, typename P2, typename T>
void test_all_2d(std::string const& wkt_p,
                 std::string const& wkt_sp1,
                 std::string const& wkt_sp2,
                 T expected_distance)
{
    P1 p;
    P2 sp1, sp2;
    bg::read_wkt(wkt_p, p);
    bg::read_wkt(wkt_sp1, sp1);
    bg::read_wkt(wkt_sp2, sp2);

    {
        typedef bg::strategy::distance::projected_point
            <
                P1,
                P2
            > strategy_type;

        BOOST_CONCEPT_ASSERT
            (
                (bg::concept::PointSegmentDistanceStrategy<strategy_type>)
            );

        strategy_type strategy;
        typedef typename bg::strategy::distance::services::return_type<strategy_type>::type return_type;
        return_type d = strategy.apply(p, sp1, sp2);
        BOOST_CHECK_CLOSE(d, expected_distance, 0.001);
    }

    // Test combination with the comparable strategy
    {
        typedef bg::strategy::distance::projected_point
            <
                P1,
                P2,
                void,
                bg::strategy::distance::comparable::pythagoras<P1, P2>
            > strategy_type;
        strategy_type strategy;
        typedef typename bg::strategy::distance::services::return_type<strategy_type>::type return_type;
        return_type d = strategy.apply(p, sp1, sp2);
        T expected_squared_distance = expected_distance * expected_distance;
        BOOST_CHECK_CLOSE(d, expected_squared_distance, 0.01);
    }

}

template <typename P1, typename P2>
void test_all_2d()
{
    test_all_2d<P1, P2>("POINT(1 1)", "POINT(0 0)", "POINT(2 3)", 0.27735203958327);
    test_all_2d<P1, P2>("POINT(2 2)", "POINT(1 4)", "POINT(4 1)", 0.5 * sqrt(2.0));
    test_all_2d<P1, P2>("POINT(6 1)", "POINT(1 4)", "POINT(4 1)", 2.0);
    test_all_2d<P1, P2>("POINT(1 6)", "POINT(1 4)", "POINT(4 1)", 2.0);
}

template <typename P>
void test_all_2d()
{
    //test_all_2d<P, int[2]>();
    //test_all_2d<P, float[2]>();
    //test_all_2d<P, double[2]>();
    //test_all_2d<P, test::test_point>();
    test_all_2d<P, bg::model::point<int, 2, bg::cs::cartesian> >();
    test_all_2d<P, bg::model::point<float, 2, bg::cs::cartesian> >();
    test_all_2d<P, bg::model::point<double, 2, bg::cs::cartesian> >();
    test_all_2d<P, bg::model::point<long double, 2, bg::cs::cartesian> >();
}

int test_main(int, char* [])
{
    test_all_2d<int[2]>();
    test_all_2d<float[2]>();
    test_all_2d<double[2]>();
    //test_all_2d<test::test_point>();

    test_all_2d<bg::model::point<int, 2, bg::cs::cartesian> >();
    test_all_2d<bg::model::point<float, 2, bg::cs::cartesian> >();
    test_all_2d<bg::model::point<double, 2, bg::cs::cartesian> >();

    test_services
        <
            bg::model::point<double, 2, bg::cs::cartesian>,
            bg::model::point<float, 2, bg::cs::cartesian>,
            long double
        >();

#if defined(HAVE_TTMATH)
    test_all_2d
        <
            bg::model::point<ttmath_big, 2, bg::cs::cartesian>,
            bg::model::point<ttmath_big, 2, bg::cs::cartesian>
        >();
#endif

    return 0;
}


#pragma once
#include <vector>
#include <Eigen/Core>
#include "../DerivedPtrHolder.hh"

namespace kt84 {

template <int N>
struct LaplaceIterative_VertexTraits {
    struct Data {
        bool is_fixed;
        Eigen::Matrix<double, N, 1> value;
        Eigen::Matrix<double, N, 1> laplacian;

        Data()
            : is_fixed()
            , value    (Eigen::Matrix<double, N, 1>::Zero())
            , laplacian(Eigen::Matrix<double, N, 1>::Zero())
        {}
    } laplaceIterative;
};

struct LaplaceIterative_HalfedgeTraits {
    double laplaceIterative_weight;
    LaplaceIterative_HalfedgeTraits()
        : laplaceIterative_weight(1)
    {}
};

template <class TMeshBase, class TMesh, int N>
struct LaplaceIterative : public DerivedPtrHolder<TMesh, LaplaceIterative<TMeshBase, TMesh, N>> {
    typedef Eigen::Matrix<double, N, 1> Value;

    void laplaceIterative_compute(int num_iter = 1, double damping  = 0.25) {          // 0: no damping, 1: no change
        TMesh* mesh = get_mesh();

        int nv = mesh->n_vertices();
        std::vector<Value> values_new(nv, Value::Zero());

        for (int iter = 0; iter < num_iter; ++iter) {

            for (int i = 0; i < nv; ++i) {
                auto v = mesh->vertex_handle(i);
                auto& vdata = mesh->data(v).laplaceIterative;
                if (vdata.is_fixed) continue;

                // weighted sum among one-ring vertices
                Value value_sum = Value::Zero();
                double weight_sum = 0;
                for (auto h = mesh->voh_iter(v); h.is_valid(); ++h) {
                    double weight = mesh->data(*h).laplaceIterative_weight;
                    value_sum  += weight * mesh->data(mesh->to_vertex_handle(*h)).laplaceIterative.value;
                    weight_sum += weight;
                }

                // store new value to a temporary array
                value_sum /= weight_sum;
                values_new[i] = vdata.laplacian + value_sum;
            }

            // copy new value to the mesh
            for (int i = 0; i < nv; ++i) {
                auto v = mesh->vertex_handle(i);
                auto& vdata = mesh->data(v).laplaceIterative;
                if (vdata.is_fixed) continue;

                vdata.value = (1 - damping) * values_new[i] + damping * vdata.value;
            }
        }
    }
    void laplceIterative_set_laplacian_from_value() {
        TMesh* mesh = get_mesh();

        for (auto v : mesh->vertices()) {
            auto& vdata = mesh->data(v).laplaceIterative;

            vdata.laplacian = vdata.value;

            double weight_sum = 0;
            for (auto h = mesh->voh_iter(v); h; ++h)
                weight_sum += mesh->data(h).laplaceIterative_weight;

            for (auto h = mesh->voh_iter(v); h; ++h) {
                auto w = mesh->to_vertex_handle(h);
                auto& wdata = mesh->data(w).laplaceIterative;
                double weight = mesh->data(h).laplaceIterative_weight / weight_sum;

                vdata.laplacian -= weight * wdata.value;
            }
        }
    }
private:
    TMesh* get_mesh() const { return DerivedPtrHolder<TMesh, LaplaceIterative<TMeshBase, TMesh, N>>::derived_ptr; }
};

}


//
// Created by sauron on 8/6/18.
//

#include "gtest/gtest.h"
#include "server/query.h"
#include "source/genkey.h"
#include "client/verify_subset.h"
#include <NTL/ZZ_p.h>
#include <NTL/ZZ.h>
#include <exception>
#define SETS_NUM 16
#define SIZE 10

class SubsetTest: public ::testing::Test{
protected:
    Key *k;
    DataStructure *dataStructure;
    Subset *subset;
    VerifySubset *verifySubset;
    void SetUp(int sets_no) {
        try {
            NTL::ZZ p = NTL::conv<NTL::ZZ>("16798108731015832284940804142231733909759579603404752749028378864165570215949");
            NTL::ZZ_p::init(p);
            k = new Key(p);
            dataStructure = new DataStructure(sets_no, k);
            for (int i = 1; i <= SIZE; i++) {
                NTL::ZZ_p j = NTL::random_ZZ_p();
                for (int set_index = 0; set_index < dataStructure->m; set_index++) {
                    dataStructure->insert(set_index, j, k->get_public_key(), k->get_secret_key());
                }
            }
            for (int set_index = 1; set_index < dataStructure->m; set_index++)
                for (int i = 1; i <= 9 * SIZE / 10; i++) {
                    NTL::ZZ_p j = NTL::random_ZZ_p();
                    dataStructure->insert(set_index, j, k->get_public_key(), k->get_secret_key());
                    dataStructure->insert(0, j, k->get_public_key(), k->get_secret_key());
                }

        }
        catch(std::exception& e){
            std::cerr<< e.what() << "\n";
        }

    }
    void TearDown() {
//        PUT(verifySubset->verified_subset);
//        delete (verifySubset);
//        delete (subset);
        delete (dataStructure);
        delete (k);
    }
};

TEST_F(SubsetTest, PositiveAnswer) {
    SetUp(SETS_NUM);
    Subset *subset = new Subset(0, 3, k->get_public_key(), dataStructure);
    subset->subset();
    if(subset->answer)
        subset->positiveWitness();
    else
        subset->negativeWitness();

    VerifySubset *verifySubset = new VerifySubset(k->get_public_key(), dataStructure, subset->Q, subset->W, subset->answer,
                                                  subset->index[0], subset->index[1], subset->y);
    verifySubset->verify_subset();
    EXPECT_TRUE(subset->answer);
    EXPECT_TRUE(verifySubset->verified_subset);
    delete verifySubset;
    delete subset;
}

TEST_F(SubsetTest, NegativeAnswer) {
    SetUp(SETS_NUM);
    Subset *subset = new Subset(2, 3, k->get_public_key(), dataStructure);
    subset->subset();
    if(subset->answer)
        subset->positiveWitness();
    else
        subset->negativeWitness();

    VerifySubset *verifySubset = new VerifySubset(k->get_public_key(), dataStructure, subset->Q, subset->W, subset->answer,
                                                  subset->index[0], subset->index[1], subset->y);
    verifySubset->verify_subset();
    EXPECT_FALSE(subset->answer);
    EXPECT_TRUE(verifySubset->verified_subset);
    delete verifySubset;
    delete subset;
}

TEST_F(SubsetTest, SameSets) {
    SetUp(SETS_NUM);
    Subset *subset = new Subset(2, 2, k->get_public_key(), dataStructure);
    subset->subset();
    if(subset->answer)
        subset->positiveWitness();
    else
        subset->negativeWitness();

    VerifySubset *verifySubset = new VerifySubset(k->get_public_key(), dataStructure, subset->Q, subset->W, subset->answer,
                                                  subset->index[0], subset->index[1], subset->y);
    verifySubset->verify_subset();
    EXPECT_TRUE(subset->answer);
    EXPECT_TRUE(verifySubset->verified_subset);
    delete verifySubset;
    delete subset;
}


/*
==============================================================================
KratosStructuralApplication
A library based on:
Kratos
A General Purpose Software for Multi-Physics Finite Element Analysis
Version 1.0 (Released on march 05, 2007).

Copyright 2007
Pooyan Dadvand, Riccardo Rossi, Janosch Stascheit, Felix Nagel
pooyan@cimne.upc.edu
rrossi@cimne.upc.edu
janosch.stascheit@rub.de
nagel@sd.rub.de
- CIMNE (International Center for Numerical Methods in Engineering),
Gran Capita' s/n, 08034 Barcelona, Spain
- Ruhr-University Bochum, Institute for Structural Mechanics, Germany

Permission is hereby granted, free  of charge, to any person obtaining
a  copy  of this  software  and  associated  documentation files  (the
"Software"), to  deal in  the Software without  restriction, including
without limitation  the rights to  use, copy, modify,  merge, publish,
distribute,  sublicense and/or  sell copies  of the  Software,  and to
permit persons to whom the Software  is furnished to do so, subject to
the following condition:

Distribution of this code for  any  commercial purpose  is permissible
ONLY BY DIRECT ARRANGEMENT WITH THE COPYRIGHT OWNERS.

The  above  copyright  notice  and  this permission  notice  shall  be
included in all copies or substantial portions of the Software.

THE  SOFTWARE IS  PROVIDED  "AS  IS", WITHOUT  WARRANTY  OF ANY  KIND,
EXPRESS OR  IMPLIED, INCLUDING  BUT NOT LIMITED  TO THE  WARRANTIES OF
MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
IN NO EVENT  SHALL THE AUTHORS OR COPYRIGHT HOLDERS  BE LIABLE FOR ANY
CLAIM, DAMAGES OR  OTHER LIABILITY, WHETHER IN AN  ACTION OF CONTRACT,
TORT  OR OTHERWISE, ARISING  FROM, OUT  OF OR  IN CONNECTION  WITH THE
SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.

==============================================================================
*/
/* **************************************************************************************
*
*   Last Modified by:    $Author: G.D.Huynh $
*   Date:                $Date: 25.042018 $
*   Revision:            $Revision: 1.0 $
*
* ***************************************************************************************/

// System includes
// External includes
#include <boost/timer.hpp>
// Project includes
#include "includes/define.h"
#include "custom_elements/linear_bending_strip.h"
#include "utilities/math_utils.h"
#include "utilities/openmp_utils.h"
#include "structural_application/custom_utilities/sd_math_utils.h"
#include "isogeometric_application/custom_utilities/isogeometric_math_utils.h"
#include "isogeometric_application/isogeometric_application.h"

#define ENABLE_PROFILING

#include <iostream>
using namespace std;

namespace Kratos
{
//***********************************************************************************
//***********************************************************************************
// -------- //
//  PUBLIC  //
// -------- //

// Constructor
LinearBendingStrip::LinearBendingStrip()
{
}

// Constructor
LinearBendingStrip::LinearBendingStrip( IndexType NewId, GeometryType::Pointer pGeometry )
    : Element( NewId, pGeometry )
{
    mpIsogeometricGeometry =
        boost::dynamic_pointer_cast<IsogeometricGeometryType>(pGetGeometry());
}

// Constructor
LinearBendingStrip::LinearBendingStrip( IndexType NewId, GeometryType::Pointer pGeometry,
                          PropertiesType::Pointer pProperties )
    : Element( NewId, pGeometry, pProperties )
{
    mpIsogeometricGeometry =
        boost::dynamic_pointer_cast<IsogeometricGeometryType>(pGetGeometry());
}

//***********************************************************************************
//***********************************************************************************
Element::Pointer LinearBendingStrip::Create( IndexType NewId,
                                        NodesArrayType const& ThisNodes,
                                        PropertiesType::Pointer pProperties ) const
{
    return Element::Pointer( new LinearBendingStrip( NewId, GetGeometry().Create( ThisNodes ), pProperties ) );
}

//***********************************************************************************
//***********************************************************************************
Element::Pointer LinearBendingStrip::Create( IndexType NewId,
    GeometryType::Pointer pGeom,
    PropertiesType::Pointer pProperties ) const
{
return Element::Pointer( new LinearBendingStrip( NewId, pGeom, pProperties ) );
}

//***********************************************************************************
//***********************************************************************************
// Destructor
LinearBendingStrip::~LinearBendingStrip()
{
}

//***********************************************************************************
//***********************************************************************************
void LinearBendingStrip::EquationIdVector( EquationIdVectorType& rResult,
                                    const ProcessInfo& rCurrentProcessInfo ) const
{
    KRATOS_TRY
    unsigned int number_of_nodes = GetGeometry().size();
    unsigned int dim = number_of_nodes * 3;

    if ( rResult.size() != dim )
        rResult.resize( dim );

    for ( unsigned int i = 0; i < number_of_nodes; i++ )
    {
        int index = i * 3;
        rResult[index]   = GetGeometry()[i].GetDof( DISPLACEMENT_X ).EquationId();
        rResult[index+1] = GetGeometry()[i].GetDof( DISPLACEMENT_Y ).EquationId();
        rResult[index+2] = GetGeometry()[i].GetDof( DISPLACEMENT_Z ).EquationId();
    }

    //KRATOS_WATCH(rResult)

    KRATOS_CATCH( "" )
}

//***********************************************************************************
//***********************************************************************************
void LinearBendingStrip::GetDofList( DofsVectorType& ElementalDofList,
                              const ProcessInfo& rCurrentProcessInfo ) const
{
    ElementalDofList.resize( 0 );

    for ( unsigned int i = 0; i < GetGeometry().size(); i++ )
    {
        ElementalDofList.push_back( GetGeometry()[i].pGetDof( DISPLACEMENT_X ) );
        ElementalDofList.push_back( GetGeometry()[i].pGetDof( DISPLACEMENT_Y ) );
        ElementalDofList.push_back( GetGeometry()[i].pGetDof( DISPLACEMENT_Z ) );
    }
}

//***********************************************************************************
//***********************************************************************************
void LinearBendingStrip::Initialize(const ProcessInfo& rCurrentProcessInfo)
{
    KRATOS_TRY

        ////////////////////////////////////////////////////////////////
        ////////////////////////////////////////////////////////////////
        // try to read the extraction operator from the elemental data
        Matrix ExtractionOperator;
        bool manual_initilization = false;
        if( this->Has( EXTRACTION_OPERATOR ) )
        {
            ExtractionOperator = this->GetValue( EXTRACTION_OPERATOR );
            manual_initilization = true;
        }
        else if( this->Has( EXTRACTION_OPERATOR_MCSR ) )
        {
            Matrix Temp = this->GetValue( EXTRACTION_OPERATOR_MCSR );

            // make a simple check
            if(Temp.size1() != 2)
                KRATOS_THROW_ERROR(std::logic_error, "Invalid MCSR matrix for extraction operator found at element", this->Id())

            // choose the best storage scheme based ratio between number of nonzeros and the full size of the matrix
            unsigned int size_ex_n = (unsigned int)(Temp(0, 0) - 1);
            unsigned int size_ex_nz = Temp.size2() - 1;
            if( ( (double)(size_ex_nz) ) / (size_ex_n * size_ex_n) < 0.2 )
                ExtractionOperator = IsogeometricMathUtils::MCSR2CSR(Temp);
            else
                ExtractionOperator = IsogeometricMathUtils::MCSR2MAT(Temp);

            manual_initilization = true;
        }
        else if( this->Has( EXTRACTION_OPERATOR_CSR_ROWPTR )
             && this->Has( EXTRACTION_OPERATOR_CSR_COLIND )
             && this->Has( EXTRACTION_OPERATOR_CSR_VALUES ) )
        {
            Vector rowPtr = this->GetValue( EXTRACTION_OPERATOR_CSR_ROWPTR ); // must be 0-base
            Vector colInd = this->GetValue( EXTRACTION_OPERATOR_CSR_COLIND ); // must be 0-base
            Vector values = this->GetValue( EXTRACTION_OPERATOR_CSR_VALUES );

            ExtractionOperator = IsogeometricMathUtils::Triplet2CSR(rowPtr, colInd, values);
            //            ExtractionOperator = IsogeometricMathUtils::Triplet2CSR(m, n, rowPtr, colInd, values);

            manual_initilization = true;
        }
        //        else
        //            KRATOS_THROW_ERROR(std::logic_error, "The extraction operator was not given for element", Id())

        // initialize the geometry
        if(manual_initilization)
            mpIsogeometricGeometry->AssignGeometryData(
                this->GetValue(NURBS_KNOTS_1),
                this->GetValue(NURBS_KNOTS_2),
                this->GetValue(NURBS_KNOTS_3),
                this->GetValue(NURBS_WEIGHTS),
                ExtractionOperator,
                this->GetValue(NURBS_DEGREE_1),
                this->GetValue(NURBS_DEGREE_2),
                this->GetValue(NURBS_DEGREE_3),
                2 // only need to compute 2 integration rules
            );
        //////////////////////////////////////////////////////////////////////////////////////////////////////////////
        //////////////////////////////////////////////////////////////////////////////////////////////////////////////
        mThisIntegrationMethod = GeometryData::GI_GAUSS_2;

        mDim = 3;
        mNumberOfNodes = mpIsogeometricGeometry->size();
        mStrainSize = 3;
        mNumberOfDof = mNumberOfNodes*mDim;
        mThickness = GetProperties()[THICKNESS];

        // material parameters
        mE = GetProperties()[YOUNG_MODULUS];
        mNU = GetProperties()[POISSON_RATIO];
        mLambda = mE*mNU /(1.0 +mNU)/(1.0 - 2.0*mNU);
        mMu = 0.5*mE/(1.0 + mNU);

        ////////////////////////////////////////////////////////////////
        // get nodal coordinates vector R in undeformed configuration //
        if(mNodalCoordinates.size() != mNumberOfNodes)
            mNodalCoordinates.resize(mNumberOfNodes);

        for(unsigned int I=0; I< mNumberOfNodes; ++I)
            mNodalCoordinates[I].resize(mDim);

        for (unsigned int I=0; I< mNumberOfNodes; ++I)
        {
            mNodalCoordinates[I](0) = (*mpIsogeometricGeometry)[I].X0();
            mNodalCoordinates[I](1) = (*mpIsogeometricGeometry)[I].Y0();
            mNodalCoordinates[I](2) = (*mpIsogeometricGeometry)[I].Z0();
        }

        #ifdef ENABLE_BEZIER_GEOMETRY
        //initialize the geometry
        mpIsogeometricGeometry->Initialize(mThisIntegrationMethod);
        #endif

        //Initialization of the constitutive law vector and
        // declaration, definition and initialization of the material
        // law was at each integration point
        const GeometryType::IntegrationPointsArrayType& integration_points =
        mpIsogeometricGeometry->IntegrationPoints(mThisIntegrationMethod);

        mNumberOfIntegrationPoint = integration_points.size();

        //////////////////////////////////////////////////////////////
        ///// compute  Jacobian ,Inverse Jacobian, Det Jacobian /////
        mInvJ0.resize(integration_points.size());
        mN.resize(integration_points.size());
        mDN_De.resize(integration_points.size());

        for (unsigned int i = 0; i < integration_points.size(); ++i)
        {
            mInvJ0[i].resize(mDim, mDim, false);
            noalias(mInvJ0[i]) = ZeroMatrix(mDim, mDim);

            mN[i].resize(mNumberOfNodes);
            noalias(mN[i]) = ZeroVector(mNumberOfNodes);

            mDN_De[i].resize(mNumberOfNodes, 2);
            noalias(mDN_De[i]) = ZeroMatrix(mNumberOfNodes,2);

        }

        mDetJ0.resize(integration_points.size(), false);
        // TODO remove the storage for Jacobian to save memory
        noalias(mDetJ0) = ZeroVector(integration_points.size());

        mIntegrationWeight.resize(integration_points.size());

        // calculate the Jacobian
        mJ0.resize(integration_points.size());
        mJ0 = mpIsogeometricGeometry->Jacobian0(mJ0, mThisIntegrationMethod);
        double DetJ_temp;
        mTotalDomainInitialSize = 0.0;

        for(unsigned int PointNumber = 0; PointNumber < integration_points.size(); ++ PointNumber)
        {
            mN[PointNumber] = mpIsogeometricGeometry->ShapeFunctionsValues( mN[PointNumber] , integration_points[PointNumber]);
            mDN_De[PointNumber] = mpIsogeometricGeometry->ShapeFunctionsLocalGradients( mDN_De[PointNumber], integration_points[PointNumber]);

            MathUtils<double>::InvertMatrix(mJ0[PointNumber], mInvJ0[PointNumber],  DetJ_temp);

            Matrix JtJ = prod(trans(mJ0[PointNumber]), mJ0[PointNumber]);
            mDetJ0[PointNumber] = sqrt(MathUtils<double>::Det(JtJ));

            //getting informations for integration
            mIntegrationWeight[PointNumber] = integration_points[PointNumber].Weight();

            mTotalDomainInitialSize += mDetJ0[PointNumber]* mIntegrationWeight[PointNumber];

        }

        mIsInitialized = true;

        #ifdef ENABLE_BEZIER_GEOMETRY
        // clean the geometry internal data
        mpIsogeometricGeometry->Clean();
        #endif

    KRATOS_CATCH("")
}

//***********************************************************************************
//***********************************************************************************
void LinearBendingStrip::CalculateLocalSystem( MatrixType& rLeftHandSideMatrix,
                                                VectorType& rRightHandSideVector,
                                                const ProcessInfo& rCurrentProcessInfo )
{
    //calculation flags
    bool CalculateStiffnessMatrixFlag = true;
    bool CalculateResidualVectorFlag = true;

    CalculateAll( rLeftHandSideMatrix, rRightHandSideVector, rCurrentProcessInfo,
                  CalculateStiffnessMatrixFlag, CalculateResidualVectorFlag );
}

//***********************************************************************************
//***********************************************************************************
void LinearBendingStrip::CalculateAll( MatrixType& rLeftHandSideMatrix,
                                VectorType& rRightHandSideVector,
                                const ProcessInfo& rCurrentProcessInfo,
                                bool CalculateStiffnessMatrixFlag,
                                bool CalculateResidualVectorFlag )
{
    KRATOS_TRY

    if (CalculateStiffnessMatrixFlag==true)
    {
        if (rLeftHandSideMatrix.size1() != mNumberOfDof)
            rLeftHandSideMatrix.resize(mNumberOfDof, mNumberOfDof);
        noalias(rLeftHandSideMatrix) = ZeroMatrix(mNumberOfDof, mNumberOfDof);
    }

    if (CalculateResidualVectorFlag==true)
    {
        if (rRightHandSideVector.size() != mNumberOfDof)
            rRightHandSideVector.resize(mNumberOfDof);
        noalias(rRightHandSideVector) = ZeroVector(mNumberOfDof);
    }

    ////////////////////////////////////////////////////////////////////////////////
    ////// compute residual vector and stiffness matrix over integration points/////

    #ifdef ENABLE_BEZIER_GEOMETRY
    //initialize the geometry
    mpIsogeometricGeometry->Initialize(mThisIntegrationMethod);
    #endif

    // get integration points
    const GeometryType::IntegrationPointsArrayType& integration_points =
        mpIsogeometricGeometry->IntegrationPoints(mThisIntegrationMethod);

    // Current displacements
    Matrix CurrentDisplacement(mNumberOfNodes, mDim);
    for(unsigned int node =0; node < mpIsogeometricGeometry->size() ;++node)
        noalias(row(CurrentDisplacement, node)) = (*mpIsogeometricGeometry)[node].GetSolutionStepValue(DISPLACEMENT);

    Vector NodalDisplacement = ZeroVector(mNumberOfDof);
    for(unsigned int I=0; I<mNumberOfNodes; I++)
    {
        for(unsigned int i=0; i<mDim; i++)
        {
            NodalDisplacement(I*mDim +i) = CurrentDisplacement(I,i);
        }
    }

    ///////////////////////////////////////////////////////////////////////////////////////
    //////////////////////////////////////// loop over integration points
    for(unsigned int PointNumber=0; PointNumber < integration_points.size(); ++PointNumber)
    {

            //////////// get shape function values and their derivatives
            ShapeFunctionsSecondDerivativesType D2N_De2;
            D2N_De2 = mpIsogeometricGeometry->ShapeFunctionsSecondDerivatives(D2N_De2, integration_points[PointNumber]);

            /////// i. covariant base vectors
            std::vector<Vector> A;  // covarianti base vector of undeformed configuration
            CovariantBaseVector( A, mDN_De[PointNumber], mNodalCoordinates);

            ////i. normal vector
            Vector A3Vector, AA3Vector; double A3;
            ReferenceNormalDirector( A3Vector,  AA3Vector, A3, A);

            ////ii. derivative of covariant base vectors
            boost::numeric::ublas::vector<boost::numeric::ublas::vector<Vector> > A_ab;
            DerivativeCovariantBaseVector( A_ab, D2N_De2, mNodalCoordinates);

            // metric and curvature coefficents
            Matrix Aab(2,2);
            CovariantMetricCoefficient( Aab, A);

            double detJA = sqrt(MathUtils<double>::Det(Aab));

            // contravariant base vectors
            std::vector<Vector> AA;
            ContravariantBaseVector( AA, A, Aab);

            /////////////////////////////////////////////
            ///// local Cartesian basis /////////////////
            std::vector<Vector> EE;
            LocalCartesianBasisVector(EE,  A);

            // first derivative of curvature change
            boost::numeric::ublas::vector<boost::numeric::ublas::vector<Vector> > kLocalVector_r;
            FirstDerivativeLocalCurvatureChange_r(kLocalVector_r, A, A3, A3Vector, mDN_De[PointNumber], D2N_De2,A_ab, EE, AA);

            // bending B matrix
            Matrix BBb(mStrainSize, mNumberOfDof);
            CreatingBmatrix( BBb, kLocalVector_r);

            // material matrix
            Matrix D0 = ZeroMatrix(mStrainSize,mStrainSize);
            CalculateConstitutiveMatrix(D0);

            //////////////////////////////////
            // compute the contribution to LHS
            Matrix GaussStiffnessMatrix = ZeroMatrix(mNumberOfDof,mNumberOfDof);
            AddStiffnessMatrixComponents(GaussStiffnessMatrix, D0, BBb, BBb,  pow(mThickness,3.0)/12.0*detJA , mIntegrationWeight[PointNumber]);
            noalias(rLeftHandSideMatrix) += GaussStiffnessMatrix;

            //////////////////////////////////
            // compute the contribution to RHS
            noalias(rRightHandSideVector) -= prod(GaussStiffnessMatrix, NodalDisplacement);

    }// loop over integration points

    //KRATOS_WATCH(rRightHandSideVector)

    #ifdef ENABLE_BEZIER_GEOMETRY
    // clean the geometry internal data
    mpIsogeometricGeometry->Clean();
    #endif

    KRATOS_CATCH( "" )
}

    ///////////////////////////////////////// all components of residual vectors and stiffness matrices ///////////
    ///////////////////// add left hand side contribution
    void LinearBendingStrip::AddInternalForces(VectorType& RightHandSideVector, const Vector& StressResultants,
        boost::numeric::ublas::vector<boost::numeric::ublas::vector<Vector> >& StrainVector_r, const double& DetJ, const double& Weight)
    {
       noalias(RightHandSideVector) -= StressResultants(0)*StrainVector_r[0][0]*DetJ*Weight;
       noalias(RightHandSideVector) -= StressResultants(1)*StrainVector_r[1][1]*DetJ*Weight;
       noalias(RightHandSideVector) -= StressResultants(2)*StrainVector_r[0][1]*DetJ*Weight;
    }

    void LinearBendingStrip::AddExternalForces(VectorType& RightHandSideVector, const Vector& N
        , const double& DetJ, const double& Weight )
    {
        const Vector BodyForce = GetProperties()[BODY_FORCE];

        for(unsigned int I=0; I < mNumberOfNodes; ++I)
            for(unsigned int i=0; i<mDim; ++i)
                RightHandSideVector(I*mDim + i) += N(I)*BodyForce(i)*DetJ*Weight;
    }

    void LinearBendingStrip::AddStiffnessMatrixComponents(MatrixType& LeftHandSideMatrix, const Matrix& Di, const Matrix& BlhsMatrix
        , const Matrix& BrhsMatrix, const double& DetJ, const double& Weight)
    {
        noalias(LeftHandSideMatrix) += prod( trans(BlhsMatrix), Matrix(prod(Di, BrhsMatrix)) )*DetJ*Weight;
    }

    /////////////////////////////////////////////////////////////////////////
    /////////////////////////// base vectors and their derivatives///////////
    /////////////////////////////////////////////////////////////////////////
    void LinearBendingStrip::CovariantBaseVector(std::vector<Vector>& A, const Matrix& DN_De
                                                                            , const std::vector<Vector>& X)
    {
        A.resize(2);

        for(unsigned int i=0; i< 2; ++i)
        {
            A[i].resize(mDim);
            A[i] = ZeroVector(mDim);
        }

        for(unsigned alpha=0; alpha < 2; ++alpha)
        {
            for(unsigned int i=0; i<mDim; ++i)
            {
                for(unsigned int I=0; I<mNumberOfNodes; ++I)
                {
                    // compute a1, a2
                    A[alpha](i) += DN_De(I,alpha)*X[I](i) ;
                }
            }
        }

    }

    void LinearBendingStrip::ReferenceNormalDirector(Vector& A3Vector, Vector& AA3Vector, double& A3,  std::vector<Vector>& A)
    {
        AA3Vector = MathUtils<double>::CrossProduct(A[0], A[1]);
        A3 = MathUtils<double>::Norm3(AA3Vector);
        A3Vector = AA3Vector/A3;
    }

    void LinearBendingStrip::ContravariantBaseVector(std::vector<Vector>& AA, std::vector<Vector>& A, Matrix& Aab)
    {
        AA.resize(2);
        for(unsigned i=0; i<2;++i)
        {
            AA[i].resize(3);
            AA[i] = ZeroVector(3);
        }

        double temp_ab;
        Matrix AAab(2,2);
        MathUtils<double>::InvertMatrix(Aab, AAab, temp_ab);

        noalias(AA[0]) += AAab(0,0)*A[0] ;
        noalias(AA[0]) += AAab(0,1)*A[1] ;

        noalias(AA[1]) += AAab(1,0)*A[0] ;
        noalias(AA[1]) += AAab(1,1)*A[1] ;
    }

    void LinearBendingStrip::DerivativeCovariantBaseVector(boost::numeric::ublas::vector<boost::numeric::ublas::vector<Vector> >& A_ab,
        const ShapeFunctionsSecondDerivativesType& D2N_De2, const std::vector<Vector>& X)
    {
        A_ab.resize(2);

        for(unsigned int i=0; i< 2; ++i)
        {
            A_ab[i].resize(2);

            for(unsigned int j=0; j<2; ++j)
            {
                    A_ab[i][j].resize(mDim);
                    A_ab[i][j] = ZeroVector(mDim);
            }
        }

        for(unsigned alpha=0; alpha<2; ++alpha)
        {
            for(unsigned beta=0; beta<2; ++beta)
            {
                for(unsigned int i=0; i< mDim; ++i)
                {
                    for(unsigned int I=0; I< mNumberOfNodes; ++I)
                    {
                        // compute a_11
                        A_ab[alpha][beta](i) += D2N_De2[I](alpha,beta)*X[I](i) ;
                    }
                }
            }
        }

    }

    ///////////////////////////////////////////////////////////////////
    ////////// shell fundamental properties ///////////////////////////
    ///////////////////////////////////////////////////////////////////
    void LinearBendingStrip::CovariantMetricCoefficient(Matrix& Aab, std::vector<Vector>& A)
    {
        if(Aab.size1()!=2)
            Aab.resize(2,2);

        for(unsigned int alpha=0; alpha<2; ++alpha)
        {
            for(unsigned int beta=0; beta<2; ++beta)
            {
                Aab(alpha,beta)= MathUtils<double>::Dot3(A[alpha], A[beta]);
            }
        }
    }

    ///////////////////////////////////////////////////////////////////////////
    ///// first derivative of curvature changes and necessary components //////
    ///////////////////////////////////////////////////////////////////////////
    void LinearBendingStrip::FirstDerivativeLocalCurvatureChange_r(boost::numeric::ublas::vector<boost::numeric::ublas::vector<Vector> >& kLocalVector_r
        , std::vector<Vector>& A, double& A3, Vector& A3Vector, const Matrix& DN_De
        , const ShapeFunctionsSecondDerivativesType& D2N_De2, boost::numeric::ublas::vector<boost::numeric::ublas::vector<Vector>>& A_ab
        ,std::vector<Vector>& EE, std::vector<Vector>& AA)
    {
        if (kLocalVector_r.size() != 2)
            kLocalVector_r.resize(2);

        for(unsigned int i=0; i< 2; ++i)
        {
            if(kLocalVector_r[i].size() !=2)
                kLocalVector_r[i].resize(2);

            for(unsigned int j=0; j<2; ++j)
            {
                if (kLocalVector_r[i][j].size() != mNumberOfDof)
                {
                    kLocalVector_r[i][j].resize(mNumberOfDof);
                    kLocalVector_r[i][j] = ZeroVector(mNumberOfDof);
                }
            }
        }

        for(unsigned int gamma=0; gamma<2;++gamma)
        {
            for(unsigned int delta=0; delta<2; ++delta)
            {
                Vector temp = ZeroVector(mNumberOfDof);

                for(unsigned int alpha=0; alpha<2;++alpha)
                {
                    for(unsigned int beta=0; beta<2; ++beta)
                    {
                        for(unsigned int I=0; I< mNumberOfNodes; ++I)
                        {
                            for(unsigned int i=0; i< mDim; ++i)
                            {
                                Vector A_abxA2(mDim);
                                A_abxA2 = MathUtils<double>::CrossProduct(A_ab[alpha][beta], A[1]);

                                Vector A1xA_ab(mDim);
                                A1xA_ab = MathUtils<double>::CrossProduct(A[0], A_ab[alpha][beta]);

                                Vector A2xA3(mDim);
                                A2xA3 = MathUtils<double>::CrossProduct(A[1], A3Vector);

                                Vector A3xA1(mDim);
                                A3xA1 = MathUtils<double>::CrossProduct(A3Vector, A[0]);

                                temp(I*mDim + i) +=  (- A3Vector(i)*D2N_De2[I](alpha,beta) + (1/A3)*( A_abxA2(i)*DN_De(I,0)
                                           + A1xA_ab(i)*DN_De(I,1) + MathUtils<double>::Dot3(A_ab[alpha][beta], A3Vector)*
                                             ( A2xA3(i)*DN_De(I,0) + A3xA1(i)*DN_De(I,1) ) ) )*
                                             MathUtils<double>::Dot3(EE[gamma],AA[alpha])*MathUtils<double>::Dot3(AA[beta],EE[delta]);
                            }
                        }
                    }
                }

                noalias(kLocalVector_r[gamma][delta])= temp;
            }
        }

        kLocalVector_r[0][1] *= 2.0;
        kLocalVector_r[1][0] *= 2.0;
    }

    //////////////////////////////////////////////////////////////
    //////////// addtional utilities /////////////////////////////
    //////////////////////////////////////////////////////////////
    void LinearBendingStrip::LocalCartesianBasisVector(std::vector<Vector>& EE, std::vector<Vector>& A)
    {
        EE.resize(3);
        for(unsigned int i=0; i<3; ++i)
        {
            EE[i].resize(3);
        }

        EE[0] = A[0]/MathUtils<double>::Norm3(A[0]);

        Vector EE2_temp = ZeroVector(3);
        noalias(EE2_temp) +=  A[1];
        noalias(EE2_temp) -=  MathUtils<double>::Dot3(A[1], EE[0])*EE[0];
        noalias(EE[1]) = ( EE2_temp )/MathUtils<double>::Norm3(EE2_temp);

        EE[2] = MathUtils<double>::CrossProduct(EE[0], EE[1]);

    }

    void LinearBendingStrip::UnitBaseVectors(std::vector<Vector>& e)
    {
        if (e.size() != 3)
            e.resize(3);

        e[0]=e[1]=e[2]=ZeroVector(mDim);
        e[0](0)=e[1](1)=e[2](2)= 1.0;
    }

    void LinearBendingStrip::CreatingBmatrix(Matrix& BMatrix, const boost::numeric::ublas::vector<boost::numeric::ublas::vector<Vector> >& LocalStrainVector_r)
    {
        if(BMatrix.size1() != mStrainSize)
            BMatrix.resize(mStrainSize, mNumberOfDof);
        noalias(BMatrix) = ZeroMatrix(mStrainSize, mNumberOfDof);

        for (int i=0; i< mNumberOfDof; i++)
        {
            BMatrix(0,i) = LocalStrainVector_r[0][0](i);
            BMatrix(1,i) = LocalStrainVector_r[1][1](i);
            BMatrix(2,i) = LocalStrainVector_r[0][1](i);
        }

    }

    ///////////// material matrix
    void LinearBendingStrip::CalculateConstitutiveMatrix(Matrix& D)
    {
        if (D.size1() != 3)
            D.resize(3,3);
        D = ZeroMatrix(3,3);

        D(1,1) = pow(10.0 ,4.0)*mE;
    }

} // Namespace Kratos.


#include "CalibTracker/SiStripLorentzAngle/interface/LA_Filler_Fitter.h"
#include "FWCore/Utilities/interface/EDMException.h"

#include <cmath>
#include <boost/foreach.hpp>
#include <boost/regex.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/algorithm/string/erase.hpp>
#include <TF1.h>
#include <TGraphErrors.h>
#include <TProfile.h>

LA_Filler_Fitter::Result LA_Filler_Fitter::
result(Method m, const std::string name, const Book& book) {
  Result p;
  const std::string base = boost::erase_all_copy(name,method(m));

  const TH1* const h = book[name];
  const TH1* const reco = book[base+"_reconstruction"];
  const TH1* const field = book[base+"_field"];

  if(reco) p.reco = std::make_pair<float,float>( reco->GetMean(), reco->GetMeanError());
  if(field) p.field = field->GetMean();
  if(h) { switch(m) {
    case WIDTH: {
      const TF1*const f = h->GetFunction("LA_profile_fit"); if(!f) break;
      p.measured = std::make_pair<float,float>(f->GetParameter(0),f->GetParError(0));
      p.chi2 = f->GetChisquare();
      p.ndof = f->GetNDF();
      p.entries = (unsigned)(h->GetEntries());
      break;
    }
    case PROB1: case AVGV2: case AVGV3: case RMSV2: case RMSV3: /*case MULTI:*/ {
      const TF1*const f = h->GetFunction("SymmetryFit"); if(!f) break;
      p.measured = std::make_pair<float,float>( p.reco.first + f->GetParameter(0), f->GetParameter(1) );
      p.chi2 = f->GetParameter(2);
      p.ndof = (unsigned) (f->GetParameter(3));
      p.entries =
	(m&PROB1)         ? (unsigned) book[base+"_w1"]->GetEntries() :
	(m&(AVGV2|RMSV2)) ? (unsigned) book[base+method(AVGV2,0)]->GetEntries() :
	(m&(AVGV3|RMSV3)) ? (unsigned) book[base+method(AVGV3,0)]->GetEntries() : 0 ;
      break;
    }
    default: break;
    }
  }
  return p;
}

std::map<uint32_t,LA_Filler_Fitter::Result> LA_Filler_Fitter::
module_results( const Book& book, const Method m) {
  std::map<uint32_t,Result> results;
  for(Book::const_iterator it = book.begin(".*_module\\d*"+method(m)); it!=book.end(); ++it ) {
    const uint32_t detid = boost::lexical_cast<uint32_t>( boost::regex_replace( it->first,
										boost::regex(".*_module(\\d*)_.*"),
										std::string("\\1")));
    results[detid] = result(m,it->first,book);
  }
  return results;
}

std::map<std::string,LA_Filler_Fitter::Result> LA_Filler_Fitter::
layer_results( const Book& book, const Method m) {
  std::map<std::string,Result> results;
  for(Book::const_iterator it = book.begin(".*layer\\d.*"+method(m)); it!=book.end(); ++it ) {
    const std::string name = boost::erase_all_copy(it->first,method(m));
    results[name] = result(m,it->first,book);
  }
  return results;
}

std::map<std::string, std::vector<LA_Filler_Fitter::Result> > LA_Filler_Fitter::
ensemble_results( const Book& book, const Method m) {
  std::map<std::string, std::vector<Result> > results;
  for(Book::const_iterator it = book.begin(".*_sample.*"+method(m)); it!=book.end(); ++it ) {
    const std::string name = boost::regex_replace(it->first,boost::regex("sample\\d*_"),"");
    results[name].push_back(result(m,it->first,book));
  }
  return results;
}

void LA_Filler_Fitter::
summarize_ensembles(Book& book) const {
  typedef std::map<std::string, std::vector<Result> > results_t;
  results_t results;
  for(int m = FIRST_METHOD; m <= LAST_METHOD; m<<=1)
    if(methods_ & m) { results_t g = ensemble_results(book,(Method)(m)); results.insert(g.begin(),g.end());}

  BOOST_FOREACH(const results_t::value_type group, results) {
    const std::string name = group.first;
    BOOST_FOREACH(const Result p, group.second) {
      const float pad = (ensembleUp_-ensembleLow_)/10;
      book.fill( p.reco.first,      name+"_ensembleReco", 12*ensembleBins_, ensembleLow_-pad, ensembleUp_+pad );
      book.fill( p.measured.first,  name+"_measure",      12*ensembleBins_, ensembleLow_-pad, ensembleUp_+pad );
      book.fill( p.measured.second, name+"_merr",         500, 0, 0.01);
      book.fill( (p.measured.first-p.reco.first)/p.measured.second, name+"_pull", 500, -10,10);
    }
    //Need our own copy for thread safety
    TF1 gaus("mygaus","gaus");
    book[name+"_measure"]->Fit(&gaus,"LLQ");
    book[name+"_merr"]->Fit(&gaus,"LLQ");
    book[name+"_pull"]->Fit(&gaus,"LLQ");
  }
}

std::map<std::string, std::vector<LA_Filler_Fitter::EnsembleSummary> > LA_Filler_Fitter::
ensemble_summary(const Book& book) {
  std::map<std::string, std::vector<EnsembleSummary> > summary;
  for(Book::const_iterator it = book.begin(".*_ensembleReco"); it!=book.end(); ++it) {
    const std::string base = boost::erase_all_copy(it->first,"_ensembleReco");

    const TH1*const reco = it->second;
    const TH1*const measure = book[base+"_measure"];
    const TH1*const merr = book[base+"_merr"];
    const TH1*const pull = book[base+"_pull"];

    EnsembleSummary s;
    s.samples = reco->GetEntries();
    s.truth = reco->GetMean();
    s.meanMeasured = std::make_pair<float,float>( measure->GetFunction("gaus")->GetParameter(1), measure->GetFunction("gaus")->GetParError(1) );
    s.sigmaMeasured = std::make_pair<float,float>( measure->GetFunction("gaus")->GetParameter(2), measure->GetFunction("gaus")->GetParError(2) );
    s.meanUncertainty = std::make_pair<float,float>( merr->GetFunction("gaus")->GetParameter(1), merr->GetFunction("gaus")->GetParError(1) );
    s.pull = std::make_pair<float,float>( pull->GetFunction("gaus")->GetParameter(2), pull->GetFunction("gaus")->GetParError(2) );

    const std::string name = boost::regex_replace(base,boost::regex("ensembleBin\\d*_"),"");
    summary[name].push_back(s);
  }
  return summary;
}

std::pair<std::pair<float,float>, std::pair<float,float> > LA_Filler_Fitter::
offset_slope(const std::vector<LA_Filler_Fitter::EnsembleSummary>& ensembles) {
  try {
    std::vector<float> x,y,xerr,yerr;
    BOOST_FOREACH(EnsembleSummary ensemble, ensembles) {
      x.push_back(ensemble.truth);
      xerr.push_back(0);
      y.push_back(ensemble.meanMeasured.first);
      yerr.push_back(ensemble.meanMeasured.second);
    }
    TGraphErrors graph(x.size(),&(x[0]),&(y[0]),&(xerr[0]),&(yerr[0]));
    //Need our own copy for thread safety
    TF1 pol1("mypol1","pol1");
    graph.Fit(&pol1);
    const TF1*const fit = graph.GetFunction("pol1");

    return std::make_pair( std::make_pair(fit->GetParameter(0), fit->GetParError(0)),
			   std::make_pair(fit->GetParameter(1), fit->GetParError(1)) );
  } catch(edm::Exception e) {
    std::cerr << "Fitting Line Failed " << std::endl << e << std::endl;
    return std::make_pair( std::make_pair(0,0), std::make_pair(0,0));
  }
}

float LA_Filler_Fitter::
pull(const std::vector<LA_Filler_Fitter::EnsembleSummary>& ensembles) {
  float p(0),w(0);
  BOOST_FOREACH(EnsembleSummary ensemble, ensembles) {
    const float unc2 = pow(ensemble.pull.second,2);
    p+=  ensemble.pull.first / unc2;
    w+= 1/unc2;
  }
  return p/w;
}

std::ostream& operator<<(std::ostream& strm, const LA_Filler_Fitter::Result& r) {
  return strm << r.reco.first <<"\t"<< r.reco.second <<"\t"
	      << r.measured.first <<"\t"<< r.measured.second <<"\t"
	      << r.calMeasured.first <<"\t"<< r.calMeasured.second <<"\t"
	      << r.field <<"\t"
	      << r.chi2 <<"\t"
	      << r.ndof <<"\t"
	      << r.entries;
}

std::ostream& operator<<(std::ostream& strm, const LA_Filler_Fitter::EnsembleSummary& e) {
  return strm << e.truth <<"\t"
	      << e.meanMeasured.first    <<"\t"<< e.meanMeasured.second <<"\t"
	      << e.sigmaMeasured.first   <<"\t"<< e.sigmaMeasured.second <<"\t"
	      << e.meanUncertainty.first <<"\t"<< e.meanUncertainty.second << "\t"
	      << e.pull.first            <<"\t"<< e.pull.second << "\t"
	      << e.samples;
}


#include <boost/math/tr1.hpp>

int
main ()
{
  namespace tr1 = boost::math::tr1;

  // Use the TR1 libraries.
  //
  static_cast<void> (tr1::betaf (1.0f, 1.0f));
  static_cast<void> (tr1::beta (1.0, 1.0));

  // Boost.Math does not support long double on FreeBSD so only use the long
  // double library on other systems.
  //
#ifndef __FreeBSD__
  static_cast<void> (tr1::betal (1.0l, 1.0l));
#endif

  return 0;
}


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

/*
 Copyright (C) 2006 Joseph Wang
 Copyright (C) 2012 Liquidnet Holdings, Inc.

 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 <ql/models/volatility/garch.hpp>
#include <ql/math/optimization/leastsquare.hpp>
#include <ql/math/optimization/simplex.hpp>
#include <ql/math/autocovariance.hpp>
#include <ql/math/functional.hpp>
#include <boost/foreach.hpp>

namespace QuantLib {

    namespace {

        const Real tol_level = 1.0e-8;

        class Garch11Constraint : public Constraint {
          private:
            class Impl : public Constraint::Impl {
                Real gammaLower_, gammaUpper_;
              public:
                Impl (Real gammaLower, Real gammaUpper)
                : gammaLower_(gammaLower), gammaUpper_(gammaUpper) {}
                bool test(const Array &x) const {
                    QL_REQUIRE(x.size() >= 3, "size of parameters vector < 3");
                    return x[0] > 0 && x[1] >= 0 && x[2] >= 0
                        && x[1] + x[2] < gammaUpper_
                        && x[1] + x[2] >= gammaLower_;
                }
            };
          public:
            Garch11Constraint(Real gammaLower, Real gammaUpper)
            : Constraint(ext::shared_ptr<Constraint::Impl>(
                      new Garch11Constraint::Impl(gammaLower, gammaUpper))) {}
        };

        class Garch11CostFunction : public CostFunction {
          public:
            explicit Garch11CostFunction (const std::vector<Volatility> &);
            virtual Real value(const Array& x) const;
            virtual Disposable<Array> values(const Array& x) const;
            virtual void gradient(Array& grad, const Array& x) const;
            virtual Real valueAndGradient(Array& grad, const Array& x) const;
          private:
            const std::vector<Volatility> &r2_;
        };

        Garch11CostFunction::Garch11CostFunction(
                                            const std::vector<Volatility> &r2)
        : r2_(r2) {}

        Real Garch11CostFunction::value(const Array& x) const {
            Real retval(0.0);
            Real sigma2 = 0;
            Real u2 = 0;
            BOOST_FOREACH (Volatility r2, r2_) {
                sigma2 = x[0] + x[1] * u2 + x[2] * sigma2;
                u2 = r2;
                retval += std::log(sigma2) + u2 / sigma2;
            }
            return retval / (2.0*r2_.size());
        }

        Disposable<Array> Garch11CostFunction::values(const Array& x) const {
            Array retval (r2_.size());
            Real sigma2 = 0;
            Real u2 = 0;
            Size i = 0;
            BOOST_FOREACH (Volatility r2, r2_) {
                sigma2 = x[0] + x[1] * u2 + x[2] * sigma2;
                u2 = r2;
                retval[i++] = (std::log(sigma2) + u2 / sigma2)/(2.0*r2_.size());
            }
            return retval;
        }

        void Garch11CostFunction::gradient(Array& grad, const Array& x) const {
            std::fill (grad.begin(), grad.end(), 0.0);
            Real sigma2 = 0;
            Real u2 = 0;
            Real sigma2prev = sigma2;
            Real u2prev = u2;
            Real norm = 2.0 * r2_.size();
            BOOST_FOREACH (Volatility r2, r2_) {
                sigma2 = x[0] + x[1] * u2 + x[2] * sigma2;
                u2 = r2;
                Real w = (sigma2 - u2) / (sigma2*sigma2);
                grad[0] += w;
                grad[1] += u2prev * w;
                grad[2] += sigma2prev * w;
                u2prev = u2;
                sigma2prev = sigma2;
            }
            std::transform(grad.begin(), grad.end(), grad.begin(),
                           divide_by<Real>(norm));
        }

        Real Garch11CostFunction::valueAndGradient(Array& grad,
                                                   const Array& x) const {
            std::fill (grad.begin(), grad.end(), 0.0);
            Real retval(0.0);
            Real sigma2 = 0;
            Real u2 = 0;
            Real sigma2prev = sigma2;
            Real u2prev = u2;
            Real norm = 2.0 * r2_.size();
            BOOST_FOREACH (Volatility r2, r2_) {
                sigma2 = x[0] + x[1] * u2 + x[2] * sigma2;
                u2 = r2;
                retval += std::log(sigma2) + u2 / sigma2;
                Real w = (sigma2 - u2) / (sigma2*sigma2);
                grad[0] += w;
                grad[1] += u2prev * w;
                grad[2] += sigma2prev * w;
                u2prev = u2;
                sigma2prev = sigma2;
            }
            std::transform(grad.begin(), grad.end(), grad.begin(),
                           divide_by<Real>(norm));
            return retval / norm;
        }

        class FitAcfProblem : public LeastSquareProblem {
          public:
            FitAcfProblem(Real A2, const Array &acf,
                          const std::vector<std::size_t> &idx);
            virtual Size size();
            virtual void targetAndValue(const Array& x, Array& target,
                                        Array& fct2fit);
            virtual void targetValueAndGradient(const Array& x,
                                                Matrix& grad_fct2fit,
                                                Array& target, Array& fct2fit);
          private:
            Real A2_;
            Array acf_;
            std::vector<std::size_t> idx_;
        };

        FitAcfProblem::FitAcfProblem(Real A2, const Array &acf,
                                     const std::vector<std::size_t> &idx)
        : A2_(A2), acf_(acf), idx_(idx) {}

        Size FitAcfProblem::size() { return idx_.size(); }

        void FitAcfProblem::targetAndValue(const Array& x, Array& target,
                                           Array& fct2fit) {
            Real A4 = acf_[0] + A2_*A2_;
            Real gamma = x[0];
            Real beta = x[1];
            target[0] = A2_*A2_/A4;
            fct2fit[0] =
                (1 - 3*gamma*gamma - 2*beta*beta + 4*beta*gamma)
                / (3*(1 - gamma*gamma));
            target[1] = acf_[1] / A4;
            fct2fit[1] = gamma * (1 - fct2fit[0]) - beta;
            for (std::size_t i = 2; i < idx_.size(); ++i) {
                target[i] = acf_[idx_[i]] / A4;
                fct2fit[i] = std::pow(gamma, (int)idx_[i]-1)* fct2fit[1];
            }
        }

        void FitAcfProblem::targetValueAndGradient(const Array& x,
                                                   Matrix& grad_fct2fit,
                                                   Array& target,
                                                   Array& fct2fit) {
            Real A4 = acf_[0] + A2_*A2_;
            Real gamma = x[0];
            Real beta = x[1];
            target[0] = A2_*A2_/A4;
            Real w1 = (1 - 3*gamma*gamma - 2*beta*beta + 4*beta*gamma);
            Real w2 = (1 - gamma*gamma);
            fct2fit[0] = w1 / (3*w2);
            grad_fct2fit[0][0] = (2.0/3.0) * ((2*beta-3*gamma)*w2 + 2*w1*gamma) / (w2*w2);
            grad_fct2fit[0][1] = (4.0/3.0) * (gamma - beta) / w2;
            target[1] = acf_[1] / A4;
            fct2fit[1] = gamma * (1 - fct2fit[0]) - beta;
            grad_fct2fit[1][0] = (1 - fct2fit[0]) - gamma * grad_fct2fit[0][0];
            grad_fct2fit[1][1] = -gamma * grad_fct2fit[0][1] - 1;
            for (std::size_t i = 2; i < idx_.size(); ++i) {
                target[i] = acf_[idx_[i]] / A4;
                w1 = std::pow(gamma, (int)idx_[i]-1);
                fct2fit[i] = w1 * fct2fit[1];
                grad_fct2fit[i][0] = (idx_[i]-1) * (w1/gamma)*fct2fit[1] + w1*grad_fct2fit[1][0];
                grad_fct2fit[i][1] = w1 * grad_fct2fit[1][1];
            }
        }

        class FitAcfConstraint : public Constraint {
          private:
            class Impl : public Constraint::Impl {
                Real gammaLower_, gammaUpper_;
              public:
                Impl(Real gammaLower, Real gammaUpper)
                : gammaLower_(gammaLower), gammaUpper_(gammaUpper) {}
                bool test(const Array &x) const {
                    QL_REQUIRE(x.size() >= 2, "size of parameters vector < 2");
                    return x[0] >= gammaLower_ && x[0] < gammaUpper_
                        && x[1] >= 0 && x[1] <= x[0];
                }
            };
          public:
            FitAcfConstraint(Real gammaLower, Real gammaUpper)
            : Constraint(ext::shared_ptr<Constraint::Impl>(
                       new FitAcfConstraint::Impl(gammaLower, gammaUpper))) {}
        };

        // Initial guess based on fitting ACF - initial guess for
        // fitting acf is a moment matching estimates for mean(r2),
        // acf(0), and acf(1).
        Real initialGuess1(const Array &acf, Real mean_r2,
                           Real &alpha, Real &beta, Real &omega) {
            Real A21 = acf[1];
            Real A4 = acf[0] + mean_r2*mean_r2;

            Real A = mean_r2*mean_r2/A4; // 1/sigma^2
            Real B = A21 / A4; // rho(1)

            Real gammaLower = A <= 1./3. - tol_level ? std::sqrt((1 - 3*A)/(3 - 3*A)) + tol_level : tol_level;
            Garch11Constraint constraints(gammaLower, 1.0 - tol_level);

            Real gamma = gammaLower + (1 - gammaLower) * 0.5;
            beta = std::min(gamma, std::max(gamma * (1 - A) - B, 0.0));
            alpha = gamma - beta;
            omega = mean_r2 * (1 - gamma);

            if (std::fabs(A-0.5) < QL_EPSILON) {
                gamma = std::max(gammaLower, -(1+4*B*B)/(4*B));
                beta = std::min(gamma, std::max(gamma * (1 - A) - B, 0.0));
                alpha = gamma - beta;
                omega = mean_r2 * (1 - gamma);
            } else {
                if (A > 1.0 - QL_EPSILON) {
                    gamma = std::max(gammaLower, -(1+B*B)/(2*B));
                    beta = std::min(gamma, std::max(gamma * (1 - A) - B, 0.0));
                    alpha = gamma - beta;
                    omega = mean_r2 * (1 - gamma);
                } else {
                    Real D = (3*A-1)*(2*B*B+(1-A)*(2*A-1));
                    if (D >= 0) {
                        Real d = std::sqrt(D);
                        Real b = (B - d)/(2*A-1);
                        Real g = 0;
                        if (b >= tol_level && b <= 1.0 - tol_level) {
                            g = (b + B) / (1 - A);
                        }
                        if (g < gammaLower) {
                            b = (B + d)/(2*A-1);
                            if (b >= tol_level && b <= 1.0 - tol_level) {
                                g = (b + B) / (1 - A);
                            }
                        }
                        if (g >= gammaLower) {
                            gamma = g;
                            beta = std::min(gamma, std::max(gamma * (1 - A) - B, 0.0));
                            alpha = gamma - beta;
                            omega = mean_r2 * (1 - gamma);
                        }
                    }
                }
            }

            std::vector<std::size_t> idx;
            std::size_t nCov = acf.size() - 1;
            for (std::size_t i = 0; i <= nCov; ++i) {
                if (i < 2 || (acf[i] > 0 && acf[i-1] > 0 && acf[i-1] > acf[i])) {
                    idx.push_back(i);
                }
            }

            Array x(2);
            x[0] = gamma;
            x[1] = beta;

            try {
                FitAcfConstraint c(gammaLower, 1.0 - tol_level);
                NonLinearLeastSquare nnls(c);
                nnls.setInitialValue(x);
                FitAcfProblem pr(mean_r2, acf, idx);
                x = nnls.perform(pr);
                Array guess(3);
                guess[0] = mean_r2 * (1 - x[0]);
                guess[1] = x[0] - x[1];
                guess[2] = x[1];
                if (constraints.test(guess)) {
                    omega = guess[0];
                    alpha = guess[1];
                    beta = guess[2];
                }
            } catch (const std::exception &) {
                // failed -- returning initial values
            }
            return gammaLower;
        }

        // Initial guess based on fitting ACF - initial guess for
        // fitting acf is an estimate of gamma = alpfa+beta based on
        // the property: acf(i+1) = gamma*acf(i) for i > 1.
        Real initialGuess2 (const Array &acf, Real mean_r2,
                            Real &alpha, Real &beta, Real &omega) {
            Real A21 = acf[1];
            Real A4 = acf[0] + mean_r2*mean_r2;
            Real A = mean_r2*mean_r2/A4; // 1/sigma^2
            Real B = A21 / A4; // rho(1)
            Real gammaLower = A <= 1./3. - tol_level ? std::sqrt((1 - 3*A)/(3 - 3*A)) + tol_level : tol_level;
            Garch11Constraint constraints(gammaLower, 1.0 - tol_level);

            // ACF
            Real gamma = 0;
            std::size_t nn = 0;
            std::vector<std::size_t> idx;
            std::size_t nCov = acf.size() - 1;
            for (std::size_t i = 0; i <= nCov; ++i) {
                if (i < 2) idx.push_back(i);
                if (i > 1 && acf[i] > 0 && acf[i-1] > 0 && acf[i-1] > acf[i]) {
                    gamma += acf[i]/acf[i-1];
                    nn++;
                    idx.push_back(i);
                }
            }
            if (nn > 0)
                gamma /= nn;
            if (gamma < gammaLower) gamma = gammaLower;
            beta = std::min(gamma, std::max(gamma * (1 - A) - B, 0.0));
            omega = mean_r2 * (1 - gamma);

            Array x(2);
            x[0] = gamma;
            x[1] = beta;

            try {
                FitAcfConstraint c(gammaLower, 1 - tol_level);
                NonLinearLeastSquare nnls(c);
                nnls.setInitialValue(x);
                FitAcfProblem pr(mean_r2, acf, idx);
                x = nnls.perform(pr);
                Array guess(3);
                guess[0] = mean_r2 * (1 - x[0]);
                guess[1] = x[0] - x[1];
                guess[2] = x[1];
                if (constraints.test(guess)) {
                    omega = guess[0];
                    alpha = guess[1];
                    beta = guess[2];
                }
            } catch (const std::exception &) {
                // failed -- returning initial values
            }
            return gammaLower;
        }

    }

    Garch11::time_series
    Garch11::calculate(const time_series& quoteSeries,
                       Real alpha, Real beta, Real omega) {
        time_series retval;
        const_iterator cur = quoteSeries.cbegin();
        Real u = cur->second;
        Real sigma2 = u*u;
        while (++cur != quoteSeries.end()) {
            sigma2 = omega + alpha * u * u + beta * sigma2;
            retval[cur->first] = std::sqrt(sigma2);
            u = cur->second;
        }
        sigma2 = omega + alpha * u * u + beta * sigma2;
        --cur;
        const_iterator prev = cur;
        retval[cur->first + (cur->first - (--prev)->first) ] = std::sqrt(sigma2);
        return retval;
    }

    ext::shared_ptr<Problem> Garch11::calibrate_r2(
                   Mode mode, const std::vector<Volatility> &r2, Real mean_r2,
                   Real &alpha, Real &beta, Real &omega) {
        EndCriteria endCriteria(10000, 500, tol_level, tol_level, tol_level);
        Simplex method(0.001);
        return calibrate_r2(mode, r2, mean_r2, method, endCriteria,
                            alpha, beta, omega);
    }

    ext::shared_ptr<Problem> Garch11::calibrate_r2(
                   Mode mode, const std::vector<Volatility> &r2, Real mean_r2,
                   OptimizationMethod &method, const EndCriteria &endCriteria,
                   Real &alpha, Real &beta, Real &omega) {
        Real dataSize = Real(r2.size());
        alpha = 0.0;
        beta = 0.0;
        omega = 0.0;
        QL_REQUIRE (dataSize >= 4,
                    "Data series is too short to fit GARCH model");
        QL_REQUIRE (mean_r2 > 0, "Data series is constant");
        omega = mean_r2 * dataSize / (dataSize - 1);

        // ACF
        Size maxLag = (Size)std::sqrt(dataSize);
        Array acf(maxLag+1);
        std::vector<Volatility> tmp(r2.size());
        std::transform (r2.begin(), r2.end(), tmp.begin(),
                        subtract<Real>(mean_r2));
        autocovariances (tmp.begin(), tmp.end(), acf.begin(), maxLag);
        QL_REQUIRE (acf[0] > 0, "Data series is constant");

        Garch11CostFunction cost (r2);

        // two initial guesses based on fitting ACF
        Real gammaLower = 0.0;
        Array opt1(3);
        Real fCost1 = QL_MAX_REAL;
        if (mode != GammaGuess) {
            gammaLower = initialGuess1(acf, mean_r2, opt1[1], opt1[2], opt1[0]);
            fCost1 = cost.value(opt1);
        }

        Array opt2(3);
        Real fCost2 = QL_MAX_REAL;
        if (mode != MomentMatchingGuess) {
            gammaLower = initialGuess2(acf, mean_r2, opt2[1], opt2[2], opt2[0]);
            fCost2 = cost.value(opt2);
        }

        Garch11Constraint constraints(gammaLower, 1.0 - tol_level);

        ext::shared_ptr<Problem> ret;
        if (mode != DoubleOptimization) {
            try {
                ret = calibrate_r2(r2, method, constraints, endCriteria,
                                   fCost1 <= fCost2 ? opt1 : opt2,
                                   alpha, beta, omega);
            } catch (const std::exception &) {
                if (fCost1 <= fCost2) {
                    alpha = opt1[1];
                    beta = opt1[2];
                    omega = opt1[0];
                } else {
                    alpha = opt2[1];
                    beta = opt2[2];
                    omega = opt2[0];
                }
            }
        } else {
            ext::shared_ptr<Problem> ret1, ret2;
            try {
                ret1 = calibrate_r2(r2, method, constraints, endCriteria,
                                    opt1, alpha, beta, omega);
                opt1[1] = alpha;
                opt1[2] = beta;
                opt1[0] = omega;
                if (constraints.test(opt1))
                    fCost1 = std::min(fCost1, cost.value(opt1));
            } catch (const std::exception &) {
                fCost1 = QL_MAX_REAL;
            }

            try {
                ret2 = calibrate_r2(r2, method, constraints, endCriteria,
                                    opt2, alpha, beta, omega);
                opt2[1] = alpha;
                opt2[2] = beta;
                opt2[0] = omega;
                if (constraints.test(opt2))
                    fCost2 = std::min(fCost2, cost.value(opt2));
            } catch (const std::exception &) {
                fCost2 = QL_MAX_REAL;
            }

            if (fCost1 <= fCost2) {
                alpha = opt1[1];
                beta = opt1[2];
                omega = opt1[0];
                ret = ret1;
            } else {
                alpha = opt2[1];
                beta = opt2[2];
                omega = opt2[0];
                ret = ret2;
            }
        }
        return ret;
    }

    ext::shared_ptr<Problem> Garch11::calibrate_r2(
               const std::vector<Volatility> &r2,
               OptimizationMethod &method,
               const EndCriteria &endCriteria,
               const Array &initGuess, Real &alpha, Real &beta, Real &omega) {
        Garch11Constraint constraints(0.0, 1.0 - tol_level);
        return calibrate_r2(r2, method, constraints, endCriteria,
                            initGuess, alpha, beta, omega);
    }

    ext::shared_ptr<Problem> Garch11::calibrate_r2(
               const std::vector<Volatility> &r2,
               Real mean_r2,
               OptimizationMethod &method,
               const EndCriteria &endCriteria,
               const Array &initGuess, Real &alpha, Real &beta, Real &omega) {
        std::vector<Volatility> tmp(r2.size());
        std::transform (r2.begin(), r2.end(), tmp.begin(),
                        subtract<Real>(mean_r2));
        return calibrate_r2(tmp, method, endCriteria, initGuess,
                            alpha, beta, omega);
    }

    ext::shared_ptr<Problem> Garch11::calibrate_r2(
               const std::vector<Volatility> &r2,
               OptimizationMethod &method,
               Constraint &constraints,
               const EndCriteria &endCriteria,
               const Array &initGuess, Real &alpha, Real &beta, Real &omega) {
        Garch11CostFunction cost(r2);
        ext::shared_ptr<Problem> problem(
                               new Problem(cost, constraints, initGuess));
        // TODO: check return value from minimize()
        /* EndCriteria::Type ret = */
        method.minimize(*problem, endCriteria);
        const Array &optimum = problem->currentValue();
        alpha = optimum[1];
        beta = optimum[2];
        omega = optimum[0];
        return problem;
    }

    ext::shared_ptr<Problem> Garch11::calibrate_r2(
               const std::vector<Volatility> &r2,
               Real mean_r2,
               OptimizationMethod &method,
               Constraint &constraints,
               const EndCriteria &endCriteria,
               const Array &initGuess, Real &alpha, Real &beta, Real &omega) {
        std::vector<Volatility> tmp(r2.size());
        std::transform (r2.begin(), r2.end(), tmp.begin(),
                        subtract<Real>(mean_r2));
        return calibrate_r2(tmp, method, constraints, endCriteria,
                            initGuess, alpha, beta, omega);
    }

}