//******************************************************************************
//  Project:		Weather-based simulation framework (WBSF)
//	Programmer:     Rémi Saint-Amant
//
//  It under the terms of the GNU General Public License as published by
//     the Free Software Foundation
//  It is provided "as is" without express or implied warranty.
//******************************************************************************
// 22-01-2019	1.0.3	Rémi Saint-Amant	sieve uniformly for moths reduction (optimization)
// 17-04-2018	1.0.2	Rémi Saint-Amant	Transfer liftoff here instead of in the model
// 01-01-2016	1.0.1	Rémi Saint-Amant	Creation
//******************************************************************************
#include "stdafx.h"
#include <boost\algorithm\string.hpp>
#include <Boost\multi_array.hpp>
#include <algorithm>
#pragma warning(disable: 4275 4251)
#include "gdal_priv.h"

#include "Basic/Statistic.h"
#include "Basic/HourlyDatabase.h"
#include "Basic/ModelStat.h"
#include "FileManager/FileManager.h"
#include "Geomatic/IWD.h"
#include "Simulation/Dispersal.h"
#include "Simulation/ExecutableFactory.h"
#include "Simulation/ATM.h"

#include "WeatherBasedSimulationString.h"

using namespace boost;
using namespace std;
using namespace WBSF::HOURLY_DATA;
using namespace WBSF::WEATHER;
using namespace WBSF::DIMENSION;

namespace WBSF
{
	enum TSBWInput { I_YEAR, I_MONTH, I_DAY, I_SEX, I_A, I_M, I_G, I_Fᵒ, I_Fᴰ, NB_SBW_INPUTS };
	static const char* SBW_VARIABLE_NAME[NB_SBW_INPUTS] = { "Year", "Month","Day","sex","A", "M", "G", "F°", "F" };

	//*******************************************************************************

	static const double IWD_POWER = 1.0;
	static const double WIND_SEED_BIAS_FACTOR = 67.3 / 58.7;

	std::string CWindStability::GetWindStabilityName(int stabType)
	{
		ASSERT(stabType >= 0 && stabType < NB_STABILITY);

		static const StringVector WIND_STABILITY_NAME(IDS_WG_WIND_STABILITY, "|;");

		return WIND_STABILITY_NAME[stabType];
	}

	//*******************************************************************************

	const char* CDispersalParamters::MEMBERS_NAME[CDispersalParamters::NB_MEMBERS] = { "World", "Moths" };

	//*******************************************************************************
	const char* CDispersal::XML_FLAG = "Dispersal";
	const char* CDispersal::MEMBERS_NAME[CDispersal::NB_MEMBERS_EX] = { "Parameters" };
	const int CDispersal::CLASS_NUMBER = CExecutableFactory::RegisterClass(CDispersal::GetXMLFlag(), &CDispersal::CreateObject);

	CDispersal::CDispersal()
	{
		ClassReset();
	}

	CDispersal::CDispersal(const CDispersal& in)
	{
		operator=(in);
	}

	CDispersal::~CDispersal()
	{}

	void CDispersal::Reset()
	{
		CExecutable::Reset();
		ClassReset();
	}

	void CDispersal::ClassReset()
	{
		m_name = "Dispersal";
		m_parameters.clear();
	}

	CDispersal& CDispersal::operator =(const CDispersal& in)
	{
		if (&in != this)
		{
			CExecutable::operator =(in);
			m_parameters = in.m_parameters;
		}

		ASSERT(*this == in);
		return *this;
	}

	bool CDispersal::operator == (const CDispersal& in)const
	{
		bool bEqual = true;

		if (CExecutable::operator !=(in))bEqual = false;
		if (m_parameters != in.m_parameters)bEqual = false;

		return bEqual;
	}

	ERMsg CDispersal::GetParentInfo(const CFileManager& fileManager, CParentInfo& info, CParentInfoFilter filter)const
	{
		ERMsg msg;

		if (filter[REPLICATION])
			filter.set(TIME_REF);

		msg = m_pParent->GetParentInfo(fileManager, info, filter);
		if (msg)
		{
			if (filter[LOCATION])
			{
				//same as parent
			}

			if (filter[PARAMETER])
			{
				//same as parent
			}

			if (filter[REPLICATION])
			{
				ASSERT(info.m_period.GetTType() == CTM::ATEMPORAL);
				info.m_nbReplications *= info.m_period.size();
			}

			if (filter[TIME_REF])
			{
				info.m_period.Transform(CTM(CTM::HOURLY));
			}

			if (filter[VARIABLE])
			{
				GetOutputDefinition(info.m_variables);
			}
		}

		return msg;
	}

	size_t Find(const CModelOutputVariableDefVector& vars, string name)
	{
		size_t v = NOT_INIT;

		for (size_t vv = 0; vv < vars.size() && v == NOT_INIT; vv++)
			if (WBSF::IsEqual(vars[vv].m_name, name))
				v = vv;

		return v;
	}

	void CDispersal::GetInputDBInfo(CResultPtr& pResult, CDBMetadata& info)
	{
		const CModelOutputVariableDefVector& vars = pResult->GetMetadata().GetOutputDefinition();

		if (!vars.empty())
		{
			CModelOutputVariableDefVector outputVar;
			GetOutputDefinition(outputVar);

			CTPeriod period = m_parameters.m_world.m_simulationPeriod;
			period.Transform(CTM::HOURLY);

			info.SetLocations(pResult->GetMetadata().GetLocations());
			info.SetParameterSet(pResult->GetMetadata().GetParameterSet());
			info.SetOutputDefinition(outputVar);
			info.SetTPeriod(period);
		}
	}

	ERMsg CDispersal::GetOutputDefinition(const CFileManager& fileManager, CModelOutputVariableDefVector& outputVar)const
	{
		GetOutputDefinition(outputVar);

		return ERMsg();
	}

	void CDispersal::GetOutputDefinition(CModelOutputVariableDefVector& outputVar)const
	{
		outputVar.clear();
		ASSERT(NB_ATM_OUTPUT == 35);
		outputVar.push_back(CModelOutputVariableDef("FlightNo", "FlightNo", "", "Flight numero for this moth", CTM(CTM::ATEMPORAL), 5));
		outputVar.push_back(CModelOutputVariableDef("Age", "Age", "[0..1]", "Phisiological age", CTM(CTM::ATEMPORAL), 5));
		outputVar.push_back(CModelOutputVariableDef("Sex", "Sex", "m=0|f=1", "Sex of moth", CTM(CTM::ATEMPORAL), 0));
		outputVar.push_back(CModelOutputVariableDef("A", "A", "cm²", "Forewing surface area", CTM(CTM::ATEMPORAL), 5));
		outputVar.push_back(CModelOutputVariableDef("M", "M", "g", "Dry weight", CTM(CTM::ATEMPORAL), 5));
		outputVar.push_back(CModelOutputVariableDef("Broods", "Broods", "", "Broods", CTM(CTM::ATEMPORAL), 5));
		outputVar.push_back(CModelOutputVariableDef("G", "G", "", "Gravidity", CTM(CTM::ATEMPORAL), 5));
		outputVar.push_back(CModelOutputVariableDef("F", "F", "", "F", CTM(CTM::ATEMPORAL), 5));
		outputVar.push_back(CModelOutputVariableDef("State", "State", "", "State of moth", CTM(CTM::ATEMPORAL), 0));
		outputVar.push_back(CModelOutputVariableDef("Flag", "Flag", "", "State flag", CTM(CTM::ATEMPORAL), 0));
		outputVar.push_back(CModelOutputVariableDef("X", "X", "m", "Current X coordinate", CTM(CTM::ATEMPORAL), 5));
		outputVar.push_back(CModelOutputVariableDef("Y", "Y", "m", "Current Y coordinate", CTM(CTM::ATEMPORAL), 5));
		outputVar.push_back(CModelOutputVariableDef("Latitude", "Latitude", "°", "Current latitude", CTM(CTM::ATEMPORAL), 5));
		outputVar.push_back(CModelOutputVariableDef("Longitude", "Longitude", "°", "Current longitude", CTM(CTM::ATEMPORAL), 5));
		outputVar.push_back(CModelOutputVariableDef("Tair", "Tair", "°C", "Air temperature", CTM(CTM::ATEMPORAL), 5));
		outputVar.push_back(CModelOutputVariableDef("Prcp", "Prcp", "mm", "Precipitation", CTM(CTM::ATEMPORAL), 5));
		outputVar.push_back(CModelOutputVariableDef("U", "U", "km/h", "mean U wind speed", CTM(CTM::ATEMPORAL), 5));
		outputVar.push_back(CModelOutputVariableDef("V", "V", "km/h", "mean V wind speed", CTM(CTM::ATEMPORAL), 5));
		outputVar.push_back(CModelOutputVariableDef("W", "W", "km/h", "mean W wind speed", CTM(CTM::ATEMPORAL), 5));
		outputVar.push_back(CModelOutputVariableDef("Galt", "GroundAltitude", "m", "Ground altitude.", CTM(CTM::ATEMPORAL), 5));
		outputVar.push_back(CModelOutputVariableDef("MH", "MeanHeight", "m", "Mean flight height.", CTM(CTM::ATEMPORAL), 5));
		outputVar.push_back(CModelOutputVariableDef("CH", "CurrentHeight", "m", "Current flight height. Begin at 5 meters and end at 0.", CTM(CTM::ATEMPORAL), 5));
		outputVar.push_back(CModelOutputVariableDef("MDH", "MeanDeltaHeight", "m", "Mean change in height", CTM(CTM::ATEMPORAL), 5));
		outputVar.push_back(CModelOutputVariableDef("MothSpeed", "MothSpeed", "", "Moth speed without wind speed", CTM(CTM::ATEMPORAL), 5));
		outputVar.push_back(CModelOutputVariableDef("Wh", "Whorizontal", "km/h", "Mean horizontal velocity", CTM(CTM::ATEMPORAL), 5));
		outputVar.push_back(CModelOutputVariableDef("Wv", "Wvertical", "km/h", "Mean vertical velocity", CTM(CTM::ATEMPORAL), 5));
		outputVar.push_back(CModelOutputVariableDef("D", "Direction", "°", "Direction", CTM(CTM::ATEMPORAL), 5));
		outputVar.push_back(CModelOutputVariableDef("Distance", "Distance", "m", "Distance", CTM(CTM::ATEMPORAL), 5));
		outputVar.push_back(CModelOutputVariableDef("DistanceOrigine", "DistanceOrigine", "m", "DistanceOrigine", CTM(CTM::ATEMPORAL), 5));
		outputVar.push_back(CModelOutputVariableDef("FlightTime", "FlightTime", "h", "FlightTime", CTM(CTM::ATEMPORAL), 5));
		outputVar.push_back(CModelOutputVariableDef("LiftoffTime", "LiftoffTime", "Decimal hour", "", CTM(CTM::ATEMPORAL), 5));
		outputVar.push_back(CModelOutputVariableDef("LandingTime", "LandingTime", "Decimal hour", "", CTM(CTM::ATEMPORAL), 5));
		outputVar.push_back(CModelOutputVariableDef("LiftoffT", "LiftoffT", "°C", "Liftoff Temperature", CTM(CTM::ATEMPORAL), 5));
		outputVar.push_back(CModelOutputVariableDef("LandingT", "LandingT", "°C", "Landing Temperature", CTM(CTM::ATEMPORAL), 5));
		outputVar.push_back(CModelOutputVariableDef("Defoliation", "Defoliation", "", "Defoliation", CTM(CTM::ATEMPORAL), 5));

	}

	void CDispersal::writeStruc(zen::XmlElement& output)const
	{
		string test = GetMemberName(ATM_PARAMETERS);

		CExecutable::writeStruc(output);
		zen::XmlOut out(output);
		out[GetMemberName(ATM_PARAMETERS)](m_parameters);
	}

	bool CDispersal::readStruc(const zen::XmlElement& input)
	{
		CExecutable::readStruc(input);
		zen::XmlIn in(input);
		in[GetMemberName(ATM_PARAMETERS)](m_parameters);

		return true;
	}

	//*******************************************************************************
	CGeoPoint CDispersal::GetNewPosition(const CGeoPoint& pt, double U, double V)
	{
		//formaula from site:http://williams.best.vwh.net/avform.htm#LL
		//A point{ lat, lon } is a distance d out on the tc radial from point 1 if:
		//This algorithm is limited to distances such that dlon <pi / 2, i.e those that extend around less than one quarter of the circumference of the earth in longitude.A completely general, but more complicated algorithm is necessary if greater distances are allowed :

		double tc = fmod(3 * PI / 2 - atan2(V, U), 2 * PI);
		double distance = sqrt(U*U + V * V);//distance in km
		double d = distance / 6371; //distance in radian 6366.71

		double lat1 = Deg2Rad(pt.m_lat);
		double lon1 = Deg2Rad(pt.m_lon);

		double lat2 = asin(sin(lat1)*cos(d) + cos(lat1)*sin(d)*cos(tc));
		double dlon = atan2(sin(tc)*sin(d)*cos(lat1), cos(d) - sin(lat1)*sin(lat2));
		double lon2 = fmod(lon1 + dlon + PI, 2 * PI) - PI;

		CGeoPoint pt2(Rad2Deg(lon2), Rad2Deg(lat2), PRJ_WGS_84);
		ASSERT(fabs(pt.GetDistance(pt2) / 1000 - distance) < 0.1);

		return pt2;
	}

