/****************************************************************/
/*                                                           	*/
/*                       detour.h			        */
/*                                                            	*/
/****************************************************************/

//////////////////////////////// DIMENSIONS /////////////////////////////////


//# define perc		0
//# define senmot	1
//# define motor	2

class World : NslModule
{
public:
	NslDinFloat0 xw;	
	NslDinFloat0 yw; 
	NslDinFloat0 zw;	
	NslDinFloat0 wangle;
	NslDinFloat0 xf;
	NslDinFloat0 yf; 
	NslDinFloat0 zf;
	NslDoutFloat2 worldXZ;   	
	NslDoutFloat0 yb;
	NslDoutFloat0 zb;	
	NslDoutFloat0 xb_init;	
	NslDoutFloat0 xb_end; 

private:
	NslFloat2 worldXY;

	nsl_input_3d in;
	
public:
	World(char*,NslModule*,int,int,int);
	~World() {}
	
	void initSys();
	void simRun();
};

class Prey : NslModule
{
public:
	NslDoutFloat0 xw;
	NslDoutFloat0 yw; 	
	NslDoutFloat0 zw; 	
	
private:
	NslFloat0 boundary; 
	NslFloat0 worm_speed;		

public:
	Prey(char*,NslModule*);
	~Prey() {}
	
	void simRun();
};

class VisualField : NslModule
{
public:
	NslDinFloat2 worldXZ;
	NslDinFloat0 zf; NslDinFloat0 xf;
	NslDoutFloat2 visual_field;
	
private:
	
public:
	VisualField(char*,NslModule*,int,int,int);
	~VisualField() {}
	
	void simRun();
};

class DepthField : NslModule
{
public:
	NslDinFloat0 yf;
	NslDinFloat0 xf; 	
	NslDinFloat0 yw;
	NslDinFloat0 xw; 	
	NslDinFloat0 yb;	
	NslDinFloat0 xb_init;	
	NslDinFloat0 xb_end;		
	NslDoutFloat0 ps;
	NslDoutFloat0 depthY;
	NslDoutFloat0 depthX;
		
private:
	NslFloat0 safe_distance;
	NslFloat0 th;
		
public:
	DepthField(char*,NslModule*);
	~DepthField() {}
	
	void initSys();	
	void simRun();

	void updateLabel();	
	void triggerSchema();
};

class Tactile : NslModule
{
public:
	NslDinFloat0 depthY;	
	NslDinFloat0 depthX;	
	NslDoutFloat0 ps;
	
private:
	NslFloat0 safe_distance;
	NslFloat0 th;
		
public:
	Tactile(char*,NslModule*);
	~Tactile() {}
	
	void initSys();	
	void simRun();

	void updateLabel();	
	void triggerSchema();
};

class PreyRec : NslModule
{
public:
	NslDinFloat2 visual_field;
	NslDoutFloat2 t5_2f;
	NslDoutFloat0 ps;
		
private:
	NslFloat0 th;
	NslFloat2 t5_2;		
	
public:
	PreyRec(char*,NslModule*,int,int,int);
	~PreyRec() {}

	void initSys();	
	void simRun();
	
	void updateLabel();
	void triggerSchema();
};

class SorRec : NslModule
{
public:
	NslDinFloat2 visual_field;
	NslDoutFloat2 th10f;			// Output Data Port.
	NslDoutFloat0 ps;
	NslDinFloat0 depthY; 
	NslDinFloat0 depthX; 
				
private:
	NslFloat0 th;
	NslFloat2 th10;		
	NslFloat0 edge;		// the edge of barrier
		
public:
	SorRec(char*,NslModule*,int,int,int);
	~SorRec() {}

	void initSys();	
	void simRun();
		
	void updateLabel();
	void triggerSchema();
};

class PreyApproach : NslModule
{
public:
	NslDinFloat2 t5_2f;			// Output Data Port.
	NslDinFloat0 psfo;
	NslDoutFloat0 ps;
	NslDoutFloat1 prey_hor_f;			
				
private:	
	NslFloat1 prey_hor;			// Parcellation		(1D).
	NslFloat1 prey_hor_default;
	NslFloat2 prey_f;			// PREY ATRACTANT FIELD (2D).
	NslFloat0 prey_norm;			// Normalization
	NslFloat2 t5_2_rf;		// KERNEL for Prey Attractant Field.
	NslFloat2 t5_2_erf; 
//	NslFloat2 t5_2_irf;
//	NslFloat0 t5_2_erf_dia;
	NslFloat0 t5_2_erf_sig; 
	NslFloat0 t5_2_erf_wgt;
	NslFloat0 rsfo;
	NslFloat0 th;
	
public:
	PreyApproach(char*,NslModule*,int,int);
	~PreyApproach() {}
	
