SegmentalController.cpp

/*
** SegmentalController.cpp
**
** Copyright (C) 2013-2019 Thomas Geijtenbeek and contributors. All rights
*reserved.
**
** This file is part of SCONE. For more information, see http://scone.software.
*/

#include "SegmentalController.h"

#include "scone/core/Exception.h"
#include "scone/core/Factories.h"
#include "scone/core/HasName.h"
#include "scone/core/Log.h"
#include "scone/core/types.h"
#include "scone/model/Dof.h"
#include "scone/model/Location.h"
#include "scone/model/Model.h"
#include "scone/model/Muscle.h"
#include "scone/model/SensorDelayAdapter.h"
#include "scone/model/Sensors.h"
#include "scone/model/Side.h"
#include "spot/par_tools.h"
#include "xo/container/prop_node.h"
#include "xo/container/prop_node_tools.h"
#include "xo/numerical/constants.h"
#include "xo/serialization/serialize.h"
#include "xo/string/dictionary.h"

#include <algorithm>
#include <cmath>
#include <exception>
#include <fstream>
#include <iomanip>
#include <set>
#include <utility>

using namespace scone;
using namespace xo;
using namespace std;
using namespace spinal_cord;
using std::get;
using std::make_shared;
using std::runtime_error;
constexpr auto pi = xo::constantsd::pi;
constexpr auto two_pi = xo::constantsd::two_pi;

/******************************************************************************/
// Type checking.
/******************************************************************************/

/// If other types of muscle roles should be supported they must be included
/// here. This is used for error checking.
const dictionary< muscle_role_t >
	MUSCLE_ROLE_DICT( { { agonist, "agonist" },
						{ antagonist, "antagonist" },
						{ synergist, "synergist" },
						{ extensor, "extensor" } } );

/// If other types of actions should be supported they must be included
/// here. This is used for error checking.
const dictionary< neuron_action_t > NEURON_ACTION_DICT(
	{ { excite, "excite" },
	  { inhibit, "inhibit" },
	  { excite_or_inhibit, "excite_or_inhibit" },
	  { positive_pass_through, "positive_pass_through" },
	  { negative_pass_through, "negative_pass_through" } } );

/******************************************************************************/
// Interface for getting access to muscle_info_t variables.
/******************************************************************************/

muscle_t& upd_agonist( muscle_info_t& muscle_info )
{
	return get< 0 >( muscle_info );
}

vector< muscle_t >& upd_synergists( muscle_info_t& muscle_info )
{
	return get< 1 >( muscle_info );
}

vector< muscle_t >& upd_antagonists( muscle_info_t& muscle_info )
{
	return get< 2 >( muscle_info );
}

vector< segment_t >& upd_segments( muscle_info_t& muscle_info )
{
	return get< 3 >( muscle_info );
}

bool& upd_is_extensor( muscle_info_t& muscle_info )
{
	return get< 4 >( muscle_info );
}

vector< neuron_type_t >& upd_excluded_neurons( muscle_info_t& muscle_info )
{
	return get< 5 >( muscle_info );
}

muscle_t get_agonist( const muscle_info_t& muscle_info )
{
	return get< 0 >( muscle_info );
}

vector< muscle_t > get_synergists( const muscle_info_t& muscle_info )
{
	return get< 1 >( muscle_info );
}

vector< muscle_t > get_antagonists( const muscle_info_t& muscle_info )
{
	return get< 2 >( muscle_info );
}

vector< segment_t > get_segments( const muscle_info_t& muscle_info )
{
	return get< 3 >( muscle_info );
}

bool get_is_extensor( const muscle_info_t& muscle_info )
{
	return get< 4 >( muscle_info );
}

vector< neuron_type_t > get_excluded_neurons( const muscle_info_t& muscle_info )
{
	return get< 5 >( muscle_info );
}

/******************************************************************************/
// Interface for getting access to rule_structure_t variables.
/******************************************************************************/

muscle_role_t get_source_role( const rule_structure_t& rule )
{
	return get< 0 >( rule );
}

neuron_type_t get_source_neuron_type( const rule_structure_t& rule )
{
	return get< 1 >( rule );
}

neuron_action_t get_neuron_action( const rule_structure_t& rule )
{
	return get< 2 >( rule );
}

muscle_role_t get_destination_role( const rule_structure_t& rule )
{
	return get< 3 >( rule );
}

neuron_type_t get_destination_neuron_type( const rule_structure_t& rule )
{
	return get< 4 >( rule );
}

/******************************************************************************/
// Synapse implementation.
/******************************************************************************/

synapse_t::synapse_t( double w, shared_ptr< neuron_t >& source,
					  shared_ptr< neuron_t >& destination )
	: w( w ), source( source ), destination( destination )
{
}

/******************************************************************************/
// Neuron implementation.
/******************************************************************************/

neuron_t::neuron_t( Model& model ) : model( model ) {}

void neuron_t::register_synapse( shared_ptr< synapse_t >& synapse )
{
	synapses.push_back( synapse );
}

/******************************************************************************/
// Leaky neuron implementation.
/******************************************************************************/

// activation functions
activation_t linear = []( double x ) -> double { return x; };

activation_t relu = []( double x ) -> double { return x <= 0 ? 0 : x; };

activation_t sigmoid = []( double x ) -> double {
	return 1 / ( 1 + exp( -x ) );
};

activation_t min_max = []( double x ) -> double {
	return std::min( std::max( 0.0, x ), 1.0 );
};

leaky_neuron_t::leaky_neuron_t( Model& model, double tau, double w0,
								activation_t f )
	: neuron_t( model ), tau( tau ), w0( w0 ), f( f )
{
	state = { 0, 0 };
	state_history.push_back( state );
}

double leaky_neuron_t::get_input( double t )
{
	auto sum = w0;
	for ( auto& syn : synapses )
	{
		const auto& neuron = syn->source.lock( );
		sum += syn->w * neuron->get_output( t );
	}
	return sum;
}

double leaky_neuron_t::get_output( double t )
{
	auto state = get_state( );
	if ( t <= state.t )
	{ // check if output is already calculated
		auto state = find_if(
			state_history.rbegin( ), state_history.rend( ),
			[ t ]( const auto& item ) -> bool { return item.t <= t; } );
		if ( state == state_history.rend( ) )
			throw runtime_error( "invalid history" );
		return f( state->value );
	}
	else
	{ // else advance neuron dynamics
		euler_advance( t );
		// runge_kutta_advance( t );
		return f( get_state( ).value );
	}
}

leaky_neuron_t::state_t leaky_neuron_t::get_state( ) { return state; }

void leaky_neuron_t::set_state( leaky_neuron_t::state_t new_state )
{
	state = new_state;
	state_history.push_back( new_state );
}

double leaky_neuron_t::calc_derivative( double t, double V, double I )
{
	return ( -V + I ) / tau;
}

void leaky_neuron_t::euler_advance( double tf )
{
	auto dt = 0.001; // SCONE integrator uses 0.001 time-step
	auto t0 = get_state( ).t;

	while ( t0 <= tf )
	{
		auto I = get_input( t0 );
		auto x0 = get_state( ).value;

		auto dxdt = calc_derivative( t0, x0, I );
		auto x = x0 + dt * dxdt;

		t0 += dt;
		set_state( { t0, x } );
	}
}

void leaky_neuron_t::runge_kutta_advance( double tf )
{
	throw runtime_error( "Runge-Kutta 4 not implemented properly" );

	auto dt = 0.001;
	auto t0 = get_state( ).t;

	while ( t0 <= tf )
	{
		// TODO: input is constant for intermediate steps, so it may not work
		// well
		auto I = get_input( t0 );
		auto x0 = get_state( ).value;

		auto k1 = calc_derivative( t0, x0, I ) * dt;
		auto k2 = calc_derivative( t0 + dt / 2, x0 + k1 / 2, I ) * dt;
		auto k3 = calc_derivative( t0 + dt / 2, x0 + k2 / 2, I ) * dt;
		auto k4 = calc_derivative( t0 + dt, x0 + k3, I ) * dt;
		auto x = x0 + 1 / 6.0 * ( k1 + 2 * k2 + 2 * k3 + k4 );

		t0 += dt;
		set_state( { t0, x } );
	}
}