	//http://www.ndbc.noaa.gov/view_text_file.php?filename=44011h2013.txt.gz&dir=data/historical/stdmet/

	//http ://www.meds-sdmm.dfo-mpo.gc.ca/isdm-gdsi/azmp-pmza/met/plot-graph-eng.asp?a=12
	//http ://www.meds-sdmm.dfo-mpo.gc.ca/isdm-gdsi/azmp-pmza/met/plot-graph-eng.asp?a=12
	//www.meds - sdmm.dfo - mpo.gc.ca / alphapro / zmp / met / data / Natashquan_A_HLY01.zip
	//http://www.meds-sdmm.dfo-mpo.gc.ca/isdm-gdsi/azmp-pmza/met/index-eng.asp
	//http ://isdm.gc.ca/isdm-gdsi/waves-vagues/search-recherche/map-carte/index-eng.asp?MedsID=All&ID=&StnName=&Active=ON&Lat1=&Lat2=&Long1=&Long2=&sDate=&eDate=&typedisplay=map&Search=Get+Results
	//http://isdm.gc.ca/isdm-gdsi/waves-vagues/search-recherche/list-liste/data-donnees-eng.asp?medsid=C44255

	struct location_compare
	{
		bool operator() (const CLocation& l1, const CLocation& l2) const
		{
			return l1.m_lat < l2.m_lat || l1.m_lon < l2.m_lon;
		}
	};

	double SumBroods(const CNewSectionData& section, size_t vBroods)
	{
		double sumBroods = 0;
		for (size_t t = 0; t < section.GetRows(); t++)
			sumBroods += section[t][vBroods][MEAN];

		return sumBroods;
	}

	/*ERMsg CreateGribsFromNetCDF(CCallback& callback)
	{
		ERMsg msg;

		enum VAR { V_GHT, V_T, V_U, V_V, V_W, V_PRESS, V_PRCP, V_NBV };

		static const char* VN[V_NBV] = { "geopotential_height","temperature","u_unstaggered","v_unstaggered","w_unstaggered","pressure","precipitation" };

		WBSF::ofStream convert;
		msg = convert.open("D:\\Travaux\\WRF2013Test\\Convert.bat");

		WBSF::ofStream build;
		msg += build.open("D:\\Travaux\\WRF2013Test\\Build.bat");

		if (msg)
		{
			StringVector list = GetFilesList("D:\\Travaux\\WRF2013Test\\NetCDF\\*.nc");
			for (size_t i = 0; i < list.size(); i++)
			{
				string file_title = GetFileTitle(list[i]);
				string file_path = "D:\\Travaux\\WRF2013Test\\tmp\\" + file_title + ".file_list.txt";

				WBSF::ofStream file_list;

				msg += file_list.open(file_path);
				if (msg)
				{

					for (size_t v = 0; v < V_NBV; v++)
					{
						for (size_t l = 0; l < (v!= V_PRCP ?13:1); l++)
						{
							convert << "gdal_translate -b " << l+1 << " -ot Float32 -a_ullr  145750 189000 420750 -80000 -a_srs \"+proj=lcc +lat_1=30 +lat_2=60 +lat_0=48.13746 +lon_0=-71.4 +x_0=0 +y_0=0 +datum=WGS84\" NETCDF:\"D:/Travaux/WRF2013Test/NetCDF/" << file_title << ".nc\":" << VN[v] << " ./tmp/" << file_title << "." << VN[v] << "." << l << ".tif" << endl;
							file_list << "D:\\Travaux\\WRF2013Test\\tmp\\" << file_title << "." << VN[v] << "." << l << ".tif" << endl;
						}
					}

					file_list.close();
					build << "gdalBuildVRT -overwrite -separate -input_file_list " << file_path << " D:\\Travaux\\WRF2013Test\\tmp\\" << file_title << ".vrt" << endl;
					build << "gdal_translate -co compress=LZW -ot Float32 -co TILED=YES -co BLOCKXSIZE=128 -co BLOCKYSIZE=128 .\\tmp\\" << file_title << ".vrt .\\geoTIFF\\" << file_title << ".tif" << endl << endl;

					WBSF::CopyOneFile("D:\\Travaux\\WRF2013Test\\template.inv", "D:\\Travaux\\WRF2013Test\\GeoTIFF\\"+ file_title +".inv", false);
				}
			}
		}

		return msg;
	}*/

	ERMsg CDispersal::Execute(const CFileManager& fileManager, CCallback& callback)
	{
		ERMsg msg;

		//return CreateGribsFromNetCDF(callback);

		callback.AddMessage(string("Execute dispersal with ATM-SBW version: ") + WBSF_ATM_VERSION);

		GIntBig test = GDALGetCacheMax64();
		GDALSetCacheMax64(128 * 1024 * 1024);

		CATMWorld world;
		world.m_world_param = m_parameters.m_world;
		world.m_moths_param = m_parameters.m_moths;
		world.m_nb_max_threads = CTRL.m_nbMaxThreads;

		ofStream output_file;
		ofStream sub_hourly_file;

		string DEM_filepath, gribs_filepath, hourly_DB_filepath, defoliation_filepath, host_filepath, distraction_filepath, water_filepath;
		string outputPath = GetPath(fileManager);		//Generate output path
		string DBFilePath = GetDBFilePath(outputPath);		//Generate DB file path
		string outputFilePath = !m_parameters.m_world.m_outputFileTitle.empty() && m_parameters.m_world.m_bOutputSubHourly ? fileManager.GetOutputPath() + GetFileTitle(m_parameters.m_world.m_outputFileTitle) + ".csv" : "";

		//if (!m_parameters.m_world.m_simulationPeriod.IsInside(m_parameters.m_world.m_flightPeriod))
			//msg.ajoute("Flight period (" + m_parameters.m_world.m_flightPeriod.GetFormatedString() + ") must be inside simulation period (" + m_parameters.m_world.m_simulationPeriod.GetFormatedString() + ")");

		if (!m_parameters.m_world.m_DEM_name.empty())
			msg += fileManager.MapInput().GetFilePath(m_parameters.m_world.m_DEM_name, DEM_filepath);
		else
			msg.ajoute("A DEM must be supply");

		if (!m_parameters.m_world.m_defoliation_name.empty())
			msg += fileManager.MapInput().GetFilePath(m_parameters.m_world.m_defoliation_name, defoliation_filepath);
		if (!m_parameters.m_world.m_water_name.empty())
			msg += fileManager.MapInput().GetFilePath(m_parameters.m_world.m_water_name, water_filepath);
		if (m_parameters.m_world.UseGribs())
		{
			if (!m_parameters.m_world.m_gribs_name.empty())
				msg += fileManager.Gribs().GetFilePath(m_parameters.m_world.m_gribs_name, gribs_filepath);
			else
				msg.ajoute("Gribs file is not defined");
		}

		if ((m_parameters.m_world.UseHourlyDB() || m_parameters.m_world.m_PSource == CATMWorldParamters::PRCP_WEATHER_STATION))
		{
			if (!m_parameters.m_world.m_hourly_DB_name.empty())
				msg += fileManager.Hourly().GetFilePath(m_parameters.m_world.m_hourly_DB_name, hourly_DB_filepath);
			else
				msg.ajoute("Hourly database is not defined");
		}

		output_file.open(DBFilePath + ".tmp", std::fstream::binary | std::fstream::out | std::fstream::trunc);

		if (!outputFilePath.empty())
			msg += sub_hourly_file.open(outputFilePath);

		CResultPtr pResult = m_pParent->GetResult(fileManager);
		msg += pResult->Open();

		//open outputDB
		CResult result;
		msg += result.Open(DBFilePath, std::fstream::binary | std::fstream::out | std::fstream::trunc);

		if (!msg)
			return msg;

		//init output info
		CDBMetadata& metadata = result.GetMetadata();
		GetInputDBInfo(pResult, metadata);

		const CModelOutputVariableDefVector& vars = pResult->GetMetadata().GetOutputDefinition();

		bool bMissing = false;
		std::array<size_t, NB_SBW_INPUTS> varsPos;
		for (size_t i = 0; i < NB_SBW_INPUTS; i++)
		{
			varsPos[i] = Find(vars, SBW_VARIABLE_NAME[i]);
			bMissing = bMissing || varsPos[i] == NOT_INIT;
		}

		if (bMissing)
		{
			msg.ajoute("Invalid dispersal variables input. Variable \"Year\", \"Month\", \"Day\",\"Sex\", \"A\", \"M\", \"G\", \"F°\", \"F\", must be defined");
			return msg;
		}

		callback.PushTask("Open Dispersal's Input", 4);

		if (msg)
		{
			msg += world.m_DEM_DS.OpenInputImage(DEM_filepath);
			callback.StepIt();
		}

		if (msg)
		{
			msg += world.m_weather.Load(gribs_filepath, hourly_DB_filepath, callback);
			callback.StepIt();
		}

		if (msg)
		{
			if (!defoliation_filepath.empty())
			{
				msg += world.m_defoliation_DS.OpenInputImage(defoliation_filepath);
			}
			else if (world.m_moths_param.m_maxFlights > 1)
			{
				msg.ajoute("maximum flights is more than 1 but there is no defoliation map. Reset maximum flights to 1 or provide defoliation map.");
			}

			callback.StepIt();
		}

		if (msg)
		{
			if (!water_filepath.empty())
			{
				msg += world.m_water_DS.OpenInputImage(water_filepath);
			}
			else if (world.m_moths_param.m_maxFlights > 1)
			{
				msg.ajoute("maximum flights is more than 1 but there is no water map. Reset maximum flights to 1 or provide water map.");
			}

			callback.StepIt();
		}

		callback.PopTask();

		CTPeriod outputPeriod2 = world.m_world_param.m_simulationPeriod;
		outputPeriod2.End()++;//add one day at the end
		outputPeriod2.Transform(CTM::HOURLY);
		outputPeriod2.Begin().m_hour = 16;
		outputPeriod2.End().m_hour = 15;

		CTPeriod weatherPeriod = world.m_weather.GetEntireTPeriod();
		//weatherPeriod.Transform(CTM::DAILY);

		CTPeriod savedPeriod = outputPeriod2.Intersect(weatherPeriod);

		ASSERT(outputPeriod2.GetTM() == weatherPeriod.GetTM());
		if (!outputPeriod2.IsIntersect(weatherPeriod))
		{
			msg.ajoute("Weather period doesn't intersect simulation period");
		}

		if (!msg)
			return msg;

		//if (period.End() > weatherPeriod.End())//stop simulation at the end of weather
			//period.End() = weatherPeriod.End();

		const CLocationVector& locations = metadata.GetLocations();
		callback.PushTask("Initialize dispersal moths", metadata.GetNbReplications() *locations.size()*metadata.GetParameterSet().size());

		CGeoExtents extents = world.m_DEM_DS.GetExtents();
		extents.Reproject(GetReProjection(world.m_DEM_DS.GetPrjID(), PRJ_WGS_84));

		double max_moth_prop = 1;//all moths by default
		size_t nbMoths = 0;
		if (world.m_world_param.m_maxFlyers > 0)//replace maxFlyers par maxMoths
		{
			//for optimization, clean extra moths
			nbMoths = GetNbMoths(pResult);
			if (nbMoths > world.m_world_param.m_maxFlyers)
			{
				max_moth_prop = ((double)world.m_world_param.m_maxFlyers/ nbMoths);

				callback.AddMessage("Number of moths before optimization: " + ToString(nbMoths) + " moths");
			}
		}

		std::vector<std::array<size_t, 3>> IDmap;

		size_t no = 0;
		size_t nbReplications = 0;
		size_t moths_before = 0;
		size_t moths_after = 0;
		size_t sum_individual = 0;
		for (size_t l = 0; l < locations.size() && msg; l++)
		{
			for (size_t p = 0; p < pResult->GetMetadata().GetParameterSet().size() && msg; p++)
			{
				size_t rr = 0;
				for (size_t r = 0; r < pResult->GetMetadata().GetNbReplications() && msg; r++)
				{
					CNewSectionData section;
					pResult->GetSection(l, p, r, section);
					assert(section.GetCols() == pResult->GetNbCols(false));

					for (size_t t = 0; t < section.GetRows() && msg; t++)
					{
						std::array<double, NB_SBW_INPUTS> v;
						for (size_t i = 0; i < varsPos.size(); i++)
							v[i] = section[t][varsPos[i]][MEAN];

						if (v[I_YEAR] > -999 && v[I_MONTH] > -999 && v[I_DAY] > -999)
						{
							CTRef emergingDate = CTRef(int(v[I_YEAR]), size_t(v[I_MONTH]) - 1, size_t(v[I_DAY]) - 1);

							if (world.random().Randu() <= max_moth_prop)//remove moth by optimization
							{
								if (world.m_world_param.m_simulationPeriod.IsInside(emergingDate))
								{
									CSBWMoth moth(world);

									moth.m_ID = no;
									IDmap.push_back({ { l,p,rr } });

									moth.m_emergingDate = emergingDate;//daily reference of ready moths
									moth.m_sex = v[I_SEX];//sex (MALE=0, FEMALE=1)
									moth.m_A = v[I_A];
									moth.m_M = v[I_M];
									moth.m_G = v[I_G];
									moth.m_Fᵒ = v[I_Fᵒ];
									moth.m_Fᴰ = v[I_Fᴰ];
									moth.m_F = moth.m_Fᴰ;
									moth.m_location = locations[l];
									moth.m_pt = locations[l];

									if (extents.IsInside(moth.m_pt))
										world.m_moths.push_back(moth);
									else
										callback.AddMessage("WARNING: Simulation point outside elevation map");

									rr++;
									nbReplications = max(nbReplications, rr);
									no++;
								}
								else//is inside simulation period
								{
									if (emergingDate < world.m_world_param.m_simulationPeriod.Begin())
										moths_before++;
									else
										moths_after++;
								}

								world.m_seasonalIndividuals++;
							}

							sum_individual++;
						}//is valid insect
					}//for all rows

					msg += callback.StepIt();
				}//for all replications
			}//for all parameter set
		}//for all locations

		callback.PopTask();

		callback.AddMessage("Kept ratio: " + ToString(100.0*world.m_seasonalIndividuals/ sum_individual, 1) + " %");
		callback.AddMessage("Number of moths for the entire season: " + ToString(world.m_seasonalIndividuals) + " moths");
		callback.AddMessage("Number of moths before the period: " + to_string(moths_before) + " moths (" + ToString(100.0*moths_before / world.m_seasonalIndividuals, 2) + " %)");
		callback.AddMessage("Number of moths for the period: " + to_string(world.m_moths.size()) + " moths (" + ToString(100.0*world.m_moths.size() / world.m_seasonalIndividuals, 2) + " %)");
		callback.AddMessage("Number of moths after the period: " + to_string(moths_after) + " moths (" + ToString(100.0*moths_after / world.m_seasonalIndividuals, 2) + " %)");
		callback.AddMessage("Execute dispersal with: " + to_string(world.m_moths.size()) + " moths (" + ToString(100.0*world.m_moths.size() / world.m_seasonalIndividuals, 2) + " %)");
		callback.AddMessage("Weather period:         " + world.m_weather.GetEntireTPeriod().GetFormatedString());
		callback.AddMessage("Output period:          " + outputPeriod2.GetFormatedString());
		callback.AddMessage("Output replications (max moths/location):" + ToString(nbReplications));

		metadata.SetNbReplications(nbReplications);
		metadata.SetTPeriod(outputPeriod2);

		if (!world.m_moths.empty())
		{

			/*CATMOutputMatrix output(world.m_moths.size());
			for (size_t no = 0; no < output.size(); no++)
				output[no].Init(outputPeriod, VMISS);

			msg = world.Execute(output, sub_hourly_file, callback);
			if (msg)
			{
				callback.PushTask("Save data" , output.size() );

				ATMOutput missing_output;
				missing_output.Init(outputPeriod, VMISS);

				for (size_t l = 0; l < locations.size(); l++)
				{
					for (size_t p = 0; p < metadata.GetParameterSet().size(); p++)
					{
						for (size_t r = 0; r < nbReplications; r++)
						{
							size_t section = result.GetSectionNo(l, p, r);

							std::array<size_t, 3> lpr = { {l,p,r} };
							auto it = find(IDmap2.begin(), IDmap2.end(), lpr);
							if (it != IDmap2.end())
							{
								size_t no = distance(IDmap2.begin(), it);
								msg += result.SetSection(section, output[no]);
							}
							else
							{
								msg += result.SetSection(section, missing_output);

							}
							msg += callback.StepIt();
						}
					}
				}

				callback.PopTask();

				if (msg&& m_parameters.m_world.m_bCreateEggMaps)
				{
					string outputFilePath = fileManager.GetOutputMapPath() + m_parameters.m_world.m_eggMapsTitle + ".tif";
					msg = world.CreateEggDepositionMap(outputFilePath, output, callback);
				}
			}*/

			CStatistic::SetVMiss(-999);

			world.Init(callback);

			//world.write_sub_hourly_header(output_file);

			if (sub_hourly_file.is_open())
				world.write_sub_hourly_header(sub_hourly_file);

			//get period of simulation
			CTPeriod period = world.m_world_param.m_simulationPeriod;
			callback.PushTask("Execute dispersal for year = " + ToString(period.Begin().GetYear()) + " (" + ToString(period.GetNbDay()) + " days)", period.GetNbDay() * 2);
			callback.AddMessage("Date            NotEmerged       Emerging         WaitingToFly     Flying           FinishingEggs    Finished        ");

			//for all days
			for (CTRef TRef = period.Begin(); TRef <= period.End() && msg; TRef++)
			{
				CATMOutputMatrix output;
				world.init_output(TRef, output);

				CATMOutputMatrix sub_output;
				if (sub_hourly_file.is_open())
					world.init_sub_hourly(TRef, sub_output);	//initmemory

				msg += world.ExecuteOneNight(TRef, output, sub_output, callback);

				if (msg)
					msg += world.save_output(savedPeriod, output_file, output, callback);

				if (msg && sub_hourly_file.is_open())
				{
					msg += world.save_sub_output(TRef, sub_hourly_file, sub_output, callback);
				}//if sub hourly output

			}//for all valid days

			output_file.close();
			if (sub_hourly_file.is_open())
				sub_hourly_file.close();

			callback.PopTask();

			if (msg)
				msg += copy_result(DBFilePath + ".tmp", IDmap, savedPeriod, result, callback);

			if (msg&& m_parameters.m_world.m_bCreateEggMaps)
			{
				string inputFilePath = DBFilePath + ".tmp";
				string outputFilePath = fileManager.GetOutputMapPath() + m_parameters.m_world.m_eggMapsTitle + ".tif";
				msg = world.CreateEggDepositionMap(inputFilePath, outputFilePath, savedPeriod, callback);
			}

			WBSF::RemoveFile(DBFilePath + ".tmp");
		}

		result.Close();

		return msg;
	}

