/* 
 * Thomas Pantzer: Predict_demo.cc, v0.4.7  8.6.99 16:32 
 * 
 * 
 * Copyright 1999 by Thomas Pantzer
 * 
 * Permission to use, copy, modify, and distribute this software for noncommercial 
 * use and without fee is hereby granted, provided that the above copyright notice 
 * appear in all copies and that both that copyright notice and this permission 
 * notice appear in supporting documentation, and that the name of the Author not be 
 * used in advertising or publicity pertaining to distribution of the software 
 * without specific, written prior permission. The Author makes no representations about
 * the suitability of this software for any purpose.  It is provided "as is" 
 * without expressed or implied warranty. 
 * 
 * 
 * pantec@isys2.informatik.uni-leipzig.de   (Thomas Pantzer) 
 * 
 */




#include <forms.h>
#include <math.h>
#include <stdio.h>
#include <unistd.h>
#include <fcntl.h>
#include <string.h>

#include "copyright.h"

#include "new_panel.h"
#include "trigonom.h"
#include "neuronal_demo.h"

#include "t_runtime_obj.h"

#include "graphix.h"
#include "stuff.h"
#include "user.h"

#include "CrashTest_demo.h"


#define _predict_DEMO_G
#include "Predict_demo.h"


#define predict_nBoxes	4
#define predict_nSliders	8


#define _sl_num		2 // we have to add 2 to the slider num, because 
			  // we are using the  _p_logctl feature
#define _sl_freq	(0 + _sl_num)	
#define _sl_learn	(1 + _sl_num)
#define _sl_maxspeed	(2 + _sl_num)
#define _sl_sinamp	(3 + _sl_num)
#define _sl_speed	(4 + _sl_num)
#define _sl_ampl	(5 + _sl_num)
#define _sl_wheel_ss	(6 + _sl_num)
#define _sl_couple	(7 + _sl_num)


#define _bt_disturb	(0 + _bt_num)
#define _bt_random	(1 + _bt_num)
#define _bt_bias	(2 + _bt_num)
#define _bt_usine	(3 + _bt_num)




char predict_boxes[predict_nBoxes][32] = {"add noise\0"
				     ,"init random/ load\0"
				     ,"ignore bias\0"
				     ,"use sine\0"
				     };

//				     ,"c*(X[i]-X[i+1])\0"
//				     ,"c*(X[i]*X[j])\0"
//				     ,"c*(X[i]*X[j])\0"

char predict_sliders[predict_nSliders][16] = {
		"freq.\0",
		"learn\0",
		"max.speed\0",
		"störung\0",
		"speed\0",
		"ampl.\0",
		"wheel ss.\0",
		"couple\0"};


void t_predict_demo::Init(RobotContext *robot)
{
	char *cb_pointers[predict_nBoxes];
	char *sl_pointers[predict_nSliders];

	for (int i=0; i<predict_nBoxes; i++)
          cb_pointers[i]  = &predict_boxes[i][0];

	for (int i=0; i<predict_nSliders; i++)
          sl_pointers[i]  = &predict_sliders[i][0];

	int pro = 0; 
	pro |= _p_control;	//  does something to control the robot
	pro |= _p_calc;		//  needs calculations
	pro |= _p_extras;	//  uses user settable parameters
	pro |= _p_logctl;	//  uses user settable parameters

	t_runtime_object::Init("Predict",robot,pro,
		predict_nSliders,predict_nBoxes,0,
		sl_pointers,cb_pointers,NULL);

	g_radius.Init(t_depth);
	sinus.Init(t_depth);
	cosin.Init(t_depth);

	ClearP = NULL;

	Poly = new t_Polynom;
	Poly->Init(1,10);
	for (int i=0; i<12; i++)
	  {
	   Poly->SetVal(0,i,0.0);
	   Poly->UseVal(0,i,true);

	   Sensor[i].Init(t_depth); 
	   q_sin[i].Init(14);
	   q_cos[i].Init(14);
	   qx[i].Init(14);
	   dxka[i].Init(3); 	
	   dxpa[i].Init(14); 	
	  }

	Poly->UseVal(0,8,false);                                                              
	Poly->UseVal(0,9,false);                                                              

	for (int i=0; i<2; i++)
	   Motor[i].Init(t_depth); 

	Va.Init(20);
	Sl.Init(20);
	Dva.Init(20);
	alpha_a.Init(20);
	ES.Init(20);
	EM.Init(20);
// logging

	Qv.Init(14); 
	RNS.Init(10);

	Reset();

	ClearP = new t_Polynom;
	ClearP->Init(Poly);

//	fprintf(stderr,"fd:0x%08lx\n",Robot->_FD_);
}


                                                                                                
void t_predict_demo::Reset(void)                                                                  
{                                                                                               
	Poly->Reset(ClearP); 

	for (int i=0; i<2; i++)
	   Motor[i].Reset(); 

	for (int i=0; i<10; i++)
	  {
	   Sensor[i].Reset(); 

           a[i] = 0.0;
	   dxk[i] = 0.0;
	   dxp[i] = 0.0;

	   q_sin[i].Reset();
	   q_cos[i].Reset();
	   qx[i].Reset();

	   dxka[i].Reset(); 	
	   dxpa[i].Reset(); 

	   D[i] = 0.0;	
	  } 

// logging
	good = 0;
	bad = 0;
        alpha = 0.0;   
	step = 0;

	Va.Reset();
	Sl.Reset();
	Dva.Reset();
	alpha_a.Reset();
	ES.Reset();
	EM.Reset();
// logging

	Qv.Reset();
	RNS.Reset();

	sinus.Reset();
	cosin.Reset();
	g_radius.Reset();

	V = 0.0;
	EL = 0.0;
	ER = 0.0;
	D[10] = 0.0;
	robot_netspeed = U = fl_get_slider_value(fd_demo->slider[_sl_speed]);


        // do some Resetting                                                                    
        fprintf(stderr,"t_Predict_demo::Reset()\n");                                                              
}                                                                                               
                                                                                                


