//***************************************************************************
//			
//	file: detour.C
//
//
// 	TITLE: Learning to Detour.
//      GOAL:  Schema Construction and Dynamics based on SBL.
//
//	REFERENCES: 
//	------------
//	Refer to Corbacho and Arbib (1995) for details on detour itself.
//	Refer to Corbacho and Arbib (1997) for further details on Learning.
//
//	ARCHITECTURE: 
//	------------
//	Perceptual Schemas as functional units.
//	Fields corresponding to schema instances;
//	Integration of fields; WTA; Sensorimotor Xform;
//	Motor schemas as functional units.
//	
//
//
//	SCHEMA DYNAMICS:
//	--------
//	Relaxation Labeling (Hummel and Zucker, 1983).
//	Control of schemas based on distributed C&C dynamics.
//	Inclusion of NeuroSchemas.
//
//
//	SIMPLIFICATIONS:
//	---------------
//	Perceptual Schemas (not Neural Implementation).
//	Motor Schemas as functional units.
//
//
//	SCHEMA CONSTRUCTION:
//	--------------------
//	Autonomous Schema Construction in Run time.
// 	Coherence and Performance Maximization.
//
//
//
//	AUTHOR: Fernando Jose Corbacho
// 	DATE (last update): July 3rd, 98        (prev): July 2nd, 98.
//
//
// ***************************************************************************

#include "nsl_include.h"
#include "detour.h"	 

void gauss1D(nsl_vector& kernel,nsl_data& sigma)
{
  int xn = kernel.get_imax();
  float sig = sigma.get_data();
  float k = xn/2;
  const float PI = 3.14159;
  float f= sig*sqrt(2.0*PI);

	for (int i=-k; i <= k - (1 - xn%2); i++)
	    kernel.elem(i+k) = exp(-(i*i)/(2*sig*sig))/f;
}

void gauss2D(nsl_matrix& kernel,nsl_data& sigma)	
{
  int xn = kernel.get_imax();
  int yn = kernel.get_jmax();

  float sig = sigma.get_data();
  int k1 = xn/2;
  int k2 = yn/2;
  const float PI = 3.14159;
  float f= sig*sqrt(2.0*PI);

  for (int i=-k1; i<=k1 - (1 - xn%2); i++)
    for (int j=-k2; j<=k2 - (1 - yn%2); j++)
      kernel.elem(i+k1,j+k2) = exp(-(i*i+j*j)/(2*sig*sig))/f;
}

nsl_vector maxim(nsl_vector& u)
// returns the vector normalized (substraction) by its maximum.
// only the maximum is above threshold (by 0.01).
{
	int imax = u.get_imax();
	nsl_vector c(imax);

	float max = 0;			// Maximum value
	int max_i=0;			// Coordinates of max value
	
	max = NSLmax(u);
	if (max == 0)
	 	for (int i=0; i < imax; i++)
			c.elem(i)= - 0.01;
	else
		for (int i=0; i < imax; i++)
			c.elem(i)= u.elem(i) - max + 0.01;

	c = NSLstep(c);
	return c;
}

nsl_matrix filter(nsl_matrix& view, int f)
// filters out all the elements in the matrix that do not have a 
// value of "f". Simplified feature detector where "f" is the feature.
{
	int i,j;
	int imax = view.get_imax();
	nsl_matrix c(imax, imax);

	for (i=0; i<imax; i++)
	  for (j=0; j<imax; j++)
		if (view.elem(i,j) == f)
			c.elem(i,j) = 1;
		else 
			c.elem(i,j) = 0;	

	return c;
}

nsl_data Dist(nsl_vector& L1, nsl_vector& L2)
// Computes distance between two 1D patterns.
// Used to calculate Coherence.
{
	int imax = L1.get_imax();
	nsl_data c;
	c = 0;

	for (int i=0; i < imax; i++)
		c = c + ( (L1.elem(i) - L2.elem(i)) * 
				(L1.elem(i) - L2.elem(i)));
	return c;
}

nsl_data Corr(nsl_vector& X, nsl_vector& Y)
// Computes Correlation between two vectors
// Current implementation as similitude i.e. opposite of distance.
{
	return (1 - Dist(X, Y));
}

////////////////////////////// ENVIRONMENT /////////////////////////////////