/******************************************************************************/
// Sensory neuron implementation.
/******************************************************************************/

sensor_neuron_t::sensor_neuron_t( Model& model, double tau, double w0,
								  activation_t f, double s, TimeInSeconds delay,
								  SensorDelayAdapter* sensor )
	: leaky_neuron_t( model, tau, w0, f ), s( s ), delay( delay ),
	  sensor( sensor )
{
}

double sensor_neuron_t::get_input( double t )
{
	return w0 + s * sensor->GetValue( delay );
}

/******************************************************************************/
// Feed-forward neuron implementation.
/******************************************************************************/

proportional_derivative_neuron_t::proportional_derivative_neuron_t(
	Model& model, double tau, double w0, activation_t f, double kp, double p0,
	double kd, double v0 )
	: inter_neuron_t( model, tau, w0, f ), kp( kp ), p0( p0 ), kd( kd ),
	  v0( v0 )
{
}

double proportional_derivative_neuron_t::get_input( double t )
{
	SCONE_ASSERT_MSG( dynamic_cast< sensor_neuron_t* >(
						  synapses[ 0 ]->source.lock( ).get( ) ) != nullptr,
					  "proportional_derivative_neuron_t assumes that the first "
					  "synapse is with a sensory_neuron_t" );
	SCONE_ASSERT_MSG(
		dynamic_cast< sensor_neuron_t* >(
			synapses[ 1 ]->source.lock( ).get( ) ) != nullptr,
		"proportional_derivative_neuron_t assumes that the second "
		"synapse is with a sensory_neuron_t" )

	auto y0 = synapses[ 0 ]->w * synapses[ 0 ]->source.lock( )->get_output( t );
	auto y1 = synapses[ 1 ]->w * synapses[ 1 ]->source.lock( )->get_output( t );
	auto rest = 0.0;
	for ( int i = 2; i < synapses.size( ); i++ )
		rest +=
			synapses[ i ]->w * synapses[ i ]->source.lock( )->get_output( t );

	return w0 + kp * ( y0 - p0 ) + kd * ( y1 - v0 ) + rest;
}

/******************************************************************************/
// Feed-forward neuron implementation.
/******************************************************************************/

// gets leg's state [0, 4]
int get_leg_phase( const Model& model, string leg_identifier )
{
	int phase = 0;
	const auto& data = model.GetData( );
	if ( ! data.IsEmpty( ) )
	{
		auto index = model.GetData( ).GetChannelIndex( leg_identifier );
		SCONE_ASSERT_MSG( index != NoIndex,
						  "please make sure that you use GaitStateController" );
		phase = data.GetChannelData( index ).back( );
	}
	return phase;
}

feed_forward_neuron_t::feed_forward_neuron_t( Model& model, double tau,
											  double w0, activation_t f,
											  string leg_identifier,
											  vector< double > phase_weights )
	: inter_neuron_t( model, tau, w0, f ), leg_identifier( leg_identifier ),
	  phase_weights( phase_weights )
{
}

double feed_forward_neuron_t::get_input( double t )
{
	int phase = get_leg_phase( model, leg_identifier );
	return w0 + phase_weights[ phase ];
}

/******************************************************************************/
// Pattern formation neuron implementation.
/******************************************************************************/

pattern_formation_neuron_t::pattern_formation_neuron_t( Model& model,
														double tau, double w0,
														activation_t f,
														string oscillator_state,
														FunctionUP& pattern )
	: inter_neuron_t( model, tau, w0, f ), oscillator_state( oscillator_state ),
	  pattern( pattern )
{
	SCONE_ASSERT_MSG( model.GetState( ).GetIndex( oscillator_state ) != NoIndex,
					  oscillator_state + "does not exist in model's state" );
}

double pattern_formation_neuron_t::get_input( double t )
{
	// Bezier accepts 0 <= phi <= 1 so we map from [0, 2 pi] to [0, 1]
	auto phi = clamp( model.GetState( ).GetValue(
						  model.GetState( ).GetIndex( oscillator_state ) ),
					  0.0, two_pi( ) ) /
			   two_pi( );

	return w0 + pattern->GetValue( phi );
}

/******************************************************************************/
// Neurons utilities.
/******************************************************************************/

bool is_sensor_neuron( const neuron_type_t& type )
{
	return type.substr( 0, 2 ) == "SN";
}

bool is_inter_neuron( const neuron_type_t& type )
{
	return type.substr( 0, 2 ) == "IN";
}

bool is_motor_neuron( const neuron_type_t& type )
{
	return type.substr( 0, 2 ) == "MN";
}

bool is_proportional_derivative_neuron( const neuron_type_t& type )
{
	return type.substr( 0, 2 ) == "PD";
}

bool is_feed_forward_neuron( const neuron_type_t& type )
{
	return type.substr( 0, 2 ) == "FF";
}

bool is_pattern_formation_neuron( const neuron_type_t& type )
{
	return type.substr( 0, 2 ) == "PF";
}

shared_ptr< neuron_t > make_sensor_neuron( const PropNode& props, Params& par,
										   Model& model,
										   const PropNode& sensory_delay,
										   muscle_t muscle, neuron_type_t type,
										   segment_t segment )
{
	SCONE_ASSERT_MSG( is_sensor_neuron( type ), "not sensory neuron" );

	// get default time constant for the leaky-integrator
	double tau;
	INIT_PAR_NAMED( props, par, tau, "tau", 0.01 );

	// define parameter prefix name (e.g., tib_ant.L4.SN_II.)
	ScopedParamSetPrefixer prefixer( par, GetNameNoSide( muscle ) + "." +
											  segment + "." + type + "." );

	// get sensor scale factor
	double s = par.try_get( "s", props, "sensor_scale", 1.0 );

	// get the communication delay for this muscle
	TimeInSeconds delay;
	INIT_PROP( props, delay, 0 );
	sensory_delay.try_get( delay, GetNameNoSide( muscle ) );

	// get parameters based on sensor type
	activation_t f;
	double w0;
	SensorDelayAdapter* sensor;
	if ( type == "SN_II" )
	{
		f = relu;
		w0 = par.try_get( "w0", props, "length_offset", 0.0 );
		// we might switch to MuscleIISpindleSensor in the future
		Muscle& sconeMuscle(
			*TryFindByName( model.GetMuscles( ), muscle )->get( ) );
		sensor =
			&model.AcquireDelayedSensor< MuscleLengthSensor >( sconeMuscle );
	}
	else if ( type == "SN_Ia" )
	{
		f = relu;
		w0 = par.try_get( "w0", props, "velocity_offset", 0.0 );
		Muscle& sconeMuscle(
			*TryFindByName( model.GetMuscles( ), muscle )->get( ) );
		// might want to make Ia neuron slower to reject oscillations
		INIT_PAR_NAMED( props, par, tau, "tau_Ia", 0.01 );
		bool use_prochazka;
		INIT_PROP_NAMED( props, use_prochazka, "use_prochazka", false );
		if ( use_prochazka )
			sensor = &model.AcquireDelayedSensor< MuscleIaSpindleSensor >(
				sconeMuscle );
		else
			sensor = &model.AcquireDelayedSensor< MuscleVelocitySensor >(
				sconeMuscle );
	}
	else if ( type == "SN_Ib" )
	{
		f = relu;
		w0 = par.try_get( "w0", props, "force_offset", 0.0 );
		Muscle& sconeMuscle(
			*TryFindByName( model.GetMuscles( ), muscle )->get( ) );
		sensor =
			&model.AcquireDelayedSensor< MuscleForceSensor >( sconeMuscle );
	}
	else if ( type == "SN_DP" )
	{
		f = linear;
		w0 = 0;
		auto& dof( *FindByNameTrySided(
			model.GetDofs( ), GetNameNoSide( muscle ), Side::NoSide ) );
		sensor = &model.AcquireDelayedSensor< DofPositionSensor >( dof );
	}
	else if ( type == "SN_DV" )
	{
		f = linear;
		w0 = 0;
		Dof& dof( *FindByNameTrySided(
			model.GetDofs( ), GetNameNoSide( muscle ), Side::NoSide ) );
		sensor = &model.AcquireDelayedSensor< DofVelocitySensor >( dof );
	}
	else
		throw runtime_error( "sensory type: " + type + " not supported" );

	return make_shared< sensor_neuron_t >( model, tau, w0, f, s, delay,
										   sensor );
}