void t_predict_demo::button_cb(FL_OBJECT *ob,long data)
{	
	switch(data)
	  {
	   case _bt_disturb:
	   case _bt_random:
	     // new buttons defined in t_predict_demo, call dummy 
	     // prevent "not overloaded" errors 	
	     break;
	
	   default:
	     t_runtime_object::button_cb(ob,data);
	     break; 	
	  }
}

void t_predict_demo::slider_cb(FL_OBJECT *ob,long data)
{
	switch(data)
	  {
	   case _sl_speed:
	     robot_netspeed = fl_get_slider_value(ob);
	     break;

           case _sl_freq:	// prevent ``not overloaded'' messages
           case _sl_learn:      // sliders are queried directly whilst 
           case _sl_maxspeed:	// the demo is running, you can speed up
           case _sl_sinamp:	// the program by adding some double variables 
           case _sl_ampl:	// and get them here with fl_get_slider_value()	     
           case _sl_couple:	// and get them here with fl_get_slider_value()	     
	     break;

	   case _sl_logavg:
	     {	
	      double d = fl_get_slider_value(ob);
	      Dva.Init(d);
	      Sl.Init(d);
	      Va.Init(d);		 
	      alpha_a.Init(d);		 
	      ES.Init(d*8);
	      EM.Init(d*2); 		 
	     } 	 
	     t_runtime_object::slider_cb(ob,data);
	     break;

	   default:
	     t_runtime_object::slider_cb(ob,data);
	     break; 	
	  }
}

void t_predict_demo::Load(char *to_parse)
{
	// put your loading stuff here and 
	int i;
	double d;

	if (sscanf(to_parse,"poly.C[%d]=%lf",&i,&d) == 2)
	   {
	    if (i < 10) 
	       {
		Poly->SetVal(0,i,d);
	        Poly->UseVal(0,i,true);
		fprintf(stderr,"using p{%d)\n",i);
	       }

	    return;
	   }

	ClearP->Reset(Poly);

	if (sscanf(to_parse,"slider[%d].value=%lf",&i,&d) == 2)
	   {
	    if (i >= nsliders+_sl_num)
	       return;
	       
	    if (i == _sl_logavg)
	       {	
		Dva.Init(d);
		Sl.Init(d);
		Va.Init(d);		 
		alpha_a.Init(d);
		ES.Init(d*8);
		EM.Init(d*2); 		 
	       }
           }

	// if we come here that wasn't private stuff 
	// try the generic parser from parent object      
	t_runtime_object::Load(to_parse);

	robot_netspeed = fl_get_slider_value(fd_demo->slider[_sl_speed]);
}

void t_predict_demo::Save(int fd)
{
	t_runtime_object::Save(fd);	// call parent for generic savings

	fprintf(stderr,"t_predict_demo::Save()\n");

	// then save our private stuff
	char st[256];
	
//	for (int j=0; j<MAX_POLY_PARMS; j++)
	  for (int i=0; i<10; i++)
	    {
	     if (Poly->C_used(0,i) == true)
	        {
		 sprintf(st,"poly.C[%d]=%g\n",i,Poly->C_Val(0,i));
	         write(fd,st,strlen(st)); 	   
	        }
	    }
}