	ERMsg CDispersal::copy_result(const string& file_path, const std::vector<std::array<size_t, 3>>& IDmap, CTPeriod savedPeriod, CResult& result, CCallback& callback)
	{
		ERMsg msg;

		ifStream input_file;
		msg = input_file.open(file_path, ios_base::in | ios_base::binary);
		if (msg)
		{
			const size_t size_struct = sizeof(size_t) + savedPeriod.size()*(sizeof(__int32) + sizeof(float)*NB_ATM_OUTPUT);
			size_t length = input_file.length();
			ASSERT(length == IDmap.size()*size_struct);

			callback.PushTask("Save result", result.GetNbSection());

			for (size_t l = 0; l < result.GetDimension()[LOCATION] && msg; l++)
			{
				for (size_t p = 0; p < result.GetDimension()[PARAMETER] && msg; p++)
				{
					for (size_t r = 0; r < result.GetDimension()[REPLICATION] && msg; r++)
					{
						ATMOutput output;
						output.Init(savedPeriod, VMISS);

						std::array<size_t, 3> lpr = { { l,p,r } };
						auto it = find(IDmap.begin(), IDmap.end(), lpr);
						if (it != IDmap.end())
						{
							CTRef trefTest = savedPeriod.Begin();
							size_t no = input_file.read_value<size_t>();
							ASSERT(no < result.GetNbSection());

							for (size_t t = 0; t < savedPeriod.size() && msg; t++)
							{
								__int32 tmp = input_file.read_value<__int32 >();
								ASSERT(!input_file.eof());

								CTRef TRef;
								TRef.Set__int32(tmp);
								ASSERT(output.IsInside(TRef));
								ASSERT(TRef == trefTest);
								trefTest++;

								for (size_t v = 0; v < NB_ATM_OUTPUT; v++)
								{
									float value = input_file.read_value<float>();
									ASSERT(!input_file.eof());

									if (value > -999)
										output[TRef][v] = value;

									ASSERT(output[TRef][ATM_DEFOLIATION] != 0);
								}

							}
						}

						size_t section = result.GetSectionNo(l, p, r);
						msg += result.SetSection(section, output);

						msg += callback.StepIt();
					}//r
				}//p
			}//l

			input_file.close();
			callback.PopTask();
		}

		return msg;
	}

	size_t CDispersal::GetNbMoths(CResultPtr pResult)
	{
		size_t nbMoths = 0;
		const CModelOutputVariableDefVector& vars = pResult->GetMetadata().GetOutputDefinition();
		std::array<size_t, NB_SBW_INPUTS> varsPos;
		for (size_t i = 0; i < NB_SBW_INPUTS; i++)
			varsPos[i] = Find(vars, SBW_VARIABLE_NAME[i]);

		const CLocationVector& locations = pResult->GetMetadata().GetLocations();
		for (size_t l = 0; l < pResult->GetMetadata().GetLocations().size(); l++)
		{
			for (size_t p = 0; p < pResult->GetMetadata().GetParameterSet().size(); p++)
			{
				size_t rr = 0;
				for (size_t r = 0; r < pResult->GetMetadata().GetNbReplications(); r++)
				{
					CNewSectionData section;
					pResult->GetSection(l, p, r, section);
					assert(section.GetCols() == pResult->GetNbCols(false));

					for (size_t t = 0; t < section.GetRows(); t++)
					{
						std::array<double, NB_SBW_INPUTS> v;
						for (size_t i = 0; i < varsPos.size(); i++)
							v[i] = section[t][varsPos[i]][MEAN];

						if (v[I_YEAR] > -999 && v[I_MONTH] > -999 && v[I_DAY] > -999)
						{
							nbMoths++;
							CTRef emergingDate = CTRef(int(v[I_YEAR]), size_t(v[I_MONTH]) - 1, size_t(v[I_DAY]) - 1);

						}
					}//for all rows
				}//for all replications
			}//for all paramterset
		}//for all locations

		return nbMoths;
	}
}


/* Siconos is a program dedicated to modeling, simulation and control
 * of non smooth dynamical systems.
 *
 * Copyright 2018 INRIA.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
*/

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>

#include "SiconosVector.hpp"
#include "SimpleMatrix.hpp"
#include "BlockMatrixIterators.hpp"
#include "BlockMatrix.hpp"

#include "SiconosAlgebra.hpp"

using namespace Siconos;

//=============================
// Elements access (get or set)
//=============================

double SimpleMatrix::getValue(unsigned int row, unsigned int col) const
{
  if (row >= size(0) || col >= size(1))
    SiconosMatrixException::selfThrow("SimpleMatrix:getValue(index): Index out of range");

  if (_num == 1)
    return (*mat.Dense)(row, col);
  else if (_num == 2)
    return (*mat.Triang)(row, col);
  else if (_num == 3)
    return (*mat.Sym)(row, col);
  else if (_num == 4)
  {
    double * d = (*mat.Sparse).find_element(row, col);
    if (d)
      return *d;
    else
      return 0.0;
  }
  else if (_num == 5)
    return (*mat.Banded)(row, col);
  else if (_num == 6)
    return 0;
  else //if (_num==7)
    return(row == col);
}

void SimpleMatrix::setValue(unsigned int row, unsigned int col, double value)
{
  if (row >= size(0) || col >= size(1))
    SiconosMatrixException::selfThrow("SimpleMatrix:setValue: Index out of range");

  if (_num == 1)
    (*mat.Dense)(row, col) = value;
  else if (_num == 2)
    (*mat.Triang)(row, col) = value;
  else if (_num == 3)
    (*mat.Sym)(row, col) = value ;
  else if (_num == 4)
  {
    double * d = (*mat.Sparse).find_element(row, col);
    if (d)
    {
      *d = value;
    }
    else
    {
      (*mat.Sparse).insert_element(row, col, value);
    }
  }
  else if (_num == 5)
    (*mat.Banded)(row, col) = value;
  else if (_num == 6 || _num == 7)
    SiconosMatrixException::selfThrow("SimpleMatrix:setValue: forbidden for Identity or Zero type matrices.");
  resetLU();

}

//============================================
// Access (get or set) to blocks of elements
//============================================