shared_ptr< neuron_t > make_inter_neuron( const PropNode& props, Params& par,
										  Model& model, muscle_t muscle,
										  neuron_type_t type,
										  segment_t segment )
{
	SCONE_ASSERT_MSG( is_inter_neuron( type ), "not inter neuron" );

	// get default time constant for the leaky-integrator
	double tau;
	INIT_PAR_NAMED( props, par, tau, "tau", 0.01 );

	// define parameter prefix name (e.g., tib_ant.L4.IN_Ia.)
	ScopedParamSetPrefixer prefixer( par, GetNameNoSide( muscle ) + "." +
											  segment + "." + type + "." );

	auto w0 = par.try_get( "w0", props, "bias", 0.0 );
	auto f = min_max;
	return make_shared< inter_neuron_t >( model, tau, w0, f );
}

shared_ptr< neuron_t > make_motor_neuron( const PropNode& props, Params& par,
										  Model& model, muscle_t muscle,
										  neuron_type_t type,
										  segment_t segment )
{
	SCONE_ASSERT_MSG( is_motor_neuron( type ), "not motor neuron" );

	// get default time constant for the leaky-integrator
	double tau;
	INIT_PAR_NAMED( props, par, tau, "tau", 0.01 );

	// define parameter prefix name (e.g., tib_ant.L4.MN.)
	ScopedParamSetPrefixer prefixer( par, GetNameNoSide( muscle ) + "." +
											  segment + "." + type + "." );

	auto w0 = par.try_get( "w0", props, "bias", 0.0 );
	auto f = min_max;
	return make_shared< motor_neuron_t >( model, tau, w0, f );
}

shared_ptr< neuron_t >
make_proportional_derivative_neuron( const PropNode& props, Params& par,
									 Model& model, muscle_t muscle,
									 neuron_type_t type, segment_t segment )
{
	SCONE_ASSERT_MSG( is_proportional_derivative_neuron( type ),
					  "not proportional derivative neuron" );

	// get default time constant for the leaky-integrator
	double tau;
	INIT_PAR_NAMED( props, par, tau, "tau", 0.01 );

	// define parameter prefix name (e.g., tib_ant.L4.MN.)
	ScopedParamSetPrefixer prefixer( par, GetNameNoSide( muscle ) + "." +
											  segment + "." + type + "." );

	auto w0 = par.try_get( "w0", props, "bias", 0.0 );
	auto f = linear;
	auto kp = par.try_get( "kp", props, "kp", 0.0 );
	auto p0 = par.try_get( "p0", props, "p0", 0.0 );
	auto kv = par.try_get( "kv", props, "kv", 0.0 );
	auto v0 = par.try_get( "v0", props, "v0", 0.0 );
	return make_shared< proportional_derivative_neuron_t >( model, tau, w0, f,
															kp, p0, kv, v0 );
}

shared_ptr< neuron_t > make_feed_forward_neuron( const PropNode& props,
												 Params& par, Model& model,
												 muscle_t muscle,
												 neuron_type_t type,
												 segment_t segment )
{
	SCONE_ASSERT_MSG( is_feed_forward_neuron( type ),
					  "not feed forward neuron" );

	// get default time constant for the leaky-integrator
	double tau;
	INIT_PAR_NAMED( props, par, tau, "tau", 0.01 );

	// define parameter prefix name (e.g., tib_ant.L4.FF.)
	ScopedParamSetPrefixer prefixer( par, GetNameNoSide( muscle ) + "." +
											  segment + "." + type + "." );

	auto w0 = par.try_get( "w0", props, "feed_forward_offset", 0.0 );
	auto f = min_max;

	// determine leg identifier (experimental, only works with
	// GaitStateController)
	string leg_identifier;
	if ( muscle.find( "_l" ) != string::npos )
		leg_identifier = "leg0_l.state";
	else
		leg_identifier = "leg1_r.state";

	// get phase weights
	vector< double > phase_weights;
	for ( int p = 0; p < 5; p++ )
	{
		double wp = par.try_get( "P" + to_str( p ) + ".w", props,
								 "default_phase_weight", 0.1 );
		phase_weights.push_back( wp );
	}

	return make_shared< feed_forward_neuron_t >(
		model, tau, w0, f, leg_identifier, phase_weights );
}

void get_patterns( const PropNode& props, Params& par,
				   vector< FunctionUP >& patterns )
{
	// get patterns as functions
	int P = 0;
	if ( const auto* patterns_probs = props.try_get_child( "Patterns" ) )
	{
		for ( const auto& pattern_prob : *patterns_probs )
		{
			// No need to be only Bezier, because we introduced other curves
			// (e.g., RaisedCosine).

			// SCONE_ASSERT_MSG( pattern_prob.first == "Bezier",
			// 				  "only Bezier functions allowed" );
			if ( auto fp = MakeFactoryProps( GetFunctionFactory( ),
											 pattern_prob, "Function" ) )
			{
				// second level prefix (PF.P0.Y0, PF.P1.Y1)
				ScopedParamSetPrefixer prefixer( par,
												 "P" + to_string( P ) + "." );
				patterns.push_back( CreateFunction( fp, par ) );
				P++;
			}
		}
	}
}

// Construct the motor neuron active state dictionary. One can specify
// the muscle, gait phase state, and weight factor that is to be
// multiplied to calculate the excitation of a muscle (experimental
// Andrea).
//
// MotorNeuronActiveStates {
// 	tib_ant_0 = 1.0
// 	tib_ant_1 = 1.0
// 	tib_ant_2 = 1.0
// 	tib_ant_3 = 1.0
// 	tib_ant_4 = 1.0
// 	soleus_0 = 1.0
// 	soleus_1 = 1.0
// 	soleus_2 = 1.0
// 	gastroc_0 = 1.0
// 	gastroc_1 = 1.0
// 	gastroc_2 = 1.0
// }
map< muscle_t, vector< double > > MN_ACTIVE_STATE;
void get_motor_neuron_active_states( const PropNode& props, Params& par,
									 const set< muscle_t >& unique_muscles )
{
	if ( const auto* mnas_probs =
			 props.try_get_child( "MotorNeuronActiveStates" ) )
	{
		for ( const auto& mnas_prob : *mnas_probs )
		{
			auto dot_separation = mnas_prob.first.rfind( "_" );
			SCONE_ASSERT_MSG( dot_separation != string::npos,
							  mnas_prob.first +
								  " should be formatted as muscle_phase" );

			auto muscle = muscle_t(
				mnas_prob.first.substr( 0, dot_separation ).c_str( ) );
			SCONE_ASSERT_MSG( unique_muscles.count( muscle ) > 0,
							  muscle +
								  " does not exist in spinal cord segment" );

			auto phase = stoi( mnas_prob.first.substr(
				dot_separation + 1, mnas_prob.first.length( ) ) );
			SCONE_ASSERT_MSG( phase >= 0 && phase <= 4, "0 <= phase <= 4" );

			if ( MN_ACTIVE_STATE.count( muscle ) == 0 )
				MN_ACTIVE_STATE.insert( { muscle, { 0, 0, 0, 0, 0 } } );

			MN_ACTIVE_STATE[ muscle ][ phase ] =
				stod( mnas_prob.second.raw_value( ) );
		}
	}
}

/******************************************************************************/
// Segment organization implementation.
/******************************************************************************/

void segment_organization_t::ensure_neuron_exists(
	const neuron_identifier_t& neuron_identifier,
	shared_ptr< neuron_t >& neuron )
{
	// check if neuron already exists
	if ( neurons.count( neuron_identifier ) > 0 )
		return;

	// otherwise add neuron
	neurons.insert( make_pair( neuron_identifier, neuron ) );

	if ( is_sensor_neuron( neuron_identifier.second ) )
		sensor_neurons.insert( make_pair( neuron_identifier, neuron ) );
	else if ( is_inter_neuron( neuron_identifier.second ) ||
			  is_proportional_derivative_neuron( neuron_identifier.second ) ||
			  is_feed_forward_neuron( neuron_identifier.second ) ||
			  is_pattern_formation_neuron( neuron_identifier.second ) )
		inter_neurons.insert( make_pair( neuron_identifier, neuron ) );
	else if ( is_motor_neuron( neuron_identifier.second ) )
		motor_neurons.insert( make_pair( neuron_identifier, neuron ) );
	else
		; // no need to do anything
}