void t_predict_demo::make_logdata(void)
{
	for (int i=0; i<10; i++)
	  {
	   int k = Poly->Index(0,i);
	   Poly->_C[k].logV += ((Poly->_C[k].Get() - Poly->_C[k].logV)/Logs.average);

//	   qx[i].Add(q[i].AvgV/(0.1 + Sensor[i].Get(0)));
	   qx[i].Add(a[i]);
//	   dxka[i].Add(dxk[i]);
//	   dxpa[i].Add(dxp[i]);
	   dxka[i].Add(D[i]);	   		
	  }

	Sl.Add(robot_netspeed);			   
	Va.Add(V);				
	Dva.Add(D[10]);
	alpha_a.Add(alpha);
}


void t_predict_demo::make_logstring(void)
{
	char st[4096],s2[256];

	strcpy(st,"\0");

/*
	for (int i=0; i<8; i++)
	  {
	   double log = Poly->C_logV(0,i);
	   sprintf(s2,"%g ",log); 
	   strcat(st,s2);
	  }

	strcat(st,"\t");
*/

	sprintf(s2,"%g ",D[10]); 
	strcat(st,s2);
	sprintf(s2,"%g ",Qv.AvgV); 
	strcat(st,s2);
	sprintf(s2,"%g ",robot_netspeed); 
	strcat(st,s2);
	sprintf(s2,"%g ",Va.AvgV); 
	strcat(st,s2);
	sprintf(s2,"%g ",alpha_a.AvgV); 
	strcat(st,s2);
	sprintf(s2,"%g ",ES.AvgV); 
	strcat(st,s2);
	sprintf(s2,"%g ",EM.AvgV); 
	strcat(st,s2);


	strcat(st,"\t");
	for (int i=0; i<10; i++)
	  {
	   sprintf(s2,"%g ",dxka[i].AvgV); 
	   strcat(st,s2);
	  }

	strcat(st,"\t");
	for (int i=0; i<10; i++)
	  {
	   sprintf(s2,"%g ",qx[i].AvgV); 
	   strcat(st,s2);
	  }

//	strcat(st,"\t");
//	for (int i=0; i<8; i++)
//	  {
//	   sprintf(s2,"%g ",b[i].AvgV); 
//	   strcat(st,s2);
//	  }

	sprintf(Logs.data,"%s\t%g %g\n",st,EL,ER);
//	    write(log_fd,st,strlen(st));
}


int nac = 0;

char number_alert(double &d,const char *msg,double def)
{
	if ((nac > 30) && ((nac % 10) == 0))
	   printf("\a");

	if (nac > 100)
	   exit(1);


	char st[256];
	sprintf(st,"%g",d);

	if (strcmp(st,"NaN\0") == 0)
	   {
	    fprintf(stderr,"\aNaN: %s\n",msg);
	    d = def;
	    nac++;

	    return true;
	   }

	if (strcmp(st,"Inf\0") == 0)
	   {
	    fprintf(stderr,"\aInf: %s\n",msg);
	    d = def;
	    nac++;

	    return true;
	   }

	return false;
}