void SimpleMatrix::setBlock(unsigned int row_min, unsigned int col_min, const SiconosMatrix& m)
{
  // Set current matrix elements, starting from row row_min and column col_min, with the values of the matrix m.
  // m may be a BlockMatrix.

  // Exceptions ...
  if (&m == this)
    SiconosMatrixException::selfThrow("SimpleMatrix::setBlock(pos,..., m): m = this.");

  if (row_min >= size(0))
    SiconosMatrixException::selfThrow("SimpleMatrix::setBlock(row,col): row is out of range");

  if (col_min >= size(1))
    SiconosMatrixException::selfThrow("SimpleMatrix::setBlock(row,col): col is out of range");

  unsigned int row_max, col_max;
  row_max = m.size(0) + row_min;
  col_max = m.size(1) + col_min;

  if (row_max > size(0))
    SiconosMatrixException::selfThrow("SimpleMatrix::setBlock(row,col,m): m.row + row is out of range.");

  if (col_max > size(1))
    SiconosMatrixException::selfThrow("SimpleMatrix::setBlock(row,col,m): m.col + col is out of range.");

  unsigned int numM = m.num();

  if (numM == 0) // if m is a block matrix ...
  {
    const BlockMatrix& mB = static_cast<const BlockMatrix&>(m);
    BlocksMat::const_iterator1 it;
    BlocksMat::const_iterator2 it2;
    unsigned int posRow = row_min;
    unsigned int posCol = col_min;

    for (it = mB._mat->begin1(); it != mB._mat->end1(); ++it)
    {
      for (it2 = it.begin(); it2 != it.end(); ++it2)
      {
        setBlock(posRow, posCol, **it2);
        posCol += (*it2)->size(1);
      }
      posRow += (*it)->size(0);
      posCol = col_min;
    }
  }
  else // if m is a SimpleMatrix
  {
    if (numM != _num)
      SiconosMatrixException::selfThrow("SimpleMatrix::setBlock(i,j,m), inconsistent types.");

    if (_num == 1)
      noalias(ublas::subrange(*mat.Dense, row_min, row_max, col_min, col_max)) = *(m.dense());
    else if (_num == 2)
      noalias(ublas::subrange(*mat.Triang, row_min, row_max, col_min, col_max)) = *(m.triang());
    else if (_num == 3)
      noalias(ublas::subrange(*mat.Sym, row_min, row_max, col_min, col_max)) = *(m.sym());
    else if (_num == 4)
      noalias(ublas::subrange(*mat.Sparse, row_min, row_max, col_min, col_max)) = *(m.sparse());
    else if (_num == 5)
      noalias(ublas::subrange(*mat.Banded, row_min, row_max, col_min, col_max)) = *(m.banded());
    else // if(_num==6) or _num == 7 nothing to do
    {}
    resetLU();
  }
}

void setBlock(SPC::SiconosMatrix  MIn, SP::SiconosMatrix MOut, const Index& dim, const Index& start)
{
  // To copy a subBlock of MIn into a subBlock of MOut.
  // dim[0], dim[1]: number of rows and columns of the sub-block
  // start[0], start[1]: position (row, column) of the first element of the subBlock in MIn
  // start[2], start[3]: position (row, column) of the first element of the subBlock in MOut

  if (MIn == MOut) // useless op => nothing to be done.
  {}// SiconosVectorException::selfThrow("");
  else
  {
    unsigned int numIn = MIn->num();
    unsigned int numOut = MOut->num();

    if (numOut == 6 || numOut == 7) // if MOut = 0 or Identity => read-only
      SiconosMatrixException::selfThrow("matrices, setBlock(MIn, MOut...): MOut is read-only (zero or identity matrix?).");

    // Check dimension
    Index MDim(4); // dim. of matrices MIn and MOut.
    MDim[0] = MIn->size(0);
    MDim[1] = MIn->size(1);
    MDim[2] = MOut->size(0);
    MDim[3] = MOut->size(1);

    for (unsigned int i = 0; i < 4 ; ++i)
      if (start[i] >= MDim[i])
        SiconosMatrixException::selfThrow("matrices, setBlock(MIn, ...): sub-block indices are out of range.");

    // index position of the last element in subBlock ...
    Index end(4);
    end[0] = dim[0] + start[0];
    end[1] = dim[1] + start[1];
    end[2] = dim[0] + start[2];
    end[3] = dim[1] + start[3];

    for (unsigned int i = 0; i < 4 ; ++i)
      if (end[i] > MDim[i])
        SiconosMatrixException::selfThrow("matrices, setBlock(MIn, ...): sub-block indices are out of range.");

    // Elements from row/col start[i] to row/col (end[i]-1) will be copied.

    // If both matrices MIn and MOut are block, exception.
    if (numIn == 0 && numOut == 0)
      SiconosMatrixException::selfThrow("matrices, setBlock(MIn, MOut ...): not yet implemented for MIn and MOut both BlockMatrix. Try to use setBlock on the sub-matrices?");

    else if (numOut == 0) // if MOut is a BlockMatrix.
    {

      // Steps:
      // A - Find the blocks of MOut that "own" indices start[2] and end[2] ie
      //     the first and last sub-block to be set in a block-column
      //         --> numbers blockStart0 and blockEnd0
      // B - Find the  Block of MOut that "owns" index start[3] and end[3] ie
      //     the first sub-block to be set in a block-row
      //         --> numbers blockStart1 and blockEnd1
      //
      //        => The blocks concerned in MOut, are those between (block) rows blockStart0 and blockEnd0
      //           and (block) columns blockStart1 and blockEnd1.
      //
      // C - Loop through the concerned blocks (name = currentBlock) of MOut and call setBlock(MIn, currentBlock, subSize, currentPos).
      //     subSize: dim. of the considered sub-block of currentBlock to be set
      //     currentPos: same as "start" vector but for currentBlock
      //

      // A - Block-Row position: we look for the block of MOut that include index start[2] and end[2].
      //
      unsigned int blockStart0 = 0;
      SPC::Index tab = MOut->tabRow();
      while (start[2] >= (*tab)[blockStart0] && blockStart0 < tab->size())
        blockStart0 ++;
      // Relative position in the block blockStart0 of the first element to be set.
      unsigned int posOut0 = start[2];
      if (blockStart0 != 0)
        posOut0 -= (*tab)[blockStart0 - 1];

      unsigned int blockEnd0 = blockStart0;
      while (end[2] > (*tab)[blockEnd0] && blockEnd0 < tab->size())
        blockEnd0 ++;

      // Size of the last sub-block in the column of block
      unsigned int lastBlockSize0 = end[2];
      if (blockEnd0 != 0)
        lastBlockSize0 -= (*tab)[blockEnd0 - 1];

      // B - Block-Col position: we look for the block of MOut that include index start[3] and end[3].
      unsigned int blockStart1 = 0;
      tab = MOut->tabCol();
      while (start[3] >= (*tab)[blockStart1] && blockStart1 < tab->size())
        blockStart1 ++;
      // Relative position in the block blockStart1 of the first element to be set.
      unsigned int posOut1 = start[3];
      if (blockStart1 != 0)
        posOut1 -= (*tab)[blockStart1 - 1];

      unsigned int blockEnd1 = blockStart1;
      while (end[3] > (*tab)[blockEnd1] && blockEnd1 < tab->size())
        blockEnd1 ++;

      // Size of the last sub-block in the row of block
      unsigned int lastBlockSize1 = end[3];
      if (blockEnd1 != 0)
        lastBlockSize1 -= (*tab)[blockEnd1 - 1];

      //C - Next, 3 steps for each row:
      // - set first sub-block in the row (number blockStart1)
      // - set all other blocks in the row except the last one
      // - set last block (number blockEnd1)
      // Same process for other rows ...

      // The current considered block
      SP::SiconosMatrix   currentBlock = MOut->block(blockStart0, blockStart1);

      // dim of the subBlock of currentBlock to be set.
      Index subSize(2);
      // indices of the first element of MIn (resp. currentBlock) to be read (resp. set)  (same as start for MIn and MOut).
      Index currentPos(4);

      // currentBlock position in MOut.
      unsigned int numRow = blockStart0;
      unsigned int numCol = blockStart1;

      // Init currentPos
      // row and col position for first element to be read in MIn,
      currentPos[0] = start[0];
      currentPos[1] = start[1];
      // row and col position for first element in sub-block of Mout (namely currentBlock).
      currentPos[2] = posOut0;
      currentPos[3] = posOut1;

      while (numRow != blockEnd0 + 1)
      {

        while (numCol != blockEnd1 + 1)
        {
          // Get the block of MOut from which a sub-block will be set ...
          currentBlock = MOut->block(numRow, numCol);

          // Set subSize[0], dim (rows) and subSize[1], dim (columns) of the sub-block.
          // subSize[0] is only required for the first block in the row, after it remains constant.
          subSize[1] = currentBlock->size(1);

          // Warning: test "a" must be done before test "b"
          if (numCol == blockEnd1) // if last column of blocks -> test "a"
            subSize[1] = lastBlockSize1;

          if (numCol == blockStart1) // -> test "b"
          {
            subSize[1] -= posOut1;
            subSize[0] = currentBlock->size(0);
            if (numRow == blockEnd0) // if last row of blocks
              subSize[0] = lastBlockSize0;
            if (numRow == blockStart0) // if first row of blocks
              subSize[0] -= posOut0;
          }

          // Set sub-block
          setBlock(MIn, currentBlock, subSize, currentPos);

          // Update currentPos:
          // col position for first element to be read in MIn,
          currentPos[1] += subSize[1] ;
          // col position for first element to be set in sub-block.
          currentPos[3] = 0;
          numCol++;
        }

        numCol = blockStart1;
        numRow++;

        // Update currentPos:
        // row position for first element to be read in MIn,
        currentPos[0] += subSize[0] ;
        // col position for first element to be read in MIn,
        currentPos[1] = start[1] ;
        // row position for first element to be set in sub-block.
        currentPos[2] = 0;
        // col position for first element to be set in sub-block.
        currentPos[3] = posOut1;

      }

    }
    else if (numIn == 0) // If MIn is a BlockMatrix.
    {

      // Same process as for numOut == 0

      unsigned int blockStart0 = 0;
      SPC::Index tab = MIn->tabRow();
      while (start[0] >= (*tab)[blockStart0] && blockStart0 < tab->size())
        blockStart0 ++;
      // Relative position in the block blockStart0 of the first element to be set.
      unsigned int posOut0 = start[0];
      if (blockStart0 != 0)
        posOut0 -= (*tab)[blockStart0 - 1];

      unsigned int blockEnd0 = blockStart0;
      while (end[0] > (*tab)[blockEnd0] && blockEnd0 < tab->size())
        blockEnd0 ++;

      // Size of the last sub-block in the column of block
      unsigned int lastBlockSize0 = end[0];
      if (blockEnd0 != 0)
        lastBlockSize0 -= (*tab)[blockEnd0 - 1];

      // B - Block-Col position: we look for the block of MOut that include index start[3] and end[3].
      unsigned int blockStart1 = 0;
      tab = MIn->tabCol();
      while (start[1] >= (*tab)[blockStart1] && blockStart1 < tab->size())
        blockStart1 ++;
      // Relative position in the block blockStart1 of the first element to be set.
      unsigned int posOut1 = start[1];
      if (blockStart1 != 0)
        posOut1 -= (*tab)[blockStart1 - 1];

      unsigned int blockEnd1 = blockStart1;
      while (end[1] > (*tab)[blockEnd1] && blockEnd1 < tab->size())
        blockEnd1 ++;

      // Size of the last sub-block in the row of block
      unsigned int lastBlockSize1 = end[1];
      if (blockEnd1 != 0)
        lastBlockSize1 -= (*tab)[blockEnd1 - 1];

      //C - Next, 3 steps for each row:
      // - set first sub-block in the row (number blockStart1)
      // - set all other blocks in the row except the last one
      // - set last block (number blockEnd1)
      // Same process for other rows ...

      // The current considered block
      SPC::SiconosMatrix  currentBlock = MIn->block(blockStart0, blockStart1);

      // dim of the subBlock of currentBlock to be set.
      Index subSize(2);
      // indices of the first element of MIn (resp. currentBlock) to be read (resp. set)  (same as start for MIn and MOut).
      Index currentPos(4);

      // currentBlock position in MOut.
      unsigned int numRow = blockStart0;
      unsigned int numCol = blockStart1;

      // Init currentPos
      // row and col position for first element to be read in MIn,
      currentPos[0] = posOut0;
      currentPos[1] = posOut1;
      // row and col position for first element in sub-block of Mout (namely currentBlock).
      currentPos[2] = start[2];
      currentPos[3] = start[3];

      while (numRow != blockEnd0 + 1)
      {

        while (numCol != blockEnd1 + 1)
        {
          // Get the block of MOut from which a sub-block will be set ...
          currentBlock = MIn->block(numRow, numCol);

          // Set subSize[0], dim (rows) and subSize[1], dim (columns) of the sub-block.
          // subSize[0] is only required for the first block in the row, after it remains constant.
          subSize[1] = currentBlock->size(1);
          // Warning: test "a" must be done before test "b"
          if (numCol == blockEnd1) // if last column of blocks -> test "a"
            subSize[1] = lastBlockSize1;

          if (numCol == blockStart1) // -> test "b"
          {
            subSize[1] -= posOut1;
            subSize[0] = currentBlock->size(0);
            if (numRow == blockEnd0) // if last row of blocks
              subSize[0] = lastBlockSize0;
            if (numRow == blockStart0) // if first row of blocks
              subSize[0] -= posOut0;
          }

          // Set sub-block
          setBlock(currentBlock, MOut, subSize, currentPos);

          // Update currentPos:
          // col position for first element to be read in MIn,
          currentPos[1] = 0 ;
          // col position for first element to be set in sub-block.
          currentPos[3] += subSize[1];
          numCol++;
        }

        numCol = blockStart1;
        numRow++;

        // Update currentPos:
        // row position for first element to be read in MIn,
        currentPos[0] = 0;
        // col position for first element to be read in MIn,
        currentPos[1] = posOut1;
        // row position for first element to be set in sub-block.
        currentPos[2] += subSize[0] ;
        // col position for first element to be set in sub-block.
        currentPos[3] = start[3];

      }
      MOut->resetLU();

    }
    else // neither MIn nor MOut is a BlockMatrix.
    {
      switch (numIn)
      {
      case 1:
        if (numOut != 1)
          SiconosMatrixException::selfThrow("matrix, setBlock(MIn, MOut, ...), unconsistent types between MIn and MOut.");
        noalias(ublas::subrange(*MOut->dense(), start[2], end[2], start[3], end[3])) = ublas::subrange(*MIn->dense(), start[0], end[0], start[1], end[1]);
        break;

      case 2:
        if (numOut != 1)
          SiconosMatrixException::selfThrow("matrix, setBlock(MIn, MOut, ...), unconsistent types between MIn and MOut.");
        noalias(ublas::subrange(*MOut->dense(), start[2], end[2], start[3], end[3])) = ublas::subrange(*MIn->triang(), start[0], end[0], start[1], end[1]);
        break;

      case 3:
        if (numOut != 1)
          SiconosMatrixException::selfThrow("matrix, setBlock(MIn, MOut, ...), unconsistent types between MIn and MOut.");
        noalias(ublas::subrange(*MOut->dense(), start[2], end[2], start[3], end[3])) = ublas::subrange(*MIn->sym(), start[0], end[0], start[1], end[1]);
        break;

      case 4:
        if (numOut == 1)
          noalias(ublas::subrange(*MOut->dense(), start[2], end[2], start[3], end[3])) = ublas::subrange(*MIn->sparse(), start[0], end[0], start[1], end[1]);
        else if (numOut == 4)
          noalias(ublas::subrange(*MOut->sparse(), start[2], end[2], start[3], end[3])) = ublas::subrange(*MIn->sparse(), start[0], end[0], start[1], end[1]);
        else
          SiconosMatrixException::selfThrow("matrix, setBlock(MIn, MOut, ...), unconsistent types between MIn and MOut.");
        break;

      case 5:
        if (numOut != 1)
          SiconosMatrixException::selfThrow("matrix, setBlock(MIn, MOut, ...), unconsistent types between MIn and MOut.");
        noalias(ublas::subrange(*MOut->dense(), start[2], end[2], start[3], end[3])) = ublas::subrange(*MIn->banded(), start[0], end[0], start[1], end[1]);
        break;

      case 6:
        if (numOut == 1)
          ublas::subrange(*MOut->dense(), start[2], end[2], start[3], end[3]) *= 0.0;
        else if (numOut == 2)
          ublas::subrange(*MOut->triang(), start[2], end[2], start[3], end[3]) *= 0.0;
        else if (numOut == 4)
          ublas::subrange(*MOut->sparse(), start[2], end[2], start[3], end[3]) *= 0.0;
        else if (numOut == 5)
          ublas::subrange(*MOut->banded(), start[2], end[2], start[3], end[3]) *= 0.0;
        else
          SiconosMatrixException::selfThrow("matrix, setBlock(MIn, MOut, ...), unconsistent types between MIn and MOut.");
        break;

      case 7:
        if (numOut == 1)
          noalias(ublas::subrange(*MOut->dense(), start[2], end[2], start[3], end[3])) = ublas::subrange(*MIn->identity(), start[0], end[0], start[1], end[1]);
        else if (numOut == 4)
          noalias(ublas::subrange(*MOut->sparse(), start[2], end[2], start[3], end[3])) = ublas::subrange(*MIn->identity(), start[0], end[0], start[1], end[1]);
        else
          SiconosMatrixException::selfThrow("matrix, setBlock(MIn, MOut, ...), unconsistent types between MIn and MOut.");
        break;

      default:
        SiconosMatrixException::selfThrow("matrix, setBlock(MIn, MOut, ...), unconsistent types between MIn and MOut.");
        break;
      }
      MOut->resetLU();
    }
  }
}