World::World(char* str, NslModule* parent,int wx,int wy, int wz) : NslModule(str,parent),
	worldXZ("worldXZ",this, wx, wy),  
	worldXY("worldXY",this, wx, wz),
	wangle("wangle",this),
	yw("yw", this), xw("xw", this),	zw("zw", this),	// coordinates of the prey.
	xb_end("xb_end", this), yb("yb", this),	zb("zb", this),	// coordinates of the barrier.
	xb_init("xb_init", this),	
	xf("xf", this), yf("yf", this), zf("zf", this),		// COORDINATES of the frog.
	in("in",this,wx,wy,wx,wz)
{
}

void World::initSys()
{
//	worldXY = 0;
//	worldXZ = 0;
}

void World::simRun()
{
	in.set_transx(xf.get_value());
	in.set_transy(yf.get_value());
	in.set_angle(wangle.get_value());

	in.set_data("frog.xc",xf.get_value());
	in.set_data("frog.yc",yf.get_value());
	in.set_data("frog.angle",wangle.get_value());

	in.run();
	worldXZ = in.get_xzview();
}

Prey::Prey(char* str, NslModule* parent) : NslModule(str,parent),
	worm_speed("worm_speed", this),		
	boundary("boundary",this),
	xw("xw", this),		
	yw("yw", this), 
	zw("zw", this)		
{
}

void Prey::simRun()
{
/*	if (xw.elem() < boundary.elem()) 
		xw = xw + worm_speed.elem(); 	
	worm_speed = worm_speed * -1;		// Oscillatory behavior.
*/
}

VisualField::VisualField(char* str, NslModule* parent,int wx,int wz,int recsize) :
	NslModule(str,parent),
	visual_field("visual_field", this, recsize, recsize),
	worldXZ("worldXZ",this, wx,wz),  
	zf("zf", this), xf("xf", this)		
{
}

void VisualField::simRun()
{
	int recsize = visual_field.get_imax();
	int isize = worldXZ.get_imax();
	int jsize = worldXZ.get_jmax();
	int i2size = isize/2;
	int j2size = jsize/2;
	
	visual_field = 0;
	visual_field = worldXZ.get_sector(isize-recsize,isize-1,j2size-recsize/2,j2size+recsize/2-1);
}

DepthField::DepthField(char* str, NslModule* parent) : NslModule(str,parent),
	yf("yf",this),
	xf("xf",this),
	yw("yw",this),
	xw("xw",this),
	yb("yb",this),
	xb_init("xb_init",this),
	xb_end("xb_end",this),
	safe_distance("safe_distance",this),
	depthY("depthY",this),
	depthX("depthX",this),
	ps("ps",this),
	schema("schema",this),
	th_sch("th_sch",this)		
{
}

void DepthField::initSys()
{
	ps = 0;
	schema = 0;
}

void DepthField::simRun()
{
	if (yf.elem() > yb.elem())
		depthY = yf - yb;
	else 
		depthY = 0;

	if (xf.elem() > xb_end.elem())
		depthX = xf - xb_end;
	else if (xf.elem() < xb_init.elem())
		depthX = xb_init - xf;
	else
		depthX = 0;

	// this is not used
	if (depthY.elem() > safe_distance.elem() || depthX.elem() > safe_distance.elem())     // safe_distance.elem() 
		ps= 2;			// Go up fast
	else 
		ps = 0.0;
		
	triggerSchema();
} 

void DepthField::triggerSchema()
{
	if (schema.elem() > th_sch.elem()) {	
		printf("Depth \t");
	}
}

Tactile::Tactile(char* str, NslModule* parent) : NslModule(str,parent),
	safe_distance("safe_distance",this),
	depthY("depthY",this),
	depthX("depthX",this),
	ps("ps",this),
	schema("schema",this),
	th_sch("th_sch",this)		
{
}

void Tactile::initSys()
{
	ps = 0;
	schema = 0;
}

void Tactile::simRun()
{
	if (depthY.elem() > 0 && depthY.elem() <= safe_distance.elem() && depthX.elem() <= safe_distance.elem())      
		ps = 1; // AW99 ps=2;			// Go up fast
	else 
		ps = 0.0;		// Go down fast: -2.0

	updateLabel();
	triggerSchema();
} 