char t_predict_demo::calculate_module(char allow_learn, char any_new)
{
//	if (any_new == false)
//	   return false;

	if (t_runtime_object::calculate_module(allow_learn,any_new) == false)
	   return false;
	
	double L = fl_get_slider_value(fd_demo->slider[_sl_learn]);

	int H = 2; //(int)ceil(fl_get_slider_value(fd_demo->slider[_sl_horiz]));
	int ph = 3; //(int)ceil(CrashTest_Demo.Phase);

	double v_radius = g_radius.Get(ph);		
	double s = sinus.Get(ph) /* /- Sin2.Get(ph) */;
	double c = cosin.Get(ph);
	double ew = exp(-v_radius);
	double div = (1.0 + ew);
	div *= div;
	double g_ = 4.0 * ew / div;
	
	double v[2];

	EL = 0.0;
	ER = 0.0;
	double EC = 0.0; 

	for (int i=0; i<10; i++)
	  {
	   if (i<8)
	      {
	       double tv1 = Sensor[i].Get((int)0);	
	       double tv2 = Sensor[i].Get(H);	
	
	       if ((tv1 < 0.0) || (tv1 > 1.0))
	          {
		   fprintf(stderr,"Sensor[%d]=%g\n",i,tv1);
		   exit(1);	 
		  }

	       if ((tv2 < 0.0) || (tv2 > 1.0))
	          {
		   fprintf(stderr,"Sensor[%d]=%g\n",i,tv2);
		   exit(1);	 
		  }

	       dxk[i] = tv1 - tv2;	
	       dxp[i] = (a[i]*(v_radius + sinus.Get(ph))) * tv1;
              } 
            else
	      {
	       double s0 = Sensor[i].Get((int)0);
	       double s1 = Sensor[i].Get((int)1);	

	       number_alert(s0,"s0",0.0);
	       number_alert(s1,"s1",0.0);

//	       dxk[i] = s0 - s1;
 	       Sensor[i+2].Add(s0 - s1);
	       
	       double dg = Sensor[i+2].Get((int)0);
	       dxk[i] = ((dg - Sensor[i+2].min) * 2.0 / Sensor[i+2].size) - 1.0;
			
	
	       if (fabs(dxk[i]) > fabs(5.0    * 6.0                 * 2.0))
		  {			   //  ^^robot_netspeed    ^^update_delay
  	           if (dxk[i] < 0.0)
		      dxk[i] = -60.0;
	            else
                      dxk[i] = 60.0; 
		  } 

	       v[i-8] = dxk[i];	

	       dxp[i] = Motor[i-8].Get(H) * a[i];
	      }	

	   char st[128];
	   sprintf(st,"dxk[%d]",i); 
	   number_alert(dxk[i],st,0.0);

	   sprintf(st,"dxp[%d]",i); 
	   number_alert(dxp[i],st,0.0);

	   D[i] = (dxk[i] - dxp[i]);
	   if (number_alert(D[i],"D[i]",0.0) == true)
	      {
	       number_alert(a[i],"a[i]",0.0);
	      }	

	   q_sin[i].Add(D[i] * s);
	   q_cos[i].Add(D[i] * c);

	   number_alert(q_sin[i].AvgV,"q_sin[i].AvgV",0.0);

	   double tmp = (D[i] * q_sin[i].AvgV);  

	   if (i<8)
	      {
               if (leftside(i) < 0)
		  EL +=  tmp;
		else
		  ER +=  tmp;

	       ES.Add(tmp); 
	       EC += (D[i] * q_cos[i].AvgV);
	      }
	    else
	      {
	       EL +=  tmp;
	       ER +=  tmp;

 	       EM.Add(tmp);	
	      } 
	  }


	  {
	   double Vp;

	   double nv = (v[0] + v[1]) / 2.0;

//	   double sv = (Motor[0].Get(H) + Motor[1].Get(H)) / 2.0; 
	   double sv = 6.0 * robot_netspeed;	

	   if (fabs(nv) < fabs(5.0 * sv * (update_freq/10.0)))
	      {
		V = nv;
	        good++;
	      }
	     else
	      {
//		fprintf(stderr,"\awheel speed left over %g,%g, %d/%d\n",nv,sv,good,bad);	
		bad++; 
              }

	   number_alert(alpha,"alpha",0.0); 	
	   number_alert(V,"V",0.0); 	

	   Vp = alpha * sv;

/*
	   if (fabs(V) > 0.1)
	       {
		D[10] = (V - Vp) / V;   	   
	       }
            else
	       {
	        D[10] = 0.0;	
//		fprintf(stderr,"\awheel speed too small %g\n",V);	
	       }	
*/
           D[10] = (V - Vp);
	   number_alert(D[10],"D[10]",0.0); 	

	   if (fabs(D[10]) > 1.0)
	       D[10] = 0.0; 

	   Qv.Add(c * D[10]);

	   double ee = exp(2.0 * U);
	   double e1 = (ee+1);
	   double ed = e1 * e1;
	   double ea = 4.0 * ee / ed;		// Ableitung von tanh(U.AvgV)

	   double LM = L * fl_get_slider_value(fd_demo->slider[_sl_couple]);

	   U += (-LM * ea * ((Qv.AvgV *D[10]) + EC));

	   RNS.Add(LM * (ee - 1.0) / (ee + 1.0));

	   robot_netspeed = RNS.AvgV;

	   alpha += (LM * 0.05 * D[10]);	
	  }

	char bias_bool = fl_get_button(fd_demo->cbox[_bt_bias]);

	for (int i=0; i<8; i++)
	  {
	   double SV = Sensor[i].Get((int)0);
	  
	   if (bias_bool)
	      {
	      }
	    else
	      {  	
               if (SV > 0.99)
		  continue;
	      }

	   int k = Poly->Index(0,i);

	   if (leftside(i) < 0)
	   	Poly->_C[k].AddV(-L * g_ * EL * SV);
	     else
	   	Poly->_C[k].AddV(-L * g_ * ER * SV);

	   a[i] += (D[i] * L * v_radius * SV);
	  } 

	for (int i=8; i<10; i++)
	   a[i] += (D[i] * L * 0.1 * Motor[i-8].Get(H));

	return true;
}