void SimpleMatrix::getRow(unsigned int r, SiconosVector &vOut) const
{
  // Get row number r of current matrix and copy it into vOut.
  if (r >= size(0))
    SiconosMatrixException::selfThrow("getRow(row): row is out of range");

  if (vOut.size() != size(1))
    SiconosMatrixException::selfThrow("getRow(row,v): inconsistent sizes between this and v.");

  if (_num == 7) // identity matrix
  {
    vOut.zero();
    vOut(r) = 1.0;
  }
  else if (_num == 6) // Zero matrix
    vOut.zero();
  else
  {
    unsigned int numV = vOut.num();
    if (numV == 1)
    {
      if (_num == 1)
      {
        noalias(*(vOut.dense())) = ublas::row(*mat.Dense, r);
      }
      else if (_num == 2)
      {
        noalias(*(vOut.dense())) = ublas::row(*mat.Triang, r);
      }
      else if (_num == 3)
      {
        noalias(*(vOut.dense())) = ublas::row(*mat.Sym, r);
      }
      else if (_num == 4)
      {
        noalias(*(vOut.dense())) = ublas::row(*mat.Sparse, r);
      }
      else //if(_num==5){
        noalias(*(vOut.dense())) = ublas::row(*mat.Banded, r);
    }
    else // if numV == 4
    {
      if (_num == 4)
      {
        noalias(*(vOut.sparse())) = ublas::row(*mat.Sparse, r);
      }
      else
        SiconosMatrixException::selfThrow("getRow(row,v): inconsistent types between this (not sparse) and v (sparse).");
    }
  }
}

void SimpleMatrix::setRow(unsigned int r, const SiconosVector& vIn)
{
  // Set row number r of current matrix with vIn.
  unsigned int numV = vIn.num();
  if (r >= size(0))
    SiconosMatrixException::selfThrow("setRow(row): row is out of range");

  if (vIn.size() != size(1))
    SiconosMatrixException::selfThrow("setRow(row,v): inconsistent sizes between this and v.");

  if (_num == 6 || _num == 7)
    SiconosMatrixException::selfThrow("setRow(row,v): current matrix is read-only (zero or identity).");

  {
    if (_num == 1)
    {
      if (numV == 1)
      {
        noalias(ublas::row(*mat.Dense, r)) = *vIn.dense();
      }
      else if (numV == 4)
      {
        noalias(ublas::row(*mat.Dense, r)) = *vIn.sparse();
      }
    }
    else if (_num == 4 && numV == 4)
      noalias(ublas::row(*mat.Sparse, r)) = *vIn.sparse();
    else
      SiconosMatrixException::selfThrow("setRow(row,v): inconsistent types between current matrix and v.");
  }

  resetLU();
}

void SimpleMatrix::getCol(unsigned int r, SiconosVector &vOut)const
{
  // Get column number r of current matrix and copy it into vOut.
  if (r >= size(1))
    SiconosMatrixException::selfThrow("getCol(col): col is out of range");

  if (vOut.size() != size(0))
    SiconosMatrixException::selfThrow("getCol(col,v): inconsistent sizes between this and v.");

  if (_num == 7) // identity matrix
  {
    vOut.zero();
    vOut(r) = 1.0;
  }
  else if (_num == 6) // Zero matrix
    vOut.zero();
  else
  {
    unsigned int numV = vOut.num();

    if (numV == 1)
    {

      if (_num == 1)
      {
        noalias(*(vOut.dense())) = ublas::column(*mat.Dense, r);
      }
      else if (_num == 2)
      {
        noalias(*(vOut.dense())) = ublas::column(*mat.Triang, r);
      }
      else if (_num == 3)
      {
        noalias(*(vOut.dense())) = ublas::column(*mat.Sym, r);
      }
      else if (_num == 4)
      {
        noalias(*(vOut.dense())) = ublas::column(*mat.Sparse, r);
      }
      else //if(_num==5){
        noalias(*(vOut.dense())) = ublas::column(*mat.Banded, r);
    }
    else // if _numV == 4
    {
      if (_num == 4)
      {
        noalias(*(vOut.sparse())) = ublas::column(*mat.Sparse, r);
      }
      else
        SiconosMatrixException::selfThrow("getCol(col,v): inconsistent types between this (not sparse) and v (sparse).");
    }
  }
}

void SimpleMatrix::setCol(unsigned int r, const SiconosVector &vIn)
{
  // Set column number r of current matrix with vIn.
  unsigned int numV = vIn.num();
  if (r >= size(1))
    SiconosMatrixException::selfThrow("setCol(col): col is out of range");

  if (vIn.size() != size(0))
    SiconosMatrixException::selfThrow("setCol(col,v): inconsistent sizes between this and v.");

  if (_num == 6 || _num == 7)
    SiconosMatrixException::selfThrow("setCol(col,v): current matrix is read-only (zero or identity).");

  {
    if (_num == 1)
    {
      if (numV == 1)
      {
        noalias(ublas::column(*mat.Dense, r)) = *vIn.dense();
      }
      else if (numV == 4)
      {
        noalias(ublas::column(*mat.Dense, r)) = *vIn.sparse();
      }
    }
    else if (_num == 4 && numV == 4)
      noalias(ublas::column(*mat.Sparse, r)) = *vIn.sparse();
    else
      SiconosMatrixException::selfThrow("setCol(col,v): inconsistent types between current matrix and v.");
  }

  resetLU();
}

void SimpleMatrix::getSubRow(unsigned int r, unsigned int pos, SP::SiconosVector vOut) const
{
  // Get row number r of current matrix, starting from element at position pos, and copy it into vOut.
  if (r >= size(0))
    SiconosMatrixException::selfThrow("getSubRow(row,pos,v): row is out of range");

  if (vOut->size() > size(1) - pos)
    SiconosMatrixException::selfThrow("getSubRow(row,pos,v): inconsistent sizes between this and v.");

  if (_num == 7) // identity matrix
  {
    vOut->zero();
    if (r >= pos)
      (*vOut)(r - pos) = 1.0;
  }
  else if (_num == 6) // Zero matrix
    vOut->zero();
  else
  {
    unsigned int numV = vOut->num();
    unsigned int nbEl = vOut->size();

    if (numV == 1)
    {
      if (_num == 1)
      {
        //      noalias(*(vOut->dense())) = ublas::row(ublas::subrange(*mat.Dense, r, r+1,pos, endPos),0);
        noalias(*(vOut->dense())) = ublas::matrix_vector_slice<DenseMat >(*mat.Dense, ublas::slice(r, 0, nbEl), ublas::slice(pos, 1, nbEl));
      }
      else if (_num == 2)
      {
        noalias(*(vOut->dense())) = ublas::matrix_vector_slice<TriangMat >(*mat.Triang, ublas::slice(r, 0, nbEl), ublas::slice(pos, 1, nbEl));
      }
      else if (_num == 3)
      {
        noalias(*(vOut->dense())) = ublas::matrix_vector_slice<SymMat >(*mat.Sym, ublas::slice(r, 0, nbEl), ublas::slice(pos, 1, nbEl));
      }
      else if (_num == 4)
      {
        // #ifdef BOOST_LIMITATION
        //         SiconosMatrixException("SimpleMatrix::getSubRow warning - ublas::matrix_vector_slice<SparseMat> does not exist for your boost distribution and your architecture.");
        // #else
        noalias(*(vOut->dense())) = ublas::matrix_vector_slice<SparseMat >(*mat.Sparse, ublas::slice(r, 0, nbEl), ublas::slice(pos, 1, nbEl));
        // #endif
      }
      else //if(_num==5){
        noalias(*(vOut->dense())) = ublas::matrix_vector_slice<BandedMat >(*mat.Banded, ublas::slice(r, 0, nbEl), ublas::slice(pos, 1, nbEl));
    }
    else // if numV == 4
    {
      if (_num == 4)
      {
#ifdef BOOST_LIMITATION
        SiconosMatrixException("SimpleMatrix::getSubRow warning - ublas::matrix_vector_slice<SparseMat> does not exist for your boost distribution and your architecture.");
#else
        noalias(*(vOut->sparse())) = ublas::matrix_vector_slice<SparseMat >(*mat.Sparse, ublas::slice(r, 0, nbEl), ublas::slice(pos, 1, nbEl));
#endif
      }
      else
        SiconosMatrixException::selfThrow("getSubRow(row,v): inconsistent types between this (not sparse) and v (sparse).");
    }
  }

}

void SimpleMatrix::setSubRow(unsigned int r, unsigned int pos, SP::SiconosVector vIn)
{
  // Set row number r, starting from element at position pos, of current matrix with vIn.
  unsigned int numV = vIn->num();
  if (r >= size(0))
    SiconosMatrixException::selfThrow("setSubRow(row): row is out of range");

  if (vIn->size() > size(1) - pos)
    SiconosMatrixException::selfThrow("setSubRow(row,v): inconsistent sizes between this and v.");

  if (_num == 6 || _num == 7)
    SiconosMatrixException::selfThrow("setSubRow(row,v): current matrix is read-only (zero or identity).");

  {
    unsigned int nbEl = vIn->size();
    if (_num == 1)
    {
      if (numV == 1)
      {
        noalias(ublas::matrix_vector_slice<DenseMat >(*mat.Dense, ublas::slice(r, 0, nbEl), ublas::slice(pos, 1, nbEl))) = *vIn->dense();
      }
      else if (numV == 4)
      {
        ublas::matrix_vector_slice<DenseMat >(*mat.Dense, ublas::slice(r, 0, nbEl), ublas::slice(pos, 1, nbEl)) = *vIn->sparse();
      }
    }
    else if (_num == 4 && numV == 4)
#ifdef BOOST_LIMITATION
      SiconosMatrixException("SimpleMatrix::setSubRow warning - ublas::matrix_vector_slice<SparseMat> does not exist for your boost distribution and your architecture.");
#else
      ublas::matrix_vector_slice<SparseMat >(*mat.Sparse, ublas::slice(r, 0, nbEl), ublas::slice(pos, 1, nbEl)) = *vIn->sparse();
#endif
    else
      SiconosMatrixException::selfThrow("setSubRow(row,v): inconsistent types between current matrix and v.");
    resetLU();
  }

}