void Tactile::updateLabel()
{
//	ps = ps + qs;
//AW99    ps = NSLsigma(ps,-1.0,1.0,-1.0,1.0);	
	schema = ps;
}

void Tactile::triggerSchema()
{
	if (schema.elem() > th_sch.elem()) {	
		printf("Tactile \t");
	}
}

PreyRec::PreyRec(char* str, NslModule* parent, int recsize,int tx,int ty) : 
	NslModule(str,parent),
	visual_field("visual_field", this, recsize, recsize),
	t5_2f("t5_2f", this,tx,ty),			// Output Data Port.
	t5_2("t5_2", this,tx,ty),
	ps("ps",this),
	schema("schema",this),
	th_sch("th_sch",this)		
{
}

void PreyRec::initSys()
{
	ps = 0;
	schema = 0;
}

void PreyRec::simRun()
{	
	t5_2 = filter(visual_field,2);		// INSTANTIATED Schema.
	if (t5_2.sum() >= 1) // orig >1
		ps=0.9;		// Confidence									  
	else 
		ps=0;

	updateLabel();
	triggerSchema();
}

void PreyRec::updateLabel()
{
//	ps = ps + qs;
//AW99    ps = NSLsigma(ps,-1.0,1.0,-1.0,1.0);	

	schema = ps;
}

void PreyRec::triggerSchema()
{
	if (schema.elem() > th_sch.elem()) {
		printf("Prey Recog \t");		// ASSERTED Schema.
		t5_2f = NSLramp(t5_2);			// Output to other Schemas
	}
	else
		t5_2f = 0;
}

SorRec::SorRec(char* str, NslModule* parent, int recsize,int tx,int ty) : 
	NslModule(str,parent),
	visual_field("visual_field", this, recsize, recsize),
	th10f("th10f", this,tx,ty),	
	th10("th10", this,tx,ty),				
	depthY("depthY", this),		
	depthX("depthXY", this),	
	edge("edge",this),		// the edge of barrier
	ps("ps",this),
	schema("schema",this),
	th_sch("th_sch",this)
{
}

void SorRec::initSys()
{
	ps = 0;
	schema = 0;
}

void SorRec::simRun()
{	
    th10 = filter(visual_field, 1);	 // view far from barrier.

	if (th10.sum() >= 1) // orig >1
		ps=0.9;		// Confidence
	else 
		ps=0;  
		
	updateLabel();
	triggerSchema();
}

void SorRec::updateLabel()
{
//	ps = ps + qs;
 //AW99   ps = NSLsigma(ps,-1.0,1.0,-1.0,1.0);	
	schema = ps;
}

void SorRec::triggerSchema()
{
	if (schema.elem() > th_sch.elem())	{
		printf("SO Recog \t");
		th10f =	NSLramp(th10);  		// Output to other Schemas
	}
	else
		th10f = 0;
}

PreyApproach::PreyApproach(char* str, NslModule* parent,int tx,int ty) : 
	NslModule(str,parent),
	t5_2f("t5_2f", this,tx,ty),			// Output Data Port.
	prey_hor_f("prey_hor_f", this, tx),		// OUTPUT Data Port.
	prey_hor("prey_hor", this, tx),		// Parcellation		(1D).
	prey_hor_default("prey_hor_default", this, tx),
	prey_f("prey_f", this, tx, ty),		// PREY ATRACTANT FIELD (2D).
	prey_norm("prey_norm", this),		// Normalization
	t5_2_rf("t5_2_rf", this, 80, 80),	// KERNEL for Prey Attractant Field.
	t5_2_erf("t5_2_erf", this, 80, 80), 
//	t5_2_irf("t5_2_irf", this, 40, 40),
	t5_2_erf_sig("t5_2_erf_sig", this), 
	t5_2_erf_wgt("t5_2_erf_wgt", this),
	psfo("psfo",this),
	rsfo("rsfo",this),
	ps("ps",this),
	schema("schema",this),
	th_sch("th_sch",this)		
{
}

void PreyApproach::initSys()
{
	rsfo = 1.0;	// Prey & Prey Approach.
	ps = 0;
	schema = 0;

//	t5_2_erf = t5_2_erf_wgt * gauss2D(t5_2_erf_sig, t5_2_erf_dia, t5_2_erf_dia);
//	t5_2_rf = t5_2_erf;
	gauss2D(t5_2_erf,t5_2_erf_sig);
	t5_2_rf = t5_2_erf_wgt * t5_2_erf;
}