char t_predict_demo::control_module(void)
{
	if (t_runtime_object::control_module() == false)
	   return false;

	Robot->MsgQueue.request_proximity();
	Robot->MsgQueue.request_wheelposition();

//	if (uselight == true)
//	   Robot->MsgQueue.request_ambient();

	double tS[10];

	for (int i=0; i<8; i++)
	  {
	   tS[i] = Robot->Ired->Sensor[i]->Value;
	   if (tS[i] > 1.0)
	      fprintf(stderr,"tS[%d]=%g\a\n",i,tS[i]);	   	
	   Sensor[i].Add(tS[i]);	// Schieberegister 
          }

	tS[8] = ((double)Robot->gpl)/ (fl_get_slider_value(fd_demo->slider[_sl_wheel_ss]) +1.0);
	tS[9] = ((double)Robot->gpr)/ (fl_get_slider_value(fd_demo->slider[_sl_wheel_ss]) +1.0);

	for (int i=8; i<10; i++)
	  Sensor[i].Add(tS[i]);       


	double radius = Poly->Result(tS);
//	fprintf(stdout,"radius:%g\n",radius);

	double r = ((2.0/(1.0+exp(-radius))) - 1.0) * fl_get_slider_value(fd_demo->slider[_sl_ampl]);
	g_radius.Add(r);

	double T = fl_get_slider_value(fd_demo->slider[_sl_freq]);
	double t = (double)step;
	double freq = 1.0 / T;
	double omega = 2.0 * M_PI * freq;
	double ot = omega *t;
	double s = sin(ot);
	double c = cos(ot*2.0);

	if (fl_get_button(fd_demo->cbox[_bt_usine]))
	   {
	   } 
	  else
	   {
	    if (s > 0.0)
	       s = 1.0;

            if (s < 0.0)
               s = -1.0; 

	    if (c > 0.0)
	       c = 1.0;

            if (c < 0.0)
               c = -1.0; 
	   }

	sinus.Add(s);
	cosin.Add(c);

	s *= (fl_get_slider_value(fd_demo->slider[_sl_sinamp]) * omega); 
	c *= (fl_get_slider_value(fd_demo->slider[_sl_sinamp]) * omega); 

        if (fl_get_button(fd_demo->cbox[_bt_disturb]))
	   r += s;

	double rns = r * robot_netspeed;

	double vr,vl;

	vr = robot_netspeed + rns + c;
	vl = robot_netspeed - rns + c;

	step++;

	Motor[0].Add(vl*6.0);
	Motor[1].Add(vr*6.0);
        Robot->SetMotorSpeed(vl,vr);

	return true;
}





char t_predict_demo::start_module(void)                                                           
{                                                                                               
        if (t_runtime_object::start_module() == false)                                          
           return false;                                                                        
                                                                                                
//      place after that                                                                        
        fprintf(stderr,"t_predict_demo::start_module()\n");                                       
                                                                                                
        if (fl_get_button(fd_demo->cbox[_bt_random]))                                           
           {                                                                                    
            for (int i=0; i<8; i++)                                                            
//              for (int j=0; j<MAX_POLY_PARMS; j++)                                              
                 Poly->SetVal(0,i,(2.0*random_uniform())-1.0);                                            

           }
        return true;                                                                            
}                                                                                               
                                                                                                
                                                                                                
char t_predict_demo::stop_module(void)                                                            
{                                                                                               
        if (t_runtime_object::stop_module() == false)                                           
           return false;                                                                        
                                                                                                
//      place after that                                                                        
        fprintf(stderr,"t_predict_demo::stop_module()\n");                                        
                                                                                                
        return true;                                                                            
}                                                                                               
                                                                                                
                                                                                                 
void t_predict_demo::Done(void)                                                                   
{                                                                                               
        t_runtime_object::Done();                                                               
                                                                                                

        for (int i=0; i<10; i++)                                                         
           Sensor[i].Done();                                                                    
                                                                                                
        sinus.Done();        
	cosin.Done();                                                                   
        g_radius.Done();                                                                        
//        Sin2.Done();                                                                            
                                                                                                
        Poly->Done();                                                                           
        delete Poly;                                                                            
}                                                                                               
                                                                                                
                                                                                                
                                                                                                
                                                                                                
 








/**/
/**/
/**/
/**/
/**/
/**/
/**/
/**/
/*****  End Of File  ****************************/