void segment_organization_t::add_connection(
	const PropNode& props, Params& par, const segment_t& segment,
	const neuron_identifier_t& source, const neuron_identifier_t& destination,
	const neuron_action_t& neuron_action )
{
	// define parameter prefix name (e.g., gastroc.L5.IN_Ia->tib_ant.L5.IN_Ia.w)
	auto symbol = "->";
	ScopedParamSetPrefixer prefixer(
		par, GetNameNoSide( source.first ) + "." + segment + "." +
				 source.second + symbol + GetNameNoSide( destination.first ) +
				 "." + segment + "." + destination.second + "." );

	// get parameter
	double w;
	if ( neuron_action == excite )
		w = par.try_get( "w", props, "excitatory_synapse", 1.0 );
	else if ( neuron_action == inhibit )
		w = par.try_get( "w", props, "inhibitory_synapse", -1.0 );
	else if ( neuron_action == excite_or_inhibit )
		w = par.try_get( "w", props, "excitatory_or_inhibitory_synapse", 0.0 );
	else if ( neuron_action == positive_pass_through )
		w = 1;
	else if ( neuron_action == negative_pass_through )
		w = -1;
	else
		throw runtime_error( "synapse type not supported yet" );

	// create synapse
	auto& source_neuron = neurons.at( source );
	auto& destination_neuron = neurons.at( destination );
	auto synapse =
		make_shared< synapse_t >( w, source_neuron, destination_neuron );
	auto synapse_identifier = synapse_identifier_t( source, destination );

	// register synapse
	synapses.insert( make_pair( synapse_identifier, synapse ) );
	destination_neuron->register_synapse( synapse );
}

void segment_organization_t::prune_neuron(
	const PropNode& props, Params& par,
	const neuron_identifier_t& neuron_identifier )
{
	log::warning( "neuron: " + neuron_identifier.first + "." +
				  neuron_identifier.second +
				  " does not connect to any neuron" );
	// not handled yet

	// remove neuron from collections (triggers a core dump)
	// neurons.erase(neuron_identifier);
	// sensor_neurons.erase(neuron_identifier);
	// inter_neurons.erase(neuron_identifier);
	// motor_neurons.erase(neuron_identifier);

	// remove synapses that connect to this neuron
	// vector<synapse_identifier_t> remove;
	// for (const auto& synapse : synapses) {
	// 	if (synapse.first.second == neuron_identifier) {
	// 		remove.push_back(synapse.first);
	// 	}
	// }

	// for (const auto& key : remove) {
	// 	synapses.erase(key);
	// }

	// Fix parameters that must be pruned (do not know how to do this yet)
	// auto neuron_bias = neurons[neuron_identifier]->get_name( true ) + ".w0";
}

void segment_organization_t::export_graph( string file_name )
{
	ofstream file;
	file.open( file_name );

	file << "digraph {" << endl;
	file << "rankdir=LR;" << endl;

	// build sensor group
	file << "subgraph sensor_neurons {" << endl;
	file << "rank=\"same\";" << endl;
	file << "node[shape=doubleoctagon];" << endl;
	for ( const auto& neuron : sensor_neurons )
	{
		file << neuron.first.first << "_" << neuron.first.second
			 << " [xlabel=\""
			 << static_cast< sensor_neuron_t* >( neuron.second.get( ) )->w0
			 << setprecision( 2 ) << "\"]"
			 << ";" << endl;
	}
	file << "}" << endl;

	file << "subgraph inter_neurons {" << endl;
	file << "rank=\"same\";" << endl;
	file << "node[shape=circle];" << endl;
	for ( const auto& neuron : inter_neurons )
	{
		file << neuron.first.first << "_" << neuron.first.second
			 << " [xlabel=\""
			 << static_cast< inter_neuron_t* >( neuron.second.get( ) )->w0
			 << setprecision( 2 ) << "\"]"
			 << ";" << endl;
	}
	file << "}" << endl;

	file << "subgraph motor_neurons {" << endl;
	file << "rank=\"same\";" << endl;
	file << "node[shape=diamond];" << endl;
	for ( const auto& neuron : motor_neurons )
	{
		file << neuron.first.first << "_" << neuron.first.second
			 << " [xlabel=\""
			 << static_cast< motor_neuron_t* >( neuron.second.get( ) )->w0
			 << setprecision( 2 ) << "\"]"
			 << ";" << endl;
	}
	file << "}" << endl;

	for ( const auto& synapse : synapses )
	{
		file << synapse.first.first.first << "_" << synapse.first.first.second
			 << " -> " << synapse.first.second.first << "_"
			 << synapse.first.second.second << "[arrowhead=\""
			 << ( synapse.second->w >= 0 ? "inv" : "odot" ) << "\""
			 << ",label=\"" << setprecision( 2 ) << synapse.second->w << "\"];"
			 << endl;
	}

	file << "}" << endl;
	file.close( );
}

/******************************************************************************/
// Spinal cord implementation.
/******************************************************************************/

void spinal_cord_organization_t::ensure_neuron_exists(
	const PropNode& props, Params& par, Model& model,
	const PropNode& sensory_delay, const segment_t& segment,
	const neuron_identifier_t& neuron_identifier )
{
	auto muscle = neuron_identifier.first;
	auto neuron_type = neuron_identifier.second;
	shared_ptr< neuron_t > neuron;

	// check if neuron is of type sensor
	if ( is_sensor_neuron( neuron_type ) )
	{
		// first check if neuron exists in sensors and if it exists then use its
		// reference, because sensors are unique for all segments (this might
		// change)
		auto sensor_it = sensors.find( neuron_identifier );
		if ( sensor_it != sensors.end( ) )
		{ // if found get a reference
			neuron = sensor_it->second;
		}
		else
		{ // else create a new neuron and insert in sensors
			neuron = make_sensor_neuron( props, par, model, sensory_delay,
										 muscle, neuron_type, segment );
			sensors.insert( make_pair( neuron_identifier, neuron ) );
		}
	}
	else if ( is_inter_neuron( neuron_type ) )
	{
		neuron = make_inter_neuron( props, par, model, muscle, neuron_type,
									segment );
	}
	else if ( is_motor_neuron( neuron_type ) )
	{
		neuron = make_motor_neuron( props, par, model, muscle, neuron_type,
									segment );
	}
	else if ( is_feed_forward_neuron( neuron_type ) )
	{
		neuron = make_feed_forward_neuron( props, par, model, muscle,
										   neuron_type, segment );
	}
	else if ( is_proportional_derivative_neuron( neuron_type ) )
	{
		neuron = make_proportional_derivative_neuron( props, par, model, muscle,
													  neuron_type, segment );
	}
	else
		throw runtime_error( "type: " + neuron_type + " not supported" );

	segments[ segment ].ensure_neuron_exists( neuron_identifier, neuron );
}

void spinal_cord_organization_t::establish_relationship(
	const PropNode& props, Params& par, Model& model,
	const PropNode& sensory_delay, const segment_t& segment,
	const neuron_identifier_t& source_identifier,
	const neuron_identifier_t& destination_identifier,
	const neuron_action_t& neuron_action )
{
	ensure_neuron_exists( props, par, model, sensory_delay, segment,
						  source_identifier );
	ensure_neuron_exists( props, par, model, sensory_delay, segment,
						  destination_identifier );
	segments[ segment ].add_connection( props, par, segment, source_identifier,
										destination_identifier, neuron_action );
}