void PreyApproach::simRun()
{
	prey_f = t5_2_rf * t5_2f;			//** 2D: newconv(t5_2_rf, T5_2);	
	prey_hor = prey_f.reduce_row();		// Parcellation: horizontal comp.

	updateLabel();
	triggerSchema();
}

void PreyApproach::updateLabel()
{
	ps = rsfo*psfo; // ps + 
    ps = NSLsigma(ps,-1.0,1.0,-1.0,1.0);	
	schema = ps;
}

void PreyApproach::triggerSchema()
{									
	if (schema.elem() > th_sch.elem())	{
		printf("Prey Appro \t");
		float mx = NSLmax(prey_hor);	
		if (mx != 0.0)
			prey_hor_f = prey_hor/mx;	// Normalize.	// ASSERTED: Affect other schemas.
	}
	else
		prey_hor_f = 0;
}

SorAvoid::SorAvoid(char* str, NslModule* parent,int tectsize,int tx,int ty,int rrf) : 
	NslModule(str,parent),
	th10f("th10f", this,tx,ty),
	th10_hor("th10_hor", this, tx),			// Parcellation		
	// KERNEL for SO Avoid Field
//	tm_erf("tm_erf", this, rrf*2), 
	tm_irf("tm_irf", this, rrf*2), 
	tm_rf("tm_rf", this, rrf*2),
//	tm_erf_sig("tm_erf_sig", this), 
	tm_irf_sig("tm_irf_sig", this),
//	tm_erf_wgt ("tm_erf_wgt", this), 
	tm_irf_wgt("tm_irf_wgt", this),
//	tm_rf_modul("tm_rf_modul", this, rrf*2),	// modulated kernel.
	gaps("gaps", this, tectsize),			// openings within obstacles.
//	gaps_norm("gaps_norm", this),			// Normalization
	gapsf("gapsf", this, tectsize),	
	psso("psso",this),	
	rsso("rsso",this),			
	ps("ps",this),
	schema("schema",this),
	th_sch("th_sch",this)
{
}

void SorAvoid::initSys()
{
	rsso = 1.0;	// Obstacle & Obstacle Avoid
	ps = 0;
	schema = 0;

// OBSTACLE AVOIDANCE KERNEL
	
//	tm_irf = tm_irf_wgt * gauss1D(tm_irf_sig, tm_irf_dia.elem());
//	tm_rf = - tm_irf;
	gauss1D(tm_irf,tm_irf_sig);
	tm_rf = - tm_irf_wgt * tm_irf;
//	tm_rf_modul = tm_rf;
}

void SorAvoid::simRun()
{
	th10_hor = th10f.reduce_row();		// Parcellation (from 2D to 1D).
	gaps = tm_rf * th10_hor;		// Convolve with kernel.	
				
	updateLabel();
	triggerSchema();
}

void SorAvoid::updateLabel()
{
	ps = rsso*psso; // ps + 
    ps = NSLsigma(ps,-1.0,1.0,-1.0,1.0);	
	schema = ps;
}

void SorAvoid::triggerSchema()
{
	if (schema.elem() > th_sch.elem())	{
		printf("SOR avoid \t");
		float mx = NSLmax(gaps*-1);	
		if (mx != 0.0)
			gapsf = gaps/mx;	// ASSERTED: Affect other schema	
	}
	else 
		gapsf = 0;
}

BumpAvoid::BumpAvoid(char* str, NslModule* parent,int tectsize) : NslModule(str,parent),
	baf("baf", this, tectsize),				
	gaps("gaps", this, tectsize),
	gapsf("gapsf", this, tectsize),
	field_A("field_A", this),
	field_center("field_center", this),
//	field_side("field_side", this, tectsize),
//	last_orientation("last_orientation", this,tectsize),
	tune_tm("tune_tm", this),			// TUNE GAPS field (avoid bumping).
	tune_tm_base("tune_tm_base", this),
	tune_tm_layer("tune_tm_layer", this, tectsize),
	safe_distance("safe_distance",this),
	depthY("depthY",this),
	depthX("depthX",this),
	ps("ps",this),
	psta("psta",this),
	rsta("rsta",this),
	schema("schema",this),
	th_sch("th_sch",this)		
{
}