void SimpleMatrix::getSubCol(unsigned int r, unsigned int pos, SP::SiconosVector vOut) const
{
  // Get col _number r of current matrix, starting from element at position pos, and copy it into vOut.
  if (r >= size(1))
    SiconosMatrixException::selfThrow("getSubCol(col,pos,v): col is out of range");

  if (vOut->size() > size(0) - pos)
    SiconosMatrixException::selfThrow("getSubCol(col,pos,v): inconsistent sizes between this and v.");

  if (_num == 7) // identity matrix
  {
    vOut->zero();
    if (r >= pos)
      (*vOut)(r - pos) = 1.0;
  }
  else if (_num == 6) // Zero matrix
    vOut->zero();
  else
  {
    unsigned int numV = vOut->num();
    unsigned int nbEl = vOut->size();

    if (numV == 1)
    {
      if (_num == 1)
      {
        //      noalias(*(vOut->dense())) = ublas::row(ublas::subrange(*mat.Dense, r, r+1,pos, endPos),0);
        noalias(*(vOut->dense())) = ublas::matrix_vector_slice<DenseMat >(*mat.Dense, ublas::slice(pos, 1, nbEl), ublas::slice(r, 0, nbEl));
      }
      else if (_num == 2)
      {
        noalias(*(vOut->dense())) = ublas::matrix_vector_slice<TriangMat >(*mat.Triang, ublas::slice(pos, 1, nbEl), ublas::slice(r, 0, nbEl));
      }
      else if (_num == 3)
      {
        noalias(*(vOut->dense())) = ublas::matrix_vector_slice<SymMat >(*mat.Sym, ublas::slice(pos, 1, nbEl), ublas::slice(r, 0, nbEl));
      }
      else if (_num == 4)
      {
#ifdef BOOST_LIMITATION
        SiconosMatrixException("SimpleMatrix::getSubCol warning - ublas::matrix_vector_slice<SparseMat> does not exist for your boost distribution and your architecture.");
#else
        noalias(*(vOut->dense())) = ublas::matrix_vector_slice<SparseMat >(*mat.Sparse, ublas::slice(pos, 1, nbEl), ublas::slice(r, 0, nbEl));
#endif
      }
      else //if(_num==5){
        noalias(*(vOut->dense())) = ublas::matrix_vector_slice<BandedMat >(*mat.Banded, ublas::slice(pos, 1, nbEl), ublas::slice(r, 0, nbEl));
    }
    else // if numV == 4
    {
      if (_num == 4)
      {
#ifdef BOOST_LIMITATION
        SiconosMatrixException("SimpleMatrix::getSubCol warning - ublas::matrix_vector_slice<SparseMat> does not exist for your boost distribution and your architecture.");
#else
        noalias(*(vOut->sparse())) = ublas::matrix_vector_slice<SparseMat >(*mat.Sparse, ublas::slice(pos, 1, nbEl), ublas::slice(r, 0, nbEl));
#endif
      }
      else
        SiconosMatrixException::selfThrow("getSubCol(col,v): inconsistent types between this (not sparse) and v (sparse).");
    }
  }

}

void SimpleMatrix::setSubCol(unsigned int r, unsigned int pos, SP::SiconosVector vIn)
{
  // Set column number r, starting from element at position pos, of current matrix with vIn.
  unsigned int numV = vIn->num();
  if (r >= size(1))
    SiconosMatrixException::selfThrow("setSubCol(col): col is out of range");

  if (vIn->size() > size(0) - pos)
    SiconosMatrixException::selfThrow("setSubCol(col,v): inconsistent sizes between this and v.");

  if (_num == 6 || _num == 7)
    SiconosMatrixException::selfThrow("setSubCol(col,v): current matrix is read-only (zero or identity).");

  {
    unsigned int nbEl = vIn->size();
    if (_num == 1)
    {
      if (numV == 1)
      {
        noalias(ublas::matrix_vector_slice<DenseMat >(*mat.Dense, ublas::slice(pos, 1, nbEl), ublas::slice(r, 0, nbEl))) = *vIn->dense();
      }
      else if (numV == 4)
      {
        ublas::matrix_vector_slice<DenseMat >(*mat.Dense, ublas::slice(pos, 1, nbEl), ublas::slice(r, 0, nbEl)) = *vIn->sparse();
      }
    }
    else if (_num == 4 && numV == 4)
#ifdef BOOST_LIMITATION
      SiconosMatrixException("SimpleMatrix::setSubCol warning - ublas::matrix_vector_slice<SparseMat> does not exist for your boost distribution and your architecture.");
#else
      ublas::matrix_vector_slice<SparseMat >(*mat.Sparse, ublas::slice(pos, 1, nbEl), ublas::slice(r, 0, nbEl)) = *vIn->sparse();
#endif
    else
      SiconosMatrixException::selfThrow("setSubCol(row,v): inconsistent types between current matrix and v.");
    resetLU();
  }
}


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

// Copyright (c) 2007-2011 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2011 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2011 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)

#ifndef BOOST_GEOMETRY_ALGORITHMS_CONVEX_HULL_HPP
#define BOOST_GEOMETRY_ALGORITHMS_CONVEX_HULL_HPP

#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/core/point_order.hpp>
#include <boost/geometry/core/exterior_ring.hpp>

#include <boost/geometry/geometries/concepts/check.hpp>

#include <boost/geometry/strategies/convex_hull.hpp>
#include <boost/geometry/strategies/concepts/convex_hull_concept.hpp>

#include <boost/geometry/views/detail/range_type.hpp>

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

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace convex_hull
{

template
<
    typename Geometry,
    order_selector Order,
    typename Strategy
>
struct hull_insert
{

    // Member template function (to avoid inconvenient declaration
    // of output-iterator-type, from hull_to_geometry)
    template <typename OutputIterator>
    static inline OutputIterator apply(Geometry const& geometry,
            OutputIterator out, Strategy const& strategy)
    {
        typename Strategy::state_type state;

        strategy.apply(geometry, state);
        strategy.result(state, out, Order == clockwise);
        return out;
    }
};

template
<
    typename Geometry,
    typename OutputGeometry,
    typename Strategy
>
struct hull_to_geometry
{
    static inline void apply(Geometry const& geometry, OutputGeometry& out,
            Strategy const& strategy)
    {
        hull_insert
            <
                Geometry,
                geometry::point_order<OutputGeometry>::value,
                Strategy
            >::apply(geometry,
                std::back_inserter(
                    // Handle linestring, ring and polygon the same:
                    detail::as_range
                        <
                            typename range_type<OutputGeometry>::type
                        >(out)), strategy);
    }
};

}} // namespace detail::convex_hull
#endif // DOXYGEN_NO_DETAIL

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

template
<
    typename Tag1,
    typename Geometry,
    typename Output,
    typename Strategy
>
struct convex_hull
    : detail::convex_hull::hull_to_geometry<Geometry, Output, Strategy>
{};

template
<
    typename GeometryTag,
    order_selector Order,
    typename GeometryIn, typename Strategy
 >
struct convex_hull_insert
    : detail::convex_hull::hull_insert<GeometryIn, Order, Strategy>
{};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

template<typename Geometry1, typename Geometry2, typename Strategy>
inline void convex_hull(Geometry1 const& geometry,
            Geometry2& out, Strategy const& strategy)
{
    concept::check_concepts_and_equal_dimensions
        <
            const Geometry1,
            Geometry2
        >();

    BOOST_CONCEPT_ASSERT( (geometry::concept::ConvexHullStrategy<Strategy>) );

    dispatch::convex_hull
        <
            typename tag<Geometry1>::type,
            Geometry1,
            Geometry2,
            Strategy
        >::apply(geometry, out, strategy);
}

/*!
\brief \brief_calc{convex hull}
\ingroup convex_hull
\details \details_calc{convex_hull,convex hull}.
\tparam Geometry1 \tparam_geometry
\tparam Geometry2 \tparam_geometry
\param geometry \param_geometry,  input geometry
\param hull \param_geometry \param_set{convex hull}

\qbk{[include reference/algorithms/convex_hull.qbk]}
 */
template<typename Geometry1, typename Geometry2>
inline void convex_hull(Geometry1 const& geometry,
            Geometry2& hull)
{
    concept::check_concepts_and_equal_dimensions
        <
            const Geometry1,
            Geometry2
        >();

    typedef typename point_type<Geometry2>::type point_type;

    typedef typename strategy_convex_hull
        <
            typename cs_tag<point_type>::type,
            Geometry1,
            point_type
        >::type strategy_type;

    convex_hull(geometry, hull, strategy_type());
}

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace convex_hull
{

template<typename Geometry, typename OutputIterator, typename Strategy>
inline OutputIterator convex_hull_insert(Geometry const& geometry,
            OutputIterator out, Strategy const& strategy)
{
    // Concept: output point type = point type of input geometry
    concept::check<Geometry const>();
    concept::check<typename point_type<Geometry>::type>();

    BOOST_CONCEPT_ASSERT( (geometry::concept::ConvexHullStrategy<Strategy>) );

    return dispatch::convex_hull_insert
        <
            typename tag<Geometry>::type,
            geometry::point_order<Geometry>::value,
            Geometry, Strategy
        >::apply(geometry, out, strategy);
}

/*!
\brief Calculate the convex hull of a geometry, output-iterator version
\ingroup convex_hull
\tparam Geometry the input geometry type
\tparam OutputIterator: an output-iterator
\param geometry the geometry to calculate convex hull from
\param out an output iterator outputing points of the convex hull
\note This overloaded version outputs to an output iterator.
In this case, nothing is known about its point-type or
    about its clockwise order. Therefore, the input point-type
    and order are copied

 */
template<typename Geometry, typename OutputIterator>
inline OutputIterator convex_hull_insert(Geometry const& geometry,
            OutputIterator out)
{
    // Concept: output point type = point type of input geometry
    concept::check<Geometry const>();
    concept::check<typename point_type<Geometry>::type>();

    typedef typename point_type<Geometry>::type point_type;

    typedef typename strategy_convex_hull
        <
            typename cs_tag<point_type>::type,
            Geometry,
            point_type
        >::type strategy_type;

    return convex_hull_insert(geometry, out, strategy_type());
}

}} // namespace detail::convex_hull
#endif // DOXYGEN_NO_DETAIL

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_CONVEX_HULL_HPP


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2018-2020, LAAS-CNRS, University of Edinburgh
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////

#ifndef CROCODDYL_CORE_DIFF_ACTION_BASE_HPP_
#define CROCODDYL_CORE_DIFF_ACTION_BASE_HPP_

#include <stdexcept>
#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>

#include "crocoddyl/core/fwd.hpp"
#include "crocoddyl/core/state-base.hpp"
#include "crocoddyl/core/utils/to-string.hpp"
#include "crocoddyl/core/utils/math.hpp"

namespace crocoddyl {

/**
 * @brief Abstract class for differential action model
 *
 * A differential action model describes a time-continuous action model with a first-order ODE, i.e.
 * \f[
 * \dot{\mathbf{v}} = \mathbf{f}(\mathbf{q}, \mathbf{v}, \mathbf{u})
 * \f]
 * where the configuration \f$\mathbf{q}\in\mathcal{Q}\f$ lies in the configuration manifold described with a
 * `nq`-tuple, the velocity \f$\mathbf{v}\in T_{\mathbf{q}}\mathcal{Q}\f$ its a tangent vector to this manifold with
 * `nv` dimension, and \f$\mathbf{u}\in\mathbb{R}^{nu}\f$ is the input commands. Both, configuration and velocity,
 * describes the system space \f$\mathbf{x}\in\mathbf{X}\f$ which lies in the state manifold. Note that the
 * acceleration \f$\dot{\mathbf{v}}\in T_{\mathbf{q}}\mathcal{Q}\f$ lies also in the tangent space of the configuration
 * manifold.
 *
 * `calc` computes the acceleration and cost and `calcDiff` computes the derivatives of the dynamics and cost function.
 * Concretely speaking, `calcDiff` builds a linear-quadratic approximation of the differential action, where the
 * dynamics and cost functions have linear and quadratic forms, respectively. \f$\mathbf{f_x}\in\mathbb{R}^{nv\times
 * ndx}\f$, \f$\mathbf{f_u}\in\mathbb{R}^{nv\times nu}\f$ are the Jacobians of the dynamics;
 * \f$\mathbf{l_x}\in\mathbb{R}^{ndx}\f$, \f$\mathbf{l_u}\in\mathbb{R}^{nu}\f$,
 * \f$\mathbf{l_{xx}}\in\mathbb{R}^{ndx\times ndx}\f$, \f$\mathbf{l_{xu}}\in\mathbb{R}^{ndx\times nu}\f$,
 * \f$\mathbf{l_{uu}}\in\mathbb{R}^{nu\times nu}\f$ are the Jacobians and Hessians of the cost function, respectely.
 *
 * \sa `calc()`, `calcDiff()`, `createData()`
 */
template <typename _Scalar>
class DifferentialActionModelAbstractTpl {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  typedef _Scalar Scalar;
  typedef MathBaseTpl<Scalar> MathBase;
  typedef DifferentialActionDataAbstractTpl<Scalar> DifferentialActionDataAbstract;
  typedef StateAbstractTpl<Scalar> StateAbstract;
  typedef typename MathBase::VectorXs VectorXs;
  typedef typename MathBase::MatrixXs MatrixXs;