void spinal_cord_organization_t::build_reflex_network(
	const PropNode& props, Params& par, Model& model,
	const PropNode& sensory_delay,
	const muscle_organization_t& muscle_organization,
	const reflex_rules_t& reflex_rules )
{
	// define connections based on rules; iterate over muscle
	// organization
	for ( const auto& muscle_info : muscle_organization )
	{
		// get muscle info
		auto agonist_muscle = get_agonist( muscle_info );
		auto synergist_muscles = get_synergists( muscle_info );
		auto antagonist_muscles = get_antagonists( muscle_info );
		auto muscle_segments = get_segments( muscle_info );
		auto is_extensor = get_is_extensor( muscle_info );
		auto excluded_neurons = get_excluded_neurons( muscle_info );

		// Inform the user that synergist rules are not supported yet. This is
		// intended as a place holder for future use-cases.
		SCONE_THROW_IF( synergist_muscles.size( ) != 0,
						"synergy rules are not implemented yet" );

		// iterate over rules
		for ( const auto& rule : reflex_rules )
		{
			// get rule information
			auto source_role = get_source_role( rule );
			auto source_neuron_type = get_source_neuron_type( rule );
			auto neuron_action = get_neuron_action( rule );
			auto destination_role = get_destination_role( rule );
			auto destination_neuron_type = get_destination_neuron_type( rule );

			// check if neuron types (source/destination) exist in excluded
			// neurons and skip if this is the case
			if ( find( excluded_neurons.begin( ), excluded_neurons.end( ),
					   source_neuron_type ) != excluded_neurons.end( ) ||
				 find( excluded_neurons.begin( ), excluded_neurons.end( ),
					   destination_neuron_type ) != excluded_neurons.end( ) )
				continue;

			// create variables related to the synapse
			auto source_identifier =
				neuron_identifier_t( agonist_muscle, source_neuron_type );

			// iterate over each segment and implement rules
			for ( const auto& segment : muscle_segments )
			{
				// if segment does exist yet, create one
				if ( segments.count( segment ) == 0 )
					segments.insert(
						make_pair( segment, segment_organization_t( ) ) );

				// agonist to agonist || extensor to extensor
				if ( ( source_role == agonist &&
					   destination_role == agonist ) ||
					 ( is_extensor && source_role == extensor &&
					   destination_role == extensor ) )
				{
					auto destination_identifier = neuron_identifier_t(
						agonist_muscle, destination_neuron_type );
					establish_relationship( props, par, model, sensory_delay,
											segment, source_identifier,
											destination_identifier,
											neuron_action );
				}
				// agonist to antagonist || extensor to antagonist
				else if ( ( source_role == agonist &&
							destination_role == antagonist ) ||
						  ( is_extensor && source_role == extensor &&
							destination_role == antagonist ) )
				{
					// repeat the rule for each antagonist pair
					for ( const auto& antagonist_muscle : antagonist_muscles )
					{
						// check first if agonist muscle exists in segment,
						// otherwise no need to create the rule
						auto it = find_if(
							muscle_organization.begin( ),
							muscle_organization.end( ),
							[ antagonist_muscle,
							  segment ]( const auto& muscle_info ) -> bool {
								if ( get_agonist( muscle_info ) ==
									 antagonist_muscle )
								{
									auto antagonist_segments =
										get_segments( muscle_info );
									if ( find( antagonist_segments.begin( ),
											   antagonist_segments.end( ),
											   segment ) !=
										 antagonist_segments.end( ) )
									{
										return false;
									}
									else
									{
										return true;
									}
								}
								else
								{
									return false;
								}
							} );
						if ( it != muscle_organization.end( ) )
							continue;

						// if not skipped (continue), then we should create
						// the rule
						auto destination_identifier = neuron_identifier_t(
							antagonist_muscle, destination_neuron_type );
						establish_relationship(
							props, par, model, sensory_delay, segment,
							source_identifier, destination_identifier,
							neuron_action );
					}
				}
				// if rule does not follow structure
				else
					log::warning(
						"Rule: " + agonist_muscle + " " +
						to_string( source_role ) + " " + source_neuron_type +
						" " + to_string( neuron_action ) + " " +
						to_string( destination_role ) + " " +
						destination_neuron_type + " " +
						"was skipped (probably fine, useful for debugging)" );
			}
		}
	}

	// prune orphan neurons (neurons that do not connect to anything)
	for ( auto& segment : segments )
	{
		auto& synapses = segment.second.synapses;
		auto& neurons = segment.second.neurons;
		for ( auto& neuron : neurons )
		{
			// exclude motorneurons because currently they do not connect to
			// other neurons
			if ( neuron.first.second == "MN" )
				continue;

			// count the occurrence of postsynaptic connections by this neuron
			auto count =
				count_if( synapses.begin( ), synapses.end( ),
						  [ neuron ]( const auto& synapse ) -> bool {
							  if ( synapse.first.first == neuron.first )
								  return true;
							  else
								  return false;
						  } );
			if ( count == 0 ) // neuron should be removed
				segment.second.prune_neuron( props, par, neuron.first );
		}
	}
}

void spinal_cord_organization_t::build_pattern_network( const PropNode& props,
														Params& par,
														Model& model )
{
	// get primitive patterns
	get_patterns( props, par, patterns );

	// get default time constant for the leaky-integrator
	double tau;
	INIT_PAR_NAMED( props, par, tau, "tau", 0.01 );

	// activation function
	auto f = min_max;

	// oscillator state (here we assume it is always named `phase_side`)
	if ( ! patterns.empty( ) )
	{
		SCONE_ASSERT_MSG( model.GetState( ).GetIndex( "phase_left" ) != NoIndex,
						  "PhaseOscillator name should be phase" );
		SCONE_ASSERT_MSG( model.GetState( ).GetIndex( "phase_right" ) !=
							  NoIndex,
						  "PhaseOscillator name should be phase" );
	}
	const map< string, string > oscillator_state { { "_l", "phase_left" },
												   { "_r", "phase_right" } };

	// for each side create the pattern neurons
	for ( const string side : { "_r", "_l" } )
	{
		// for each pattern create a pattern formation neuron
		int P = 0;
		for ( auto& pattern : patterns )
		{
			// get bias parameter should be identical for pattern neuron P
			const auto pattern_identifier = "P" + std::to_string( P );
			double w0;
			{
				ScopedParamSetPrefixer prefixer(
					par, "PF." + pattern_identifier + "." );
				w0 =
					par.try_get( "w0", props, "pattern_formation_offset", 0.0 );
			}

			// create pattern neuron
			shared_ptr< neuron_t > pattern_neuron =
				make_shared< pattern_formation_neuron_t >(
					model, tau, w0, f, oscillator_state.at( side ), pattern );

			// Create the neuron identifier for this pattern. As this neuron is
			// not related to a muscle we use the muscle slot to set a unique
			// name (P#_r.PF).
			auto neuron_identifier = neuron_identifier_t(
				muscle_t( ( pattern_identifier + side ).c_str( ) ),
				neuron_type_t( "PF" ) );
			auto neuron_pair = make_pair( neuron_identifier, pattern_neuron );

			// insert pattern neuron in all segments and connect to all motor
			// neurons
			for ( auto& segment : segments )
			{
				// insert pattern neuron into the segment
				segment.second.neurons.insert( neuron_pair );
				segment.second.inter_neurons.insert( neuron_pair );

				// get all motor neurons of a particular side and establish a
				// connection
				for ( auto& motor_neuron : segment.second.motor_neurons )
				{
					// if motor neuron side not same skip
					auto muscle_side = GetSideName(
						GetSideFromName( motor_neuron.first.first ) );
					if ( side != muscle_side )
						continue;

					// We want the connection weight to be shared between left
					// and right side and for all segments (ALL keyword) and not
					// different in each segment (e.g.,
					// P#.ALL.PF->gastroc.ALL.MN.).
					ScopedParamSetPrefixer prefixer(
						par, pattern_identifier + ".ALL.PF->" +
								 GetNameNoSide( motor_neuron.first.first ) +
								 ".ALL.MN." );
					double w =
						par.try_get( "w", props, "pattern_muscle_weight", 0.0 );

					// create synapse
					auto synapse = make_shared< synapse_t >(
						w, pattern_neuron, motor_neuron.second );
					auto synapse_identifier = synapse_identifier_t(
						neuron_identifier, motor_neuron.first );

					// register synapse
					segment.second.synapses.insert(
						make_pair( synapse_identifier, synapse ) );
					motor_neuron.second->register_synapse( synapse );
				}
			}

			// increase pattern counter
			P++;
		}
	}
}