void BumpAvoid::initSys()
{
	rsta = 1.0;	// Tactile & Bumping Avoid
	ps = 0;
	schema = 0;
}

void BumpAvoid::simRun()
{
	// both should be larger than zero for no bump
	if (depthY.elem() <= safe_distance.elem() && depthX.elem() <= safe_distance.elem()) 	//Bumping.   
		ps = 0;
	else
		ps = -1.0;		// Go down fast: -2.0

	tuningField(); // TUNING the KERNEL
				
	updateLabel();
	triggerSchema();
}

void BumpAvoid::updateLabel()
{
	ps = ps + rsta*psta;
    ps = NSLsigma(ps,-1.0,1.0,-1.0,1.0);	
	schema = ps;
}

void BumpAvoid::tuningField()
{
	if (tune_tm.elem() < 1.5)		// saturate tune_tm.
			tune_tm = tune_tm + tune_tm_base;

	tune_tm_layer = tune_tm;
	tune_tm_layer = tune_tm_layer ^ NSLstep(-gaps);	// Modulate only already active neurons.

	gaps = gaps - tune_tm_layer; // GAPS fiels gets tuned.

	gapsf = gaps;
}

void BumpAvoid::triggerSchema()
{
	if (schema.elem() > th_sch.elem())	{
		printf("Bump Avoid \t");
					
// every time it bumps it increases tune_tm until it reaches a saturation point.

		field_center = field_center +.3;	// Tunes the B_A field by increasing eccentricity.

		// Generate Bumping Avoidance FIELD.
		baf.elem(floor(field_center.elem())) = field_A.elem();
	}
	else
		baf = 0;
}

MHM::MHM(char* str, NslModule* parent, int tectsize) : NslModule(str,parent),
	baf("baf", this, tectsize),				
	gapsf("gapsf", this, tectsize),
	prey_hor_f("prey_hor_f", this, tectsize),				
	ip("ip", this, tectsize),		// PERCEPTUAL INTEGRATION.
//	aip("aip", this),
	mhm("mhm", this, tectsize),	
	in("in", this, tectsize),
	mhm_hat("mhm_hat", this, tectsize),		// Predictive MHM.
	new_relation("new_relation", this),	// temporary: new_relation
	field_A("field_A", this),
	field_center("field_center", this),
	d_mhm("d_mhm", this),				// Coherence over MHM.
	d_norm("d_norm", this),
	ps("ps",this),
	pspa("pspa",this),
	rspa("rspa",this),
	psoa("psoa",this),
	rsoa("rsoa",this),
	schema("schema",this)	
{
}

void MHM::initSys()
{
	rspa = 1.0;	
	rsoa = 1.0;	
	ps = 0;
	schema = 0;
}
void MHM::simRun()
{
	// Dynamics of Motor Heading Map (MHM): integrating several fields.
	// New fields can also be added while learning.

	if (new_relation.elem() == 1)
	{			 
//		baf.elem(floor(field_center.elem())) = field_A.elem();
		in = baf; // New Field "inserted"
	}
	else
		in = 0; // reset input (cf. antidromic
		
	mhm_hat = mhm;					// Predictive MHM.
 
	ip = gapsf  +  prey_hor_f + baf;		// Fields over MHM.
	mhm = ip + in;
	

//AW99	coherence();
	
	updateLabel();
}

void MHM::updateLabel()
{
	ps = ps + rspa*pspa + rsoa*psoa;
    ps = NSLsigma(ps,-1.0,1.0,-1.0,1.0);	
	schema = ps;
}

void MHM::coherence()
{
// Learning Dynamics: it detects an incoherence and hence a trigger
// for a new schema.

	d_mhm = 0.03 * Dist(mhm, mhm_hat);

	if (d_mhm.elem() > d_norm.elem())		// Incoherence.
		new_relation.elem() = 1;
}

WTA::WTA(char* str, NslModule* parent, int tectsize) : NslModule(str,parent),
	mhm("mhm", this, tectsize),	
	wta_mhm("wta_mhm", this,tectsize)
{
}

void WTA::simRun()
{		
	wta_mhm = maxim(mhm);	
}