  /**
   * @brief Initialize the differential action model
   *
   * @param[in] state  State Dimension of state configuration tuple
   * @param[in] nu     Dimension of control vector
   * @param[in] nr     Dimension of cost-residual vector
   */
  DifferentialActionModelAbstractTpl(boost::shared_ptr<StateAbstract> state, const std::size_t& nu,
                                     const std::size_t& nr = 0);
  virtual ~DifferentialActionModelAbstractTpl();

  /**
   * @brief Compute the system acceleration and cost value
   *
   * @param[in] data  Differential action data
   * @param[in] x     State point
   * @param[in] u     Control input
   */
  virtual void calc(const boost::shared_ptr<DifferentialActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
                    const Eigen::Ref<const VectorXs>& u) = 0;

  /**
   * @brief Compute the derivatives of the dynamics and cost functions
   *
   * It computes the partial derivatives of the dynamical system and the cost function. It assumes that calc has been
   * run first. This function builds a quadratic approximation of the time-continuous action model (i.e. dynamical
   * system and cost function).
   *
   * @param[in] data  Differential action data
   * @param[in] x     State point
   * @param[in] u     Control input
   */
  virtual void calcDiff(const boost::shared_ptr<DifferentialActionDataAbstract>& data,
                        const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) = 0;

  /**
   * @brief Create the differential action data
   *
   * @return the differential action data
   */
  virtual boost::shared_ptr<DifferentialActionDataAbstract> createData();

  /**
   * @brief Checks that a specific data belongs to this model
   */
  virtual bool checkData(const boost::shared_ptr<DifferentialActionDataAbstract>& data);

  /**
   * @copybrief calc()
   *
   * @param[in] data  Differential action data
   * @param[in] x     State point
   */
  void calc(const boost::shared_ptr<DifferentialActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);

  /**
   * @copybrief calcDiff()
   *
   * @param[in] data  Differential action data
   * @param[in] x     State point
   */
  void calcDiff(const boost::shared_ptr<DifferentialActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);

  /**
   * @brief Computes the quasic static commands
   *
   * The quasic static commands are the ones produced for a the reference posture as an equilibrium point, i.e.
   * for \f$\mathbf{f(q,v=0,u)=0}\f$
   *
   * @param[in] data    Differential action data
   * @param[out] u      Quasic static commands
   * @param[in] x       State point (velocity has to be zero)
   * @param[in] maxiter Maximum allowed number of iterations
   * @param[in] tol     Tolerance
   */
  virtual void quasiStatic(const boost::shared_ptr<DifferentialActionDataAbstract>& data, Eigen::Ref<VectorXs> u,
                           const Eigen::Ref<const VectorXs>& x, const std::size_t& maxiter = 100,
                           const Scalar& tol = Scalar(1e-9));

  /**
   * @copybrief quasicStatic()
   *
   * @copydetails quasicStatic()
   *
   * @param[in] data    Differential action data
   * @param[in] x       State point (velocity has to be zero)
   * @param[in] maxiter Maximum allowed number of iterations
   * @param[in] tol     Tolerance
   * @return Quasic static commands
   */
  VectorXs quasiStatic_x(const boost::shared_ptr<DifferentialActionDataAbstract>& data, const VectorXs& x,
                         const std::size_t& maxiter = 100, const Scalar& tol = Scalar(1e-9));

  /**
   * @brief Return the dimension of the control input
   */
  const std::size_t& get_nu() const;

  /**
   * @brief Return the dimension of the cost-residual vector
   */
  const std::size_t& get_nr() const;

  /**
   * @brief Return the state
   */
  const boost::shared_ptr<StateAbstract>& get_state() const;

  /**
   * @brief Return the control lower bound
   */
  const VectorXs& get_u_lb() const;

  /**
   * @brief Return the control upper bound
   */
  const VectorXs& get_u_ub() const;

  /**
   * @brief Indicates if there are defined control limits
   */
  bool const& get_has_control_limits() const;

  /**
   * @brief Modify the control lower bounds
   */
  void set_u_lb(const VectorXs& u_lb);

  /**
   * @brief Modify the control upper bounds
   */
  void set_u_ub(const VectorXs& u_ub);

 protected:
  std::size_t nu_;                          //!< Control dimension
  std::size_t nr_;                          //!< Dimension of the cost residual
  boost::shared_ptr<StateAbstract> state_;  //!< Model of the state
  VectorXs unone_;                          //!< Neutral state
  VectorXs u_lb_;                           //!< Lower control limits
  VectorXs u_ub_;                           //!< Upper control limits
  bool has_control_limits_;                 //!< Indicates whether any of the control limits is finite

  void update_has_control_limits();
};

template <typename _Scalar>
struct DifferentialActionDataAbstractTpl {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  typedef _Scalar Scalar;
  typedef MathBaseTpl<Scalar> MathBase;
  typedef typename MathBase::VectorXs VectorXs;
  typedef typename MathBase::MatrixXs MatrixXs;

  template <template <typename Scalar> class Model>
  explicit DifferentialActionDataAbstractTpl(Model<Scalar>* const model)
      : cost(0.),
        xout(model->get_state()->get_nv()),
        Fx(model->get_state()->get_nv(), model->get_state()->get_ndx()),
        Fu(model->get_state()->get_nv(), model->get_nu()),
        r(model->get_nr()),
        Lx(model->get_state()->get_ndx()),
        Lu(model->get_nu()),
        Lxx(model->get_state()->get_ndx(), model->get_state()->get_ndx()),
        Lxu(model->get_state()->get_ndx(), model->get_nu()),
        Luu(model->get_nu(), model->get_nu()) {
    xout.setZero();
    r.setZero();
    Fx.setZero();
    Fu.setZero();
    Lx.setZero();
    Lu.setZero();
    Lxx.setZero();
    Lxu.setZero();
    Luu.setZero();
  }
  virtual ~DifferentialActionDataAbstractTpl() {}

  Scalar cost;
  VectorXs xout;
  MatrixXs Fx;
  MatrixXs Fu;
  VectorXs r;
  VectorXs Lx;
  VectorXs Lu;
  MatrixXs Lxx;
  MatrixXs Lxu;
  MatrixXs Luu;
};

}  // namespace crocoddyl

/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
#include "crocoddyl/core/diff-action-base.hxx"

#endif  // CROCODDYL_CORE_DIFF_ACTION_BASE_HPP_


#include "blocks.h"

#include <algorithm>
#include <string>
#include <vector>
#include <boost/pending/disjoint_sets.hpp>

namespace py = pybind11;

namespace {

const char* blockwise_means_doc = R"(blockwise_means(blocks, input)

Average the elements of the given vector within each block.

Specifically, the coordinate ``i`` of the returned vector will contain the mean
of all entries ``j`` in ``input`` that have ``blocks[j]=b``.

Arguments
---------
blocks : numpy.ndarray
    A vector of ints that denote the block memberships of each position.

    The set of numbers in this vector should start at zero and be consecutive.

input : numpy.ndarray
    A vector of same size as ``block`` containing the data to be averaged.

Returns
--------
numpy.ndarray
    A vector of same size as the inputs that contains the block-wise averages.
)";

}  // namespace

py::array_t<float> blockwise_means(const py::array_t<int>& blocks_,
                                   const py::array_t<float>& input_) {
    if (blocks_.ndim() != 1 || input_.ndim() != 1) {
        throw std::runtime_error("the given arrays must be one dimensional");
    }
    if (blocks_.shape(0) != input_.shape(0)) {
        throw std::runtime_error("the number of elements in the must match");
    }

    auto blocks = blocks_.unchecked<1>();
    auto input = input_.unchecked<1>();

    int n_blocks = 0;
    for (int i = 0; i < blocks.shape(0); i++) {
        if (blocks[i] < 0) {
            throw std::runtime_error("the block ids must be non-negative");
        }
        n_blocks = std::max(n_blocks, 1 + blocks(i));
    }

    std::vector<float> blocks_sums(n_blocks);
    std::vector<int> blocks_total(n_blocks);

    for (int i = 0; i < blocks.shape(0); i++) {
        blocks_sums[blocks(i)] += input[i];
        ++blocks_total[blocks[i]];
    }
    for (int i = 0; i < n_blocks; i++) {
        blocks_sums[i] /= static_cast<float>(blocks_total[i]);
    }

    py::array_t<float> output_ = py::array_t<float>(blocks.shape(0));
    auto output = output_.mutable_unchecked<1>();
    for (int i = 0; i < output_.shape(0); i++) {
        output(i) = blocks_sums[blocks[i]];
    }
    return output_;
}

namespace {

const char* blocks_2d_doc = R"(blocks_2d(matrix)

Return the connected components of the matrix.

Two positions are connected iff they hold the same value, and they differ in
one coordinate by one (i.e., it is a 4-connected grid).

Arguments
---------
matrix : numpy.ndarray
    The two-dimensional matrix.

Returns
--------
numpy.ndarray
    A matrix of ints, same size as the input.

    The positions corresponding to the same connected component have the
    same label. The labels are consecutive integers starting at zero.
)";

}  // namespace

py::array_t<int> blocks_2d(const py::array_t<double>& matrix_) {
    if (matrix_.ndim() != 2) {
        throw std::runtime_error("the given matrix must be two dimensional");
    }
    auto matrix = matrix_.unchecked<2>();

    std::vector<int> ranks(matrix.size());
    std::vector<int> parents(matrix.size());
    boost::disjoint_sets<int*, int*> union_find(&ranks[0], &parents[0]);

    #define IDX(i, j) ((i) * static_cast<int>(matrix.shape(1)) + (j))

    for (int i = 0; i < matrix_.shape(0); i++) {
        for (int j = 0; j < matrix_.shape(1); j++) {
            int idx = IDX(i, j);
            union_find.make_set(idx);
            if (i > 0 && matrix(i, j) == matrix(i - 1, j)) {
                union_find.union_set(idx, IDX(i - 1, j));
            }
            if (j > 0 && matrix(i, j) == matrix(i, j - 1)) {
                union_find.union_set(idx, IDX(i, j - 1));
            }
        }
    }

    std::unordered_map<int, int> root_to_idx;
    py::array_t<int> output_ = py::array_t<int>({matrix_.shape(0),
                                                 matrix_.shape(1)});
    auto output = output_.mutable_unchecked<2>();
    int next_id = 0;
    for (int i = 0; i < matrix.shape(0); i++) {
        for (int j = 0; j < matrix.shape(1); j++) {
            int idx = IDX(i, j);
            int root = union_find.find_set(idx);
            auto iter = root_to_idx.find(root);
            if (iter == root_to_idx.end()) {
                output(i, j) = next_id;
                root_to_idx[root] = next_id++;
            } else {
                output(i, j) = iter->second;
            }
        }
    }

    return output_;
}

PYBIND11_MODULE(blocks, m) {
    py::options options;
    options.disable_function_signatures();
    m.def("blockwise_means", blockwise_means, blockwise_means_doc);
    m.def("blocks_2d", blocks_2d, blocks_2d_doc);
}


// Filename: CirculationModel_with_lv.cpp
// Created on 20 Aug 2007 by Boyce Griffith

// Modified 2019, Alexander D. Kaiser

#include "CirculationModel_with_lv.h"
#include "pnpoly.h"
/////////////////////////////// INCLUDES /////////////////////////////////////

#ifndef included_IBAMR_config
#include <IBAMR_config.h>
#define included_IBAMR_config
#endif

#ifndef included_SAMRAI_config
#include <SAMRAI_config.h>
#define included_SAMRAI_config
#endif

// SAMRAI INCLUDES
#include <CartesianGridGeometry.h>
#include <CartesianPatchGeometry.h>
#include <PatchLevel.h>
#include <SideData.h>
#include <tbox/RestartManager.h>
#include <tbox/SAMRAI_MPI.h>
#include <tbox/Utilities.h>

// C++ STDLIB INCLUDES
#include <cassert>

#include <Eigen/Dense>
using namespace Eigen;

namespace
{
// Name of output file.
static const string DATA_FILE_NAME = "bc_data.m";

}

/////////////////////////////// NAMESPACE ////////////////////////////////////

/////////////////////////////// STATIC ///////////////////////////////////////