void spinal_cord_organization_t::build_direct_connections(
	const PropNode& props, Params& par, Model& model )
{
	auto connections = props.try_get_child( "DirectConnections" );
	if ( connections == nullptr )
		return;
	for ( auto& connection : *connections )
	{
		SCONE_ASSERT( connection.first == "Connection" );
		auto source_muscle =
			connection.second.get< muscle_t >( "source_muscle" );
		auto source_segment =
			connection.second.get< segment_t >( "source_segment" );
		auto source_neuron_type =
			connection.second.get< neuron_type_t >( "source_neuron_type" );
		auto neuron_action =
			NEURON_ACTION_DICT( connection.second.get< string >( "action" ) );
		auto destination_muscle =
			connection.second.get< muscle_t >( "destination_muscle" );
		auto destination_segment =
			connection.second.get< segment_t >( "destination_segment" );
		auto destination_neuron_type =
			connection.second.get< neuron_type_t >( "destination_neuron_type" );

		auto source_neuron_identifier =
			neuron_identifier_t( source_muscle, source_neuron_type );
		auto destination_neuron_identifier =
			neuron_identifier_t( destination_muscle, destination_neuron_type );

		// find source and destination neurons
		shared_ptr< neuron_t > source_neuron, destination_neuron;
		try
		{
			source_neuron = segments.at( source_segment )
								.neurons.at( source_neuron_identifier );
		}
		catch ( ... )
		{
			SCONE_THROW( "DirectConnection: cannot locate source neuron: " +
						 source_muscle + "." + source_segment + "." +
						 source_neuron_type );
		}

		try
		{
			destination_neuron =
				segments.at( destination_segment )
					.neurons.at( destination_neuron_identifier );
		}
		catch ( ... )
		{
			SCONE_THROW(
				"DirectConnection: cannot locate destination neuron: " +
				destination_muscle + "." + destination_segment + "." +
				destination_neuron_type );
		}

		// We want the connection weight to be shared between left and
		// right side.
		ScopedParamSetPrefixer prefixer(
			par, GetNameNoSide( source_muscle ) + "." + source_segment + "." +
					 source_neuron_type + +"->" +
					 GetNameNoSide( destination_muscle ) + "." +
					 destination_segment + "." + destination_neuron_type +
					 "." );

		// get parameter
		double w;
		if ( neuron_action == excite )
			w = par.try_get( "w", props, "excitatory_synapse", 1.0 );
		else if ( neuron_action == inhibit )
			w = par.try_get( "w", props, "inhibitory_synapse", -1.0 );
		else if ( neuron_action == excite_or_inhibit )
			w = par.try_get( "w", props, "excitatory_or_inhibitory_synapse",
							 0.0 );
		else
			throw runtime_error( "synapse type not supported yet" );

		// create synapse
		auto synapse =
			make_shared< synapse_t >( w, source_neuron, destination_neuron );
		auto synapse_identifier = synapse_identifier_t(
			source_neuron_identifier, destination_neuron_identifier );

		// register synapse
		segments.at( destination_segment )
			.synapses.insert( make_pair( synapse_identifier, synapse ) );
		destination_neuron->register_synapse( synapse );
	}
}

void spinal_cord_organization_t::build_root_stability(
	const PropNode& props, Params& par, Model& model,
	const PropNode& sensory_delay )
{
	auto root_stability = props.try_get_child( "RootStability" );
	if ( root_stability == nullptr )
		return;
	auto segment = root_stability->get< segment_t >( "segment" );

	// define neuron identifiers
	auto pelvis_position = neuron_identifier_t( "pelvis_tilt", "SN_DP" );
	auto pelvis_velocity = neuron_identifier_t( "pelvis_tilt", "SN_DV" );
	auto iliopsoas_r_pd = neuron_identifier_t( "iliopsoas_r", "PD" );
	auto iliopsoas_l_pd = neuron_identifier_t( "iliopsoas_l", "PD" );
	auto iliopsoas_r_mn = neuron_identifier_t( "iliopsoas_r", "MN" );
	auto iliopsoas_l_mn = neuron_identifier_t( "iliopsoas_l", "MN" );
	auto hamstrings_r_pd = neuron_identifier_t( "hamstrings_r", "PD" );
	auto hamstrings_l_pd = neuron_identifier_t( "hamstrings_l", "PD" );
	auto hamstrings_r_mn = neuron_identifier_t( "hamstrings_r", "MN" );
	auto hamstrings_l_mn = neuron_identifier_t( "hamstrings_l", "MN" );
	auto glut_max_r_pd = neuron_identifier_t( "glut_max_r", "PD" );
	auto glut_max_l_pd = neuron_identifier_t( "glut_max_l", "PD" );
	auto glut_max_r_mn = neuron_identifier_t( "glut_max_r", "MN" );
	auto glut_max_l_mn = neuron_identifier_t( "glut_max_l", "MN" );

	establish_relationship( props, par, model, sensory_delay, segment,
							pelvis_position, iliopsoas_r_pd,
							positive_pass_through );
	establish_relationship( props, par, model, sensory_delay, segment,
							pelvis_velocity, iliopsoas_r_pd,
							positive_pass_through );
	establish_relationship( props, par, model, sensory_delay, segment,
							iliopsoas_r_pd, iliopsoas_r_mn,
							positive_pass_through );
	establish_relationship( props, par, model, sensory_delay, segment,
							pelvis_position, iliopsoas_l_pd,
							positive_pass_through );
	establish_relationship( props, par, model, sensory_delay, segment,
							pelvis_velocity, iliopsoas_l_pd,
							positive_pass_through );
	establish_relationship( props, par, model, sensory_delay, segment,
							iliopsoas_l_pd, iliopsoas_l_mn,
							positive_pass_through );

	establish_relationship( props, par, model, sensory_delay, segment,
							pelvis_position, hamstrings_r_pd,
							positive_pass_through );
	establish_relationship( props, par, model, sensory_delay, segment,
							pelvis_velocity, hamstrings_r_pd,
							positive_pass_through );
	establish_relationship( props, par, model, sensory_delay, segment,
							hamstrings_r_pd, hamstrings_r_mn,
							positive_pass_through );
	establish_relationship( props, par, model, sensory_delay, segment,
							pelvis_position, hamstrings_l_pd,
							positive_pass_through );
	establish_relationship( props, par, model, sensory_delay, segment,
							pelvis_velocity, hamstrings_l_pd,
							positive_pass_through );
	establish_relationship( props, par, model, sensory_delay, segment,
							hamstrings_l_pd, hamstrings_l_mn,
							positive_pass_through );

	establish_relationship( props, par, model, sensory_delay, segment,
							pelvis_position, glut_max_r_pd,
							positive_pass_through );
	establish_relationship( props, par, model, sensory_delay, segment,
							pelvis_velocity, glut_max_r_pd,
							positive_pass_through );
	establish_relationship( props, par, model, sensory_delay, segment,
							glut_max_r_pd, glut_max_r_mn,
							positive_pass_through );
	establish_relationship( props, par, model, sensory_delay, segment,
							pelvis_position, glut_max_l_pd,
							positive_pass_through );
	establish_relationship( props, par, model, sensory_delay, segment,
							pelvis_velocity, glut_max_l_pd,
							positive_pass_through );
	establish_relationship( props, par, model, sensory_delay, segment,
							glut_max_l_pd, glut_max_l_mn,
							positive_pass_through );

	// establish_relationship( props, par, model, sensory_delay, segment,
	// 						iliopsoas_r_pd, glut_max_r_pd,
	// 						negative_pass_through );
	// establish_relationship( props, par, model, sensory_delay, segment,
	// 						glut_max_r_pd, iliopsoas_r_pd,
	// 						negative_pass_through );
	// establish_relationship( props, par, model, sensory_delay, segment,
	// 						iliopsoas_r_pd, hamstrings_r_pd,
	// 						negative_pass_through );
	// establish_relationship( props, par, model, sensory_delay, segment,
	// 						hamstrings_r_pd, iliopsoas_r_pd,
	// 						negative_pass_through );
	// establish_relationship( props, par, model, sensory_delay, segment,
	// 						iliopsoas_l_pd, glut_max_l_pd,
	// 						negative_pass_through );
	// establish_relationship( props, par, model, sensory_delay, segment,
	// 						glut_max_l_pd, iliopsoas_l_pd,
	// 						negative_pass_through );
	// establish_relationship( props, par, model, sensory_delay, segment,
	// 						iliopsoas_l_pd, hamstrings_l_pd,
	// 						negative_pass_through );
	// establish_relationship( props, par, model, sensory_delay, segment,
	// 						hamstrings_l_pd, iliopsoas_l_pd,
	// 						negative_pass_through );
}