Xform::Xform(char* str, NslModule* parent, int tectsize) : NslModule(str,parent),
	wta_mhm("wta_mhm", this, tectsize),		// Predictive MHM.
	angle_position("angle_position", this, tectsize),
	gradient("gradient", this, tectsize),		// Gradient of weights.
	angle("angle", this),				// Heading (target) angle.
	ps("ps",this),
	pspa("pspa",this),
	rspa("rspa",this),
	psoa("psoa",this),
	rsoa("rsoa",this),
	schema("schema",this)	
{
}

void Xform::initSys()
{
	rspa = 1.0;	
	rsoa = 1.0;	
	ps = 0;
	schema = 0;
}
void Xform::simRun()
{
// Sensory motor Transformation from retinotopic coordinates to population coding.

	int i;
//	angle_position = gradient ^ wta_mhm; 
	wta_mhm.max(i); 

//	angle = int (floor(angle_position.sum()));	//eccentricity
	angle = 0;
	if (i != 0)
		angle = i - wta_mhm.get_imax()/2; 

	angle.nslPrint();
	
	updateLabel();
}

void Xform::updateLabel()
{
	ps = ps + rspa*pspa + rsoa*psoa;
    ps = NSLsigma(ps,-1.0,1.0,-1.0,1.0);	
	schema = ps;
}

Forward::Forward(char* str, NslModule* parent) : NslModule(str,parent),
	depthY("depthY", this),				// Heading (target) angle.
	step("step", this),		
	out("out", this), 
	ps("ps",this),
	psfo("psfo",this),
	rsfo("rsfo",this),
	psta("psta",this),
	rsta("rsta",this),
	psdp("psdp",this),
	rsdp("rsdp",this),
	psmhm("psmhm",this),
	rsmhm("rsmhm",this),
	psbk("psbk",this),
	rsbk("rsbk",this),
	schema("schema",this),
	th_sch("th_sch",this)	
{
}

void Forward::initSys()
{
	rsfo = 0.5;
	rsta = -1.0;
	rsdp = 1.0;			
	rsmhm = 0.5;	
	rsbk = -1.0;
		
	ps = 0;
	schema = 0;
}
void Forward::simRun()
{			
	updateLabel();
	triggerSchema();
}
void Forward::updateLabel()
{
	ps = ps + rsfo*psfo + rsta*psta + rsdp*psdp + rsmhm*psmhm + rsbk*psbk;
    ps = NSLsigma(ps,-1.0,1.0,-1.0,1.0);	
	schema = ps;
}

void  Forward::triggerSchema()
{
	out = 0;
	
	if (schema.elem() > th_sch.elem())	 {
        ps = -1.0;				// Reset        
        out = step;
	}
}

Sidestep::Sidestep(char* str, NslModule* parent) : NslModule(str,parent),
	angle("angle", this),				// Heading (target) angle.
	out("out", this),		
	ps("ps",this),
	psso("psso",this),
	rsso("rsso",this),
	psta("psta",this),
	rsta("rsta",this),
	psba("psba",this),
	rsba("rsba",this),
	psmhm("psmhm",this),
	rsmhm("rsmhm",this),
	psx("psx",this),
	rsx("rsx",this),
	schema("schema",this),
	th_sch("th_sch",this)	
{
}

void Sidestep::initSys()
{
	rsso = 0.5;
	rsta = 1.0;
//	rsdp = -1.0;	
	rsba = 0.5;			
	rsmhm = 0.5;	
	rsx = 0.5;	
		
	ps = 0;
	schema = 0;
}
void Sidestep::simRun()
{
	updateLabel();
	triggerSchema();
}
void Sidestep::updateLabel()
{
	ps = ps + rsso*psso + rsta*psta + rsba*psba + rsmhm*psmhm + rsx*psx;
    ps = NSLsigma(ps,-1.0,1.0,-1.0,1.0);	
	schema = ps;
}

void Sidestep::triggerSchema()
{
	out = 0;
	
	if (schema.elem() > th_sch.elem())	 {
        ps = 0; // -1.0;			// Reset
		out = angle;
	}
}

Backward::Backward(char* str, NslModule* parent) : NslModule(str,parent),
	step("step", this),	// SPEEDS
	out("out", this), 
	ps("ps",this),
	psta("psta",this),
	rsta("rsta",this),
	psba("psba",this),
	rsba("rsba",this),
	psfw("psfw",this),
	rsfw("rsfw",this),
	schema("schema",this),
	th_sch("th_sch",this)	
{
}