/////////////////////////////// PUBLIC ///////////////////////////////////////

CirculationModel_with_lv::CirculationModel_with_lv(const fourier_series_data *fourier_aorta,
                                                   const fourier_series_data *fourier_atrium,
                                                   const fourier_series_data *fourier_ventricle,
                                                   string aorta_vertices_file_name,
                                                   string atrium_vertices_file_name,
                                                   const double  cycle_duration,
                                                   const double  t_offset_bcs_unscaled,
                                                   const double  initial_time)
    :
      d_object_name("circ_model_with_lv"),  // constant name here
      d_registered_for_restart(true),      // always true
      d_fourier_aorta (fourier_aorta),
      d_fourier_atrium(fourier_atrium),
      d_fourier_ventricle(fourier_ventricle),
      d_cycle_duration(cycle_duration),
      d_t_offset_bcs_unscaled(t_offset_bcs_unscaled),
      d_current_idx_series(0),
      d_Q_aorta      (0.0),
      d_Q_left_atrium(0.0),
      d_Q_mitral     (0.0),
      d_time(initial_time),
      d_area_aorta(0.0),
      d_area_atrium(0.0),
      d_area_initialized(false)
{

    if (d_registered_for_restart)
    {
        RestartManager::getManager()->registerRestartItem(d_object_name, this);
    }

    // Initialize object with data read from the input and restart databases.
    const bool from_restart = RestartManager::getManager()->isFromRestart();
    if (from_restart)
    {
        getFromRestart();
    }

    // read aorta and atrium vertices from file
    ifstream aorta_file(aorta_vertices_file_name.c_str(), ios::in);

    if(!aorta_file){
        TBOX_ERROR("Aorta file not found\n");
    }

    aorta_file >> d_n_pts_aorta;

    d_aorta_points_x = new double[d_n_pts_aorta];
    d_aorta_points_y = new double[d_n_pts_aorta];

    double z,z_prev;
    double tol = 1.0e-2;

    for (int i=0; i<d_n_pts_aorta; i++){
        aorta_file >> d_aorta_points_x[i];
        aorta_file >> d_aorta_points_y[i];
        aorta_file >> z;

        if (i>0){
            if (fabs(z_prev - z) > tol){
                TBOX_ERROR("Z coordinates must be consistent\n");
            }
        }
        z_prev = z;

    }
    pout << "to aorta file close\n";
    aorta_file.close();

    ifstream atrium_file(atrium_vertices_file_name.c_str(), ios::in);
    if(!atrium_file){
        TBOX_ERROR("Atrium file not found\n");
    }
    atrium_file >> d_n_pts_atrium;

    d_atrium_points_x = new double[d_n_pts_atrium];
    d_atrium_points_y = new double[d_n_pts_atrium];

    for (int i=0; i<d_n_pts_atrium; i++){
        atrium_file >> d_atrium_points_x[i];
        atrium_file >> d_atrium_points_y[i];
        atrium_file >> z;

        if (i>0){
            if (fabs(z_prev - z) > tol){
                TBOX_ERROR("Z coordinates must be consistent\n");
            }
        }
        z_prev = z;

    }
    atrium_file.close();

    pout << "passed contstructor\n";

    return;
} // CirculationModel

CirculationModel_with_lv::~CirculationModel_with_lv()
{
    return;
} // ~CirculationModel_with_lv

void CirculationModel_with_lv::advanceTimeDependentData(const double dt,
                                                        const Pointer<PatchHierarchy<NDIM> > hierarchy,
                                                        const int U_idx,
                                                        const int /*P_idx*/,
                                                        const int /*wgt_cc_idx*/,
                                                        const int wgt_sc_idx)
{
    // Compute the mean flow rates in the vicinity of the inflow and outflow
    // boundaries.

    double Q_aorta_local = 0.0;
    double Q_left_atrium_local = 0.0;

    double area_aorta_local = 0.0;
    double area_atrium_local = 0.0;

    for (int ln = 0; ln <= hierarchy->getFinestLevelNumber(); ++ln)
    {
        Pointer<PatchLevel<NDIM> > level = hierarchy->getPatchLevel(ln);
        for (PatchLevel<NDIM>::Iterator p(level); p; p++)
        {
            Pointer<Patch<NDIM> > patch = level->getPatch(p());
            Pointer<CartesianPatchGeometry<NDIM> > pgeom = patch->getPatchGeometry();
            if (pgeom->getTouchesRegularBoundary())
            {
                Pointer<SideData<NDIM, double> > U_data = patch->getPatchData(U_idx);
                Pointer<SideData<NDIM, double> > wgt_sc_data = patch->getPatchData(wgt_sc_idx);
                const Box<NDIM>& patch_box = patch->getBox();
                const double* const x_lower = pgeom->getXLower();
                const double* const dx = pgeom->getDx();
                double dV = 1.0;
                for (int d = 0; d < NDIM; ++d)
                {
                    dV *= dx[d];
                }

                // always looking for z flux here
                // side is always 1, top of box
                static const int axis = 2;
                int side = 1;
                const bool is_lower = (side == 0);
                if (pgeom->getTouchesRegularBoundary(axis, side))
                {

                    Vector n;
                    for (int d = 0; d < NDIM; ++d)
                    {
                        n[d] = axis == d ? (is_lower ? -1.0 : +1.0) : 0.0;
                    }
                    Box<NDIM> side_box = patch_box;
                    if (is_lower)
                    {
                        side_box.lower(axis) = patch_box.lower(axis);
                        side_box.upper(axis) = patch_box.lower(axis);
                    }
                    else
                    {
                        side_box.lower(axis) = patch_box.upper(axis) + 1;
                        side_box.upper(axis) = patch_box.upper(axis) + 1;
                    }
                    for (Box<NDIM>::Iterator b(side_box); b; b++)
                    {
                        const Index<NDIM>& i = b();

                        double X[NDIM];
                        for (int d = 0; d < NDIM; ++d)
                        {
                            X[d] = x_lower[d] + dx[d] * (double(i(d) - patch_box.lower(d)) + (d == axis ? 0.0 : 0.5));
                        }

                        const int in_aorta  = this->point_in_aorta (X[0],X[1]);
                        const int in_atrium = this->point_in_atrium(X[0],X[1]);

                        if (in_aorta && in_atrium){
                            TBOX_ERROR("Position is within both aorta and atrium, should be impossible\n");
                        }

                        if (in_aorta)
                        {
                            const SideIndex<NDIM> i_s(i, axis, SideIndex<NDIM>::Lower);
                            if ((*wgt_sc_data)(i_s) > std::numeric_limits<double>::epsilon())
                            {
                                double dA = n[axis] * dV / dx[axis];
                                Q_aorta_local += (*U_data)(i_s)*dA;

                                if (!d_area_initialized){
                                    area_aorta_local += dA;
                                }

                            }
                        }

                        if (in_atrium)
                        {
                            const SideIndex<NDIM> i_s(i, axis, SideIndex<NDIM>::Lower);
                            if ((*wgt_sc_data)(i_s) > std::numeric_limits<double>::epsilon())
                            {
                                double dA = n[axis] * dV / dx[axis];
                                Q_left_atrium_local += (*U_data)(i_s)*dA;

                                if (!d_area_initialized){
                                    area_atrium_local += dA;
                                }
                            }
                        }
                    }
                }

            }
        }
    }

    d_Q_aorta       = SAMRAI_MPI::sumReduction(Q_aorta_local);
    d_Q_left_atrium = SAMRAI_MPI::sumReduction(Q_left_atrium_local);

    if (!d_area_initialized){
        d_area_aorta  = SAMRAI_MPI::sumReduction(area_aorta_local);
        d_area_atrium = SAMRAI_MPI::sumReduction(area_atrium_local);
        d_area_initialized = true;
    }

    d_time += dt;

    // compute which index in the Fourier series we need here
    // always use a time in current cycle
    double t_reduced = d_time - d_cycle_duration * floor(d_time/d_cycle_duration);

    // fourier series has its own period, scale to that
    double t_scaled = t_reduced * (d_fourier_aorta->L  / d_cycle_duration);

    // start offset some arbitrary time in the cardiac cycle, but this is relative to the series length
    double t_scaled_offset = t_scaled + d_t_offset_bcs_unscaled;

    // Fourier data here
    // index without periodicity
    unsigned int k = (unsigned int) floor(t_scaled_offset / (d_fourier_aorta->dt));

    // // take periodic reduction
    d_current_idx_series = k % (d_fourier_aorta->N_times);

    // bool debug_out = false;
    // if (debug_out){
    //     pout << "circ mode: d_time = " << d_time << ", d_current_idx_series = " << d_current_idx_series << "\n";
    //     pout << "t_reduced = " << t_reduced << " t_scaled = " << t_scaled << " t_scaled_offset = " << t_scaled_offset << "\n";
    //     pout << "k (unreduced idx) = " << k << " d_current_idx_series = " << d_current_idx_series << "\n\n";
    // }

    writeDataFile();

} // advanceTimeDependentData

void CirculationModel_with_lv::set_Q_mitral(double Q_mitral){
    d_Q_mitral = Q_mitral;
}

void
CirculationModel_with_lv::putToDatabase(Pointer<Database> db)
{

    db->putInteger("d_current_idx_series", d_current_idx_series);
    db->putDouble("d_Q_aorta", d_Q_aorta);
    db->putDouble("d_Q_left_atrium", d_Q_left_atrium);
    db->putDouble("d_Q_mitral", d_Q_mitral);
    db->putDouble("d_time", d_time);
    return;
} // putToDatabase

void CirculationModel_with_lv::print_summary(){

    double P_aorta     = d_fourier_aorta->values[d_current_idx_series];
    double P_atrium    = d_fourier_atrium->values[d_current_idx_series];
    double P_ventricle = d_fourier_ventricle->values[d_current_idx_series];

    pout << "% time \t P_aorta (mmHg)\t P_atrium (mmHg)\t P_ventricle (mmHg)\t Q_Aorta (ml/s)\t d_Q_left_atrium (ml/s)\tQ_mitral (ml/s) \t idx\n" ;
    pout << d_time << " " << P_aorta <<  " " << P_atrium << " " << P_ventricle << " " << d_Q_aorta << " " << d_Q_left_atrium << " " << d_Q_mitral << " " << d_current_idx_series << "\n";

}

int CirculationModel_with_lv::point_in_aorta(double testx, double testy){
    // checks whether given point is in aorta
    return pnpoly(d_n_pts_aorta, d_aorta_points_x, d_aorta_points_y, testx, testy);
}

int CirculationModel_with_lv::point_in_atrium(double testx, double testy){
    // checks whether given point is in atrium
    return pnpoly(d_n_pts_atrium, d_atrium_points_x, d_atrium_points_y, testx, testy);
}

/////////////////////////////// PROTECTED ////////////////////////////////////

/////////////////////////////// PRIVATE //////////////////////////////////////

void
CirculationModel_with_lv::writeDataFile() const
{
    static const int mpi_root = 0;
    if (SAMRAI_MPI::getRank() == mpi_root)
    {
        static bool file_initialized = false;
        const bool from_restart = RestartManager::getManager()->isFromRestart();
        if (!from_restart && !file_initialized)
        {
            ofstream fout(DATA_FILE_NAME.c_str(), ios::out);
            fout << "% time \t P_aorta (mmHg)\t P_atrium (mmHg)\t P_ventricle (mmHg)\t Q_Aorta (ml/s)\t d_Q_left_atrium (ml/s)\tQ_mitral (ml/s)"
                 << "\n"
                 << "bc_vals = [";
            file_initialized = true;
        }

        ofstream fout(DATA_FILE_NAME.c_str(), ios::app);

        fout << d_time;
        fout.setf(ios_base::scientific);
        fout.setf(ios_base::showpos);
        fout.precision(10);

        double P_aorta     = d_fourier_aorta->values[d_current_idx_series];
        double P_atrium    = d_fourier_atrium->values[d_current_idx_series];
        double P_ventricle = d_fourier_ventricle->values[d_current_idx_series];
        fout << " " << P_aorta <<  " " << P_atrium << " " << P_ventricle << " " << d_Q_aorta << " " << d_Q_left_atrium << " " << d_Q_mitral << "; \n";

    }

    return;
} // writeDataFile

void
CirculationModel_with_lv::getFromRestart()
{
    Pointer<Database> restart_db = RestartManager::getManager()->getRootDatabase();
    Pointer<Database> db;
    if (restart_db->isDatabase(d_object_name))
    {
        db = restart_db->getDatabase(d_object_name);
    }
    else
    {
        TBOX_ERROR("Restart database corresponding to " << d_object_name << " not found in restart file.");
    }

    d_current_idx_series = db->getInteger("d_current_idx_series");
    d_Q_aorta            = db->getDouble("d_Q_aorta");
    d_Q_left_atrium      = db->getDouble("d_Q_left_atrium");
    d_Q_mitral           = db->getDouble("d_Q_mitral");
    d_time               = db->getDouble("d_time");

    return;
} // getFromRestart

/////////////////////////////// NAMESPACE ////////////////////////////////////

/////////////////////////////// TEMPLATE INSTANTIATION ///////////////////////

//////////////////////////////////////////////////////////////////////////////