/******************************************************************************/
// SCONE controller implementation.
/******************************************************************************/

SegmentalController::SegmentalController( const PropNode& props, Params& par,
										  Model& model, const Location& loc )
	: Controller( props, par, model, loc )
{
	// graphviz property
	INIT_PROP( props, generate_graph, false );

	// delay file
	INIT_PROP( props, sensory_delay_file, "" );
	if ( ! sensory_delay_file.empty( ) )
	{
		auto delay_file = FindFile( sensory_delay_file );
		model.AddExternalResource( delay_file );
		sensory_delay = xo::load_file( FindFile( delay_file ), "zml" );
	}

	// read reflex rules
	reflex_rules_t reflex_rules;
	auto& rules = props.get_child( "ReflexRules" );
	for ( auto& rule : rules )
	{
		SCONE_ASSERT( rule.first == "ReflexRule" );
		auto source_role =
			MUSCLE_ROLE_DICT( rule.second.get< string >( "source_role" ) );
		auto source_neuron =
			rule.second.get< neuron_type_t >( "source_neuron" );
		auto action =
			NEURON_ACTION_DICT( rule.second.get< string >( "action" ) );
		auto destination_role =
			MUSCLE_ROLE_DICT( rule.second.get< string >( "destination_role" ) );
		auto destination_neuron =
			rule.second.get< neuron_type_t >( "destination_neuron" );
		reflex_rules.push_back( { source_role, source_neuron, action,
								  destination_role, destination_neuron } );
	}

	// read muscle organization
	muscle_organization_t muscle_organization_layout;
	set< muscle_t > unique_muscles;
	auto& spinal_cord_organization =
		props.get_child( "SpinalCordOrganization" );
	for ( auto& organization : spinal_cord_organization )
	{
		SCONE_ASSERT( organization.first == "Organization" );
		auto agonist = organization.second.get< muscle_t >( "agonist" );
		auto synergists =
			organization.second.get< vector< muscle_t > >( "synergists", { } );
		auto antagonists =
			organization.second.get< vector< muscle_t > >( "antagonists", { } );
		auto segments =
			organization.second.get< vector< segment_t > >( "segments" );
		auto extensor = organization.second.get< bool >( "extensor", false );
		auto excluded_neurons =
			organization.second.get< vector< neuron_type_t > >(
				"excluded_neurons", { } );
		muscle_organization_layout.push_back( { agonist, synergists,
												antagonists, segments, extensor,
												excluded_neurons } );
		unique_muscles.insert( agonist );
	}

	// mirror muscle organization for left and right side (might switch to
	// scone::Location in the feature)
	muscle_organization_t muscle_organization;
	for ( auto side : { "_l", "_r" } )
	{
		for ( auto organization_rule : muscle_organization_layout )
		{
			// update agonist
			upd_agonist( organization_rule ) += side;

			// update synergists
			for ( auto& synergist : upd_synergists( organization_rule ) )
				synergist += side;

			// update antagonists
			for ( auto& antagonist : upd_antagonists( organization_rule ) )
				antagonist += side;

			// add the new rule
			muscle_organization.push_back( organization_rule );
		}
	}

	// build spinal cord model based on rules and muscle organization layout
	spinal_cord.build_reflex_network( props, par, model, sensory_delay,
									  muscle_organization, reflex_rules );

	// build pattern formation
	spinal_cord.build_pattern_network( props, par, model );

	// build root stability
	spinal_cord.build_root_stability( props, par, model, sensory_delay );

	// add direct connections
	spinal_cord.build_direct_connections( props, par, model );

	// control motor neuron based on gait phase (experimental Andrea)
	get_motor_neuron_active_states( props, par, unique_muscles );
}

bool SegmentalController::ComputeControls( Model& model, double timestamp )
{
	// determine leg state (experimental Andrea)
	map< Side, double > leg_phase;
	if ( ! MN_ACTIVE_STATE.empty( ) )
		leg_phase = { { LeftSide, get_leg_phase( model, "leg0_l.state" ) },
					  { RightSide, get_leg_phase( model, "leg1_r.state" ) } };

	// evaluate network up to time and excite muscles
	for ( const auto& segment : spinal_cord.segments )
	{
		for ( auto& motor_neuron : segment.second.motor_neurons )
		{
			auto muscle_name = motor_neuron.first.first;
			auto muscle_name_no_side = GetNameNoSide( muscle_name );
			auto side = GetSideFromName( muscle_name );
			auto u = ( ! MN_ACTIVE_STATE.empty( )
						   ? MN_ACTIVE_STATE[ muscle_name_no_side.c_str( ) ]
											[ leg_phase[ side ] ]
						   : 1 ) *
					 motor_neuron.second->get_output( timestamp );
			// auto u = motor_neuron.second->get_output( timestamp );
			auto& muscle =
				*TryFindByName( model.GetMuscles( ), muscle_name )->get( );
			muscle.AddInput( u );
		}
	}

	return false;
}

void SegmentalController::StoreData( Storage< Real >::Frame& frame,
									 const StoreDataFlags& flags ) const
{
	for ( const auto& segment : spinal_cord.segments )
	{
		for ( auto& neuron : segment.second.neurons )
		{
			// store neuron input and output
			auto name = neuron.first.first + "." + segment.first + "." +
						neuron.first.second;
			frame[ name + ".input" ] =
				neuron.second->get_input( frame.GetTime( ) );
			frame[ name + ".output" ] =
				neuron.second->get_output( frame.GetTime( ) );
		}
		for ( auto& synapse : segment.second.synapses )
		{
			// store the input_j * w_i for all connections so we can monitor the
			// influence of each neuron on other neurons
			auto name = synapse.first.first.first + "." + segment.first + "." +
						synapse.first.first.second + "->" +
						synapse.first.second.first + "." + segment.first + "." +
						synapse.first.second.second;
			frame[ name ] =
				synapse.second->w *
				synapse.second->source.lock( )->get_output( frame.GetTime( ) );
		}
	}

	// export graph only once with the initial parameters provided by the
	// optimizer
	if ( generate_graph )
	{
		generate_graph = ! generate_graph;
		for ( auto segment : spinal_cord.segments )
		{
			auto file_name = segment.first + ".gv";
			segment.second.export_graph( file_name );
		}
	}
}

/******************************************************************************/
// Phase oscillator.
/******************************************************************************/

PhaseOscillator::PhaseOscillator( const PropNode& props, Params& par,
								  Model& model )
	: StateComponent( props, par, model )
{
	// prefix for parameters related to the PhaseOscillator (phase_oscillator.)
	ScopedParamSetPrefixer prefixer( par, "phase_oscillator." );
	INIT_PAR( props, par, contact_delay, 50e-3 );
	INIT_PAR( props, par, stance_load_threshold, 0.1 );
	INIT_PAR( props, par, omega, 2 * pi( ) );
	INIT_PAR( props, par, gain, 1 );
	INIT_PAR( props, par, toe_off_percentage, 0.6 );
	INIT_PAR( props, par, synchronize_swing, true );
	initial_phase = props.get< vector< Real > >( "initial_phase" );
	INIT_PAR( props, par, phase_lock_gain, 0.5 );
	INIT_PAR( props, par, steps_ignore_phase_locking, 2 );

	SCONE_ASSERT_MSG( omega != 0, "omega != 0" );
	SCONE_ASSERT_MSG( toe_off_percentage >= 0 && toe_off_percentage <= 1,
					  " 0 <= toe_off_percentage <= 1" );
	SCONE_ASSERT_MSG( phase_lock_gain != 0, "phase_lock_gain != 0" );

	// phase lock
	T_prev = two_pi( ) / omega;
	t_left = t_right = 0;
	delta_omega = 0;
	steps = 0;

	// determine initial state
	if ( initial_phase[ 0 ] == 0 )
		state_left = STANCE;
	else
		state_left = SWING;

	if ( initial_phase[ 1 ] == 0 )
		state_right = STANCE;
	else
		state_right = SWING;

	// add events to detect heel strike and toe off for left and right leg
	m_events.push_back( new HeelStrike( this, &state_left, Side::LeftSide ) );
	m_events.push_back( new HeelStrike( this, &state_right, Side::RightSide ) );
}