void Backward::initSys()
{
	rsta = 1.0;
	rsba = 1.0;		
	rsfw = -1.0;	
		
	ps = 0;
	schema = 0;
}
void Backward::simRun()
{		
	updateLabel();
	triggerSchema();
}
void Backward::updateLabel()
{
	ps = ps + rsta*psta + rsba*psba + rsfw*psfw;
    ps = NSLsigma(ps,-1.0,1.0,-1.0,1.0);	
	schema = ps;
}

void Backward::triggerSchema()
{
	out = 0;
	
	if (schema.elem() > th_sch.elem())	 {
        ps = -1.0;			// Reset
		out = step;
	}
}

// Frog

Frog::Frog(char* str, NslModule* parent,int wx,int wz,int recsize,int tectsize,
	int tx,int ty,int rrf) :
	NslModule(str,parent),
	visual("visual",this,wx,wz,recsize),
	depth("depth",this),
	tactile("tactile",this),
	preyRec("preyRec",this,recsize,tx,ty),
	sorRec("sorRec",this,recsize,tx,ty),
	preyApproach("preyApproach",this,tx,ty),
	sorAvoid("sorAvoid",this,tectsize,tx,ty,rrf),
	bumpAvoid("bumpAvoid",this,tectsize),
	mhm("mhm",this,tectsize),
	wta("wta",this,tectsize),
	xform("xform",this,tectsize),
	forward("forward",this),
	sidestep("sidestep",this),
	backward("backward",this),
			
	worldXZ("worldXZ",this, wx,wz),  
	xf("xf", this), yf("yf", this),	zf("zf", this),	
	xw("xw", this), yw("yw", this),	// zf("zf", this),	
	xb_end("xb_end", this), yb("yb", this),	zb("zb", this),	
	xb_init("xb_init", this),
	
	forward_step("forward_step", this),
	backward_step("backward_step", this),
	wangle("wangle", this),
	angle("angle", this)
{
	makeConn();
}

void Frog::makeConn()
{
	relabel(worldXZ,visual.worldXZ);
	relabel(xb_init,depth.xb_init);
	relabel(xb_end,depth.xb_end);
	relabel(yb,depth.yb);
	relabel(sidestep.out,angle);
	relabel(forward.out,forward_step);
	relabel(backward.out,backward_step);
	relabel(xw,depth.xw);	
	relabel(yw,depth.yw);

	connect(xf,visual.xf);
	connect(zf,visual.zf);
	connect(xf,depth.xf);	
	connect(yf,depth.yf);

	connect(visual.visual_field,preyRec.visual_field);
	connect(visual.visual_field,sorRec.visual_field);
	connect(depth.ps,forward.psdp);	
	connect(depth.depthY,tactile.depthY);
	connect(depth.depthX,tactile.depthX);
	connect(depth.depthY,sorRec.depthY);
	connect(depth.depthX,sorRec.depthX);
	connect(depth.depthY,bumpAvoid.depthY);
	connect(depth.depthX,bumpAvoid.depthX);
	connect(depth.depthY,forward.depthY);
	connect(tactile.ps,bumpAvoid.psta);	
	connect(tactile.ps,forward.psta);	
	connect(tactile.ps,sidestep.psta);	
	connect(tactile.ps,backward.psta);	
	connect(preyRec.t5_2f,preyApproach.t5_2f);
	connect(preyRec.ps,preyApproach.psfo);		
	connect(preyRec.ps,forward.psfo);	
	connect(sorRec.th10f,sorAvoid.th10f);
	connect(sorRec.ps,sorAvoid.psso);	
	connect(sorRec.ps,sidestep.psso);
	connect(preyApproach.prey_hor_f,mhm.prey_hor_f);		
	connect(preyApproach.ps,mhm.pspa);	
	connect(preyApproach.ps,xform.pspa);	
	connect(sorAvoid.gapsf,bumpAvoid.gaps);		
	connect(sorAvoid.ps,mhm.psoa);	
	connect(sorAvoid.ps,xform.psoa);	
	connect(bumpAvoid.baf,mhm.baf);		
	connect(bumpAvoid.ps,sidestep.psba);	
	connect(bumpAvoid.ps,backward.psba);	
	connect(bumpAvoid.gapsf,mhm.gapsf);		
	connect(mhm.ps,forward.psmhm);	
	connect(mhm.ps,sidestep.psmhm);	
	connect(mhm.mhm,wta.mhm);	
	connect(wta.wta_mhm,xform.wta_mhm);	
	connect(xform.angle,sidestep.angle);	
	connect(xform.ps,sidestep.psx);	
	connect(forward.ps,backward.psfw);	
	connect(backward.ps,forward.psbk);	
}