	void initSys();	
	void simRun();
		
	void updateLabel();
	void triggerSchema();
};

class SorAvoid : NslModule
{
public:
	NslDinFloat2 th10f;			// Output Data Port.
	NslDinFloat0 psso;
	NslDoutFloat0 ps;
	NslDoutFloat1 gapsf;
					
private:
	NslFloat0 th;
	NslFloat0 rsso;
	NslFloat1 th10_hor;	// Parcellation		      (1D).
//	NslFloat1 tm_erf; 
	NslFloat1 tm_irf;
	NslFloat1 tm_rf;
//	NslFloat0 tm_erf_sig; 
	NslFloat0 tm_irf_sig;
//	NslFloat0 tm_erf_wgt; 
	NslFloat0 tm_irf_wgt;
//	NslFloat1 tm_rf_modul;	  	
	NslFloat1 gaps;			// openings within obstacles.
//	NslFloat0 gaps_norm;			// Normalization
				
public:
	SorAvoid(char*,NslModule*,int,int,int,int);
	~SorAvoid() {}

	void initSys();	
	void simRun();
		
	void updateLabel();
	void triggerSchema();
};

class BumpAvoid : NslModule
{
public:
	NslDinFloat0 depthY;	
	NslDinFloat0 depthX;	
	NslDinFloat0 psta;	
	NslDinFloat1 gaps;	
	NslDoutFloat1 gapsf;	
	NslDoutFloat0 ps;
	NslDoutFloat1 baf;				
		
private:
	NslFloat0 field_a;
	NslFloat0 field_center;
//	NslFloat1 field_side;
//	NslFloat1 last_orientation;
	NslFloat0 rsta;	
	NslFloat0 tune_tm;		// TUNE GAPS field (avoid bumping).
	NslFloat0 tune_tm_base;
	NslFloat1 tune_tm_layer;
	NslFloat0 safe_distance;
	NslFloat0 th;
		
public:
	BumpAvoid(char*,NslModule*,int);
	~BumpAvoid() {}
	
	void initSys();	
	void simRun();

	void tuningField();
	void updateLabel();	
	void triggerSchema();
};

class MHM : NslModule
{
public:
	NslDinFloat1 prey_hor_f;			
	NslDinFloat1 gapsf;
	NslDinFloat1 baf;	
	NslDinFloat0 pspa;	
	NslDinFloat0 psoa;	
	NslDoutFloat1 mhm;			// MOTOR HEADING MAP.
	NslDoutFloat0 ps;
				
private:	
	NslFloat1 mhm_hat;		// Predictive MHM.
	NslFloat1 in;
	NslFloat1 ip;			// PERCEPTUAL INTEGRATION.
//	NslFloat0 aip;
	NslFloat0 d_mhm;		// Coherence over MHM.
	NslFloat0 d_norm;
	NslFloat0 field_a;
	NslFloat0 field_center;
	NslFloat0 new_relation;	// temporary: new_relation
	NslInt0 learning;		// learning 1, otherwise 0
	NslFloat0 rspa;	
	NslFloat0 rsoa;	
	
public:
	MHM(char*,NslModule*,int);
	~MHM() {}
	
	void initSys();	
	void simRun();

	void coherence();
	void updateLabel();	
};

class WTA : NslModule
{
public:
	NslDinFloat1 mhm;		// MOTOR HEADING MAP.
	NslDoutFloat1 wta_mhm;		// MHM after WTA.
	
public:
	WTA(char*,NslModule*,int);
	~WTA() {}
	
	void simRun();
};

class Xform : NslModule
{
public:
	NslDinFloat0 pspa;	
	NslDinFloat0 psoa;	
	NslDinFloat1 wta_mhm;		
	NslDoutFloat0 ps;
	NslDoutFloat0 angle;		// Heading (target) angle.
				
private:	
	NslFloat1 angle_position;
	NslFloat1 gradient;		// Gradient of weights.
	NslFloat0 rspa;	
	NslFloat0 rsoa;	
	
public:
	Xform(char*,NslModule*,int);
	~Xform() {}
	