vector< String > PhaseOscillator::GetStateNames( ) const
{
	return { "phase_left", "phase_right" };
}

vector< Real > PhaseOscillator::GetInitialConditions( ) const
{
	return initial_phase;
}

vector< Real > PhaseOscillator::CalcStateDerivatives( Real t,
													  vector< Real > x0 ) const
{
	// auto grf_left = GetLoadSensorValue( state_left, LeftSide );
	// auto grf_right = GetLoadSensorValue( state_right, RightSide );

	return {
		+omega // intrinsic frequency
			+ delta_omega // phase lock
			- gain * sin( x0[ 0 ] - x0[ 1 ] - pi( ) ) // phase coupling
		// - gain * grf_left *  cos( x0[ 0 ] + pi( ) ) // delay stance
		// + gain * grf_right * cos( x0[ 1 ] + pi( ) ) // speedup swing
		,
		+omega + delta_omega - gain * sin( x0[ 1 ] - x0[ 0 ] - pi( ) )
		// - gain * grf_right * cos( x0[ 1 ] + pi( ) )
		// + gain * grf_left *  cos( x0[ 0 ] + pi( ) )
	};
}

PhaseOscillator::HeelStrike::HeelStrike( PhaseOscillator* parent,
										 state_t* state, Side side )
	: StateComponent::DiscreteEvent( parent ), oscillator( parent ),
	  side( side ), state( state )
{
}

int PhaseOscillator::HeelStrike::TriggeredOnSign( ) const { return 0; }

Real PhaseOscillator::HeelStrike::CheckForEvent( Real t,
												 vector< Real > x ) const
{
	auto grf = oscillator->GetLoadSensorValue( *state, side ) -
			   oscillator->stance_load_threshold;
	return grf;
}

vector< Real >
PhaseOscillator::HeelStrike::EventHandler( Real t, vector< Real > x ) const
{
	auto grf = oscillator->GetLoadSensorValue( *state, side ) -
			   oscillator->stance_load_threshold;

	if ( *state != STANCE && grf > 0 )
	{
		*state = STANCE;
		if ( side == LeftSide )
		{
			oscillator->UpdateDeltaOmega( t, LeftSide );
			return { 0, x[ 1 ] };
		}
		else
		{
			oscillator->UpdateDeltaOmega( t, RightSide );
			return { x[ 0 ], 0 };
		}
	}
	else if ( *state != SWING && grf <= 0 )
	{
		*state = SWING;
		if ( oscillator->synchronize_swing )
		{
			if ( side == LeftSide )
				return { oscillator->toe_off_percentage * two_pi( ), x[ 1 ] };
			else
				return { x[ 0 ], oscillator->toe_off_percentage * two_pi( ) };
		}
	}

	return x;
}

void PhaseOscillator::UpdateDeltaOmega( double t, Side side ) const
{
	steps++;
	if ( side == LeftSide )
	{
		double T = t - t_left;
		t_left = t;
		T_prev += phase_lock_gain * ( T - T_prev );
		if ( steps > steps_ignore_phase_locking )
			delta_omega = two_pi( ) / T_prev - omega;
	}
	else
	{
		double T = t - t_right;
		t_right = t;
		T_prev += phase_lock_gain * ( T - T_prev );
		if ( steps > steps_ignore_phase_locking )
			delta_omega = two_pi( ) / T_prev - omega;
	}
}

Real PhaseOscillator::GetLoadSensorValue( state_t state, Side side ) const
{
	// it is important to handle this condition properly
	if ( m_model.GetSensorDelayStorage( ).IsEmpty( ) ||
		 m_model.GetTime( ) < contact_delay )
	{
		if ( side == LeftSide )
		{
			if ( state == SWING )
				return -1;
			else
				return 1;
		}
		else
		{
			if ( state == SWING )
				return -1;
			else
				return 1;
		}
	}
	return m_model
		.AcquireDelayedSensor< LegLoadSensor >(
			m_model.GetLeg( Location( side ) ) )
		.GetValue( contact_delay );
}

/******************************************************************************/
// Pattern formation
/******************************************************************************/

PatternFormationController::PatternFormationController( const PropNode& props,
														Params& par,
														Model& model,
														const Location& loc )
	: Controller( props, par, model, loc )
{
	INIT_PROP( props, include_muscles, "*" );

	// PatternFormationController exist prepend the name because
	// pattern weights might be in conflict. This was something that
	// Andrea observed.
	if ( GetName( ) != "" )
		ScopedParamSetPrefixer prefixer( par, GetName( ) + "." );

	// get patterns as Bezier functions if multiple
	get_patterns( props, par, patterns );

	// check name of phase oscillator
	SCONE_ASSERT_MSG( model.GetState( ).GetIndex( "phase_left" ) != NoIndex,
					  "PhaseOscillator name should be phase" );
	SCONE_ASSERT_MSG( model.GetState( ).GetIndex( "phase_right" ) != NoIndex,
					  "PhaseOscillator name should be phase" );

	// get unique actuator names without side (e.g., controller is symmetric)
	set< String > actuator_names;
	const auto& actuators = model.GetActuators( );
	for ( const auto* actuator : actuators )
	{
		auto actuator_name = GetNameNoSide( actuator->GetName( ) );
		if ( include_muscles( actuator_name ) )
			actuator_names.insert( actuator_name );
	}

	// get weight parameters between patterns and muscles
	for ( int i = 0; i < patterns.size( ); i++ )
	{
		map< String, double > temp;
		for ( const auto act_name : actuator_names )
		{
			auto name = "P" + to_string( i ) + "->" + act_name + ".w";
			auto w = par.try_get( name, props, "pattern_muscle_weight", 0.0 );
			temp[ act_name ] = w;
		}
		weights.push_back( temp );
	}
}

bool PatternFormationController::ComputeControls( Model& model,
												  double timestamp )
{
	// map time into a phase variable 0 <= phi <= 1
	auto phi_l = clamp( model.GetState( ).GetValue(
							model.GetState( ).GetIndex( "phase_left" ) ),
						0.0, two_pi( ) );
	auto phi_r = clamp( model.GetState( ).GetValue(
							model.GetState( ).GetIndex( "phase_right" ) ),
						0.0, two_pi( ) );
	// Bezier accepts 0 <= phi <= 1 so we map from [0, 2 pi] to [0, 1]
	phi_cache = make_pair( phi_l / two_pi( ), phi_r / two_pi( ) );

	// apply pattern to muscles
	auto& actuators = model.GetActuators( );
	for ( int i = 0; i < patterns.size( ); i++ )
	{
		for ( auto* actuator : actuators )
		{
			auto act_no_name = GetNameNoSide( actuator->GetName( ) );
			if ( ! include_muscles( act_no_name ) )
				continue;
			auto side = GetSideFromName( actuator->GetName( ) );
			auto phi = side == LeftSide ? phi_cache.first : phi_cache.second;
			actuator->AddInput( weights[ i ][ act_no_name ] *
								patterns[ i ]->GetValue( phi ) );
		}
	}

	return false;
}

void PatternFormationController::StoreData( Storage< Real >::Frame& frame,
											const StoreDataFlags& flags ) const
{
	map< string, double > total;
	for ( int i = 0; i < patterns.size( ); i++ )
	{
		for ( const auto pair : weights[ i ] )
		{
			total[ pair.first + "_l" ] +=
				pair.second * patterns[ i ]->GetValue( phi_cache.first );
			total[ pair.first + "_r" ] +=
				pair.second * patterns[ i ]->GetValue( phi_cache.second );
			frame[ "P" + to_string( i ) + "->" + pair.first + "_l" ] =
				pair.second * patterns[ i ]->GetValue( phi_cache.first );
			frame[ "P" + to_string( i ) + "->" + pair.first + "_r" ] =
				pair.second * patterns[ i ]->GetValue( phi_cache.second );
		}
	}

	// report total
	for ( auto pair : total )
		frame[ "P#." + pair.first ] = pair.second;
}