void Frog::initSys()
{		
}

void Frog::endRun()
{
	double pi = 3.1415926535;
	double edge = 3;

	double x0 = xf.elem() - xb_init.elem();
	double x2 = xb_end.elem() - xf.elem();
	double yd = yf.elem() - yb.elem();

	double x1 = xf.elem() - xw.elem();
	double y1 = yf.elem() - yw.elem();

	double xd = angle.elem() - yd*x1/y1; // x displacement (needs framework rotation)

	double iangle = 0;
//	double iangle = angle.elem()*pi/180;
//	double t = tan(iangle);

	double t = atan(xd/yd);
	double disp = forward_step.elem() - backward_step.elem(); // overall displacement
	double dispx = fabs(disp)*sin(t);
	double dispy = disp*cos(t);
	double dispx0 = dispx;
	double dispy0 = dispy;

	if (yf.elem() > yb.elem() && (yf.elem() - dispy < yb.elem() + edge) && 
		(xf.elem() + dispx > xb_init.elem() - edge) && (xf.elem() + dispx < xb_end.elem() + edge)) 
	{
		dispy = yf.elem() - yb.elem() - edge; // maximum movement
		dispx = dispx0*dispy/dispy0;
	}

	yf = yf - dispy;

	if ((yf.elem() >= yb.elem() - edge) && (yf.elem() <= yb.elem() + edge))
	{
		if ((xf.elem() + dispx < xb_init.elem()) && (xf.elem() + dispx > xb_init.elem() - edge))
			dispx = xb_init.elem() - xf.elem() - edge; // maximum movement
		else if ((xf.elem() + dispx > xb_end.elem()) && (xf.elem() + dispx < xb_end.elem() + edge))
			dispx = xf.elem() - xb_end.elem() + edge; // maximum movement
	}

	xf = xf + dispx; 

	NSLoutput("dispx: ",dispx);
	NSLoutput("dispy: ",dispy);

	if (yf.elem() <= yw.elem())
	{
		NSLoutput("\nAte Worm");
		yf = yw;
		xf = xw; 

		nslBreakEpoch();
		return;
	}

	NSLoutput("xf: ",xf.elem());
	NSLoutput("yf: ",yf.elem());
		
	double x = 0, y = 0;

	x = xf.elem() - xw.elem();
	y = yf.elem() - yw.elem();

	if (y != 0) {
		iangle = atan(x/y);
		wangle = iangle*180/pi;
	}
	else {
		iangle = pi/2;
		wangle = 90;
	}
}

/*
void Frog::Schemas_Display()
{
	for (int i=0; i< Sch; i++)
		Schemas_disp.elem(i,0) = Schemas.elem(i);
	
	Schemas_disp.elem(Sch-1,0) = d_mhm.elem() - d_norm.elem();
}
*/

Detour::Detour() : NslModel("Detour"), 
	world("world",this,200,200,200),
	prey("prey",this),
    frog("frog",this,200,200,80,80,80,80,40)
{
	makeConn();
}

void Detour::makeConn()
{
	connect(world.worldXZ,frog.worldXZ);
	connect(world.xb_init,frog.xb_init);
	connect(world.xb_end,frog.xb_end);
	connect(world.yb,frog.yb);
	connect(world.zb,frog.zb);
	
	connect(frog.xf,world.xf);
	connect(frog.yf,world.yf);
	connect(frog.zf,world.zf);
	connect(frog.wangle,world.wangle);

	//AW99
	connect(prey.xw,frog.xw);
	connect(prey.yw,frog.yw);

	connect(prey.xw,world.xw);
	connect(prey.yw,world.yw);
	connect(prey.zw,world.zw);
}

AslSchemaModel<Detour> _Detour("Detour");