	void initSys();	
	void simRun();

	void updateLabel();	
};

class Forward : NslModule
{
public:
	NslDinFloat0 depthY;		
	NslDinFloat0 psfo;	
	NslDinFloat0 psta;	
	NslDinFloat0 psdp;	
	NslDinFloat0 psmhm;	
	NslDinFloat0 psbk;	
	NslDoutFloat0 ps;
	NslDoutFloat0 out; 
					
private:	
	NslFloat0 rsfo;	
	NslFloat0 rsta;	
	NslFloat0 rsdp;	
	NslFloat0 rsmhm;	
	NslFloat0 rsbk;	
	NslFloat0 th;
	NslFloat0 step;	
		
public:
	Forward(char*,NslModule*);
	~Forward() {}
	
	void initSys();	
	void simRun();

	void updateLabel();
	void triggerSchema();	
};

class Sidestep : NslModule
{
public:
	NslDinFloat0 angle;		// Heading (target) angle.
	NslDinFloat0 psso;	
	NslDinFloat0 psta;	
	NslDinFloat0 psba;	
	NslDinFloat0 psmhm;	
	NslDinFloat0 psx;	
	NslDoutFloat0 out;	
					
private:	
	NslDoutFloat0 ps;
	NslFloat0 rsso;	
	NslFloat0 rsta;	
	NslFloat0 rsba;	
	NslFloat0 rsmhm;	
	NslFloat0 rsx;	
	NslFloat0 th;
	
public:
	Sidestep(char*,NslModule*);
	~Sidestep() {}
	
	void initSys();	
	void simRun();

	void updateLabel();
	void triggerSchema();	
};

class Backward : NslModule
{
public:
	NslDinFloat0 psta;	
	NslDinFloat0 psba;	
	NslDinFloat0 psfw;	
	NslDoutFloat0 ps;
	NslDoutFloat0 out; 
					
private:	
	NslFloat0 rsta;	
	NslFloat0 rsba;	
	NslFloat0 rsfw;	
	NslFloat0 th;
	NslFloat0 step;
		
public:
	Backward(char*,NslModule*);
	~Backward() {}
	
	void initSys();	
	void simRun();

	void updateLabel();
	void triggerSchema();	
};

class Frog : NslModule
{
public:
	NslDinFloat2 worldXZ;
	NslDinFloat0 xb_end; 
	NslDinFloat0 yb; 
	NslDinFloat0 zb;
	NslDinFloat0 xb_init;			 
	NslDoutFloat0 xf; 
	NslDoutFloat0 yf; 
	NslDoutFloat0 zf;
	NslDinFloat0 xw;	
	NslDinFloat0 yw; 
//	NslDinFloat0 zw;	
	
	NslDoutFloat0 forward_step;
	NslDoutFloat0 backward_step;
	NslDoutFloat0 angle;
	NslDoutFloat0 wangle;

private:
	VisualField visual;
	DepthField depth;
	Tactile tactile;
	PreyRec preyRec;
	SorRec sorRec;
	PreyApproach preyApproach;
	SorAvoid sorAvoid;
	BumpAvoid bumpAvoid;
	MHM mhm;
	WTA wta;
	Xform xform;
	Forward forward;
	Sidestep sidestep;
	Backward backward;
	
public:
	Frog(char*,NslModule*,int,int,int,int,int,int,int);
	~Frog() {}
	
	void makeConn();
	
	void initSys();
	void endRun();
};

class Detour : public NslModel
{
private:
	World world;
	Prey prey;		
    Frog frog;

public:
        Detour();
        ~Detour() {}
        
       void makeConn();
};

class DetourLib // nslClass
{
public:
        DetourLib();
        ~DetourLib() {}
        
		static void gauss1D(NslFloat1&,NslFloat0&);
		static void gauss2D(NslFloat2&,NslFloat0&);
		static NslFloat1 maxim(NslFloat1&);
		static int avg_max(NslFloat1&);
		static NslFloat2 filter(NslFloat2&,int);
		static NslFloat0 dist(NslFloat1&, NslFloat1&);
};