/* 
 * Thomas Pantzer: diffeq_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 <math.h>

#include "copyright.h"

#include "t_path.h"
#include "t_sorter.h"


#define _DIFFEQ_DEMO_G
#include "diffeq_demo.h"

#include "t_deq_l.h"
//#include "t_deq_c.h"

#include "t_woeq.h"
#include "user.h"



t_PATH *LastPoint;

char GetPathInfo(t_PATH *pt, t_vector &Ds, double &da_avg, double &ds_avg) //, double &X, double &Y)
{
   if (pt == NULL)
      return false;	

   if (pt->end == true)
      return false;	

   if (pt->next == NULL)
      return false;	  

   if (pt->next->next == NULL)
      return false;	  

   t_vector *p2,*p1,*p0;

   p2 = &pt->way;
   p1 = &pt->next->way;
   p0 = &pt->next->next->way;

   Ds.x = p2->x - p1->x;
   Ds.y = p2->y - p1->y;

   double dx0 = p1->x - p0->x;
   double dy0 = p1->y - p0->y;

   ds_avg +=  ((p2->Distance(p1) - ds_avg)/35.0);
   da_avg +=  (((atan2(Ds.y,Ds.x) - atan2(dy0,dx0)) - da_avg)/35.0);

   if ((Ds.x == 0.0) && (Ds.y == 0))
      return false;

   return true;	     
}


void Rotate(t_vector &P, double phi, t_vector &C)
{
	P.Sub(&C);
	P.Rotate(-phi);
	P.Add(&C);
}






#define diffeq_nBoxes	3
#define diffeq_nSliders	5
#define diffeq_nChoices	1


#define _bt_gamma	(_bt_num)
#define _bt_addline	(_bt_num+1)
#define _bt_errinfo	(_bt_num+2)
// #define _bt_cos3err	(_bt_num+3)

#define _sl_info	0
#define _sl_learn	1
#define _sl_switch	2
#define _sl_cool	3
#define _sl_shift	4

// #define _sl_

char diffeq_boxes[diffeq_nBoxes][32] = {"use gamma correction\0","add lines\0","second info\0"};
char diffeq_sliders[diffeq_nSliders][16] = {"info threshold\0","learn\0","switch speed\0","cooling\0","update shift\0"};
char diffeq_choices[diffeq_nChoices][64] = {"errorfunc@(a-b)^2+(c-d)^2|cos(a-b)^3\0"};


void t_diffeq_demo::Init(RobotContext *robot)
{
	char *cb_pointers[diffeq_nBoxes];
	char *sl_pointers[diffeq_nSliders];
	char *ch_pointers[diffeq_nChoices];

	for (int i=0; i<diffeq_nBoxes; i++)
          cb_pointers[i]  = &diffeq_boxes[i][0];

	for (int i=0; i<diffeq_nSliders; i++)
          sl_pointers[i]  = &diffeq_sliders[i][0];

	for (int i=0; i<diffeq_nChoices; i++)
          ch_pointers[i]  = &diffeq_choices[i][0];

	int pro = 0; 
	pro |= _p_calc;		//  needs calculations
	pro |= _p_show;		//  has a method to show something in graphic window
	pro |= _p_extras;	//  uses user settable parameters

	t_runtime_object::Init("DiffEq",robot,pro,
		diffeq_nSliders,diffeq_nBoxes,diffeq_nChoices,
		sl_pointers,cb_pointers,ch_pointers);

	u_shift = 10;
	cos3_error = false;
}

char t_diffeq_demo::AddLine(void)
{
	if (fd_demo->cbox[_bt_addline])
	   return true;

	return false;
}

void t_diffeq_demo::button_cb(FL_OBJECT *ob,long data)
{
	switch(data)
	  {
	   // add your cases here

	   // rest ist handled by parent object
	   case 0:
	   case 1:
	   case 2:
	   case 3:
	   default:
	     t_runtime_object::button_cb(ob,data);
	     break; 	

	   case _bt_gamma:
	     if (KMap != NULL)
		KMap->gamma_correct = (char)fl_get_button(ob);
	     break;             
	  }
}

void t_diffeq_demo::choice_cb(FL_OBJECT *ob,long data)
{
	int d;

	switch(data)
	  {
	   case 0:
	     d = fl_get_choice(ob);

	     switch(d)
	       {
		case 1:
		  cos3_error = false;
		  KMap->Use_oldErrorFunc();		  
		  break;

		case 2:
		  cos3_error = true;
		  break;
 
		default:
		  cos3_error = false;		
		  KMap->Use_oldErrorFunc();
		  break; 	
	       }	   
	     break;	   	

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

void t_diffeq_demo::slider_cb(FL_OBJECT *ob,long data)
{
	double d = fl_get_slider_value(ob);

	switch(data)
	  {
	   case _sl_learn:
	     if (KMap != NULL)
        	 KMap->_epsilon = d;
	     break;

	   case _sl_switch:
	     if (KMap != NULL)
        	KMap->_NY = d;

      //       factor_a = d; 	
	     break;

	   case _sl_info:  
	     if (KMap != NULL)
        	KMap->_Info_TH = d;

      //       factor_b = factor_a * d;	
	     break;

	   case _sl_cool:
	     if (KMap != NULL)
        	KMap->_DRate = d;
	     break;

	   case _sl_shift:
             u_shift = (int)d;
	     break;

	   default:
	     break; 	
	  }
}

void t_diffeq_demo::Load(char *to_parse)
{
//	put your loading stuff here and 
//	if (something_matched)
//	   return;

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

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

#ifdef DEBUG
	fprintf(stderr,"t_diffeq_demo::Save()\n");
#endif
	// then save our private stuff
}

/*
void t_diffeq_demo::Reset(void)
{
	// do some Resetting
	t_runtime_object::Reset();
}
*/



double S_mittel, A_mittel, Sd_mittel, Ad_mittel;



char t_diffeq_demo::start_module(void)
{
	if (t_runtime_object::start_module() == false)
	   return false;
	
	// ********
	S_mittel = 0.0;
	A_mittel = 0.0;
	Sd_mittel = 0.0;
	Ad_mittel = 0.0;
	
//	t_vector C = {0.0,0.0};

	Robot->World.Init();
//	Robot->Trajectory->Init(&C);
	LastPoint = Robot->Trajectory;

	Robot->MsgQueue.request_coords();

	KMap = new t_woeq;
	KMap->Init();

	t_deq_l *L; //,*last;

	int count = 4;
	double a = 1.0 / (double)count;

	for (int i=0; i<count; i++)
	  {
	   L = new t_deq_l;
	   L->Init(i,a,KMap->first);
	   KMap->Add(L);
	  }
   
/*
	t_deq_c *C = new t_deq_c;
	C->Init(count,-30.0,first);
	KMap->Add(C);
   
	C = new t_deq_c;
	C->Init(count+1,70.0,first); 
	KMap->Add(C);
*/
//	C = new t_deq_c;
//	C->Init(3,170.0,first);
//	KMap->Add(C);


	KMap->_DRate = fl_get_slider_value(fd_demo->slider[_sl_cool]);
	KMap->_epsilon = fl_get_slider_value(fd_demo->slider[_sl_learn]);
	KMap->_NY = fl_get_slider_value(fd_demo->slider[_sl_switch]);
	KMap->_Info_TH = fl_get_slider_value(fd_demo->slider[_sl_info]);
	KMap->shift = (int)fl_get_slider_value(fd_demo->slider[_sl_shift]);
	
	return true;
}


double Criterion_3(void *Ptr)
{
	t_deq *dg = (t_deq *)Ptr;
        return -dg->quality; 
}



char t_diffeq_demo::stop_module(void)
{
	if (t_runtime_object::stop_module() == false)
	   return false;
	
	if (KMap == NULL)
	   return false;
	
        // ********
	t_sorter SortO;
	t_deq *AR[32];
	t_deq *DG;

	int k = 0;
	DG = KMap->first;
	while ((k < KMap->count) && (DG != NULL))
	  {
	   AR[k] = DG;
	   DG = DG->next;
	   k++;
	  }

	SortO.Init((void **)AR, Criterion_3, KMap->count, true);
	SortO.Run();
	SortO.Done();

	for (int i=0; i<KMap->count; i++)
	   {
            fprintf(stderr,"%d. dg(%d): %g\n",i,AR[i]->id,AR[i]->quality);
            AR[i]->id = i+1;
	   }

        KMap->Done();
        delete KMap;
        KMap = NULL;

//	Robot->Trajectory->Save();
//	Robot->Trajectory->Done();

	return true;
}


void t_diffeq_demo::Show(void)
{
//	if (visible == false)
//	   return;
	
	// *********
	if (KMap != NULL)
	   KMap->Show(&VScr.center,(double)(VScr.size-16));
}


void t_diffeq_demo::make_logstring(void)
{
	   char *s = KMap->write_log(Ad_mittel,Sd_mittel);
	   strcpy(Logs.data,s);
	   delete s;
}


void t_diffeq_demo::RobotPath_Shift(void)
{
}

void t_diffeq_demo::RobotPath_Rotate(void)
{
	t_PATH *tp1,*tp2,*tp3;
	tp1 = Robot->Trajectory->LastPos(384);
	
	if (tp1 != NULL)
	   {
	    tp2 = tp1->LastPos(1);
	    if (tp2 != NULL)
	       {
		tp3 = tp2->LastPos(1);
		if (tp3 != NULL)
		   {
		    tp1->border = true;
		    tp2->border = true;
		    tp3->border = false; 
		    
		    t_PATH *pn = tp2;
		    int ad = 0;	
		    int d = 0;	

		    while ((pn != NULL) && (d < 16))
		      {
		       if (pn->adjusted == false)
			  {
 		   	   Rotate(pn->way,KMap->phi,Robot->World.center);
			   pn->adjusted = true;
         	   	   Robot->World.Update(pn->way);
	                   ad++;
			  }	

		       pn = pn->next;
		       d++;	
		      }		

		    if (ad == 0)
		       {
//			fprintf(stderr,"diffeq paused\n");		    	
			pause_module(true);
		       } 	
		   }	 
	       }
	   } 
}

char t_diffeq_demo::calculate_module(char allow_learn, char any_new)
{
//	if (any_new == false) 		// coded twice
//	   return false;	

	if (t_runtime_object::calculate_module(allow_learn, any_new) == false)
	   return false;

	Robot->MsgQueue.request_coords();

	if (LastPoint == Robot->Trajectory)
	   {
	    fprintf(stderr,"too fast\n");
	    return false;	
	   }

	t_PATH *pa;
	t_vector Ds;	

	if (GetPathInfo(Robot->Trajectory,Ds,A_mittel,S_mittel) == true)
	   {
	    char f2 = fl_get_button(fd_demo->cbox[_bt_errinfo]);

	    KMap->ProbUpdate(Ds,A_mittel,S_mittel,f2);	

	    int sh = (int)fl_get_slider_value(fd_demo->slider[_sl_shift]);
	    pa = Robot->Trajectory->LastPos(sh);	//KMap->shift); 
							//(int)KMap->delay); 
	    if (GetPathInfo(pa,Ds,Ad_mittel,Sd_mittel) == true)
	       {
		pa->border = true;
		pa->end = true;
 
//		KMap->write_log(log_fd,Ad_mittel,Sd_mittel);
//		if (
		KMap->GammaUpdate(Ds,Ad_mittel,Sd_mittel);
//			 == true)	              Update_Display();  

		if (cos3_error == true)
		   KMap->NeighbourUpdate();

		KMap->_DRate += 1.3;
		pa->border = false;
	       }
	   } 

	RobotPath_Rotate();
	RobotPath_Shift();

	if (state == _s_started)
	   state = _s_running;

	return true;
}



//   pa = Robot->Trajectory->LastPos(384);
//  	  Scan_World.Update(pa->next->way);
// //     	      Scan_World.WrapMove(pa->next->way);
// 
//      	  v_s.Init(Scan_World.center);
// 	  v_s.Sub(&o_center_s);
// 
//           if (KMap->ignore_gamma == false)
//              {
// 	      t_vector mo,md,mo2,md2,md3,mo3,ms;
// 
// 	      mo3.Init(median3);
// 	      median3.x += (pa->next->way.x - median3.x)/1000.0;			      	
// 	      median3.y += (pa->next->way.y - median3.y)/1000.0;			      	
// 	      md3.Init(median3);
// 	      md3.Sub(&mo3);
// //	      fprintf(stdout,"%g %g %g %g\t",mdir3.x,mdir3.y,md3.x,md3.y);		
// 	      mdir3.Add(&md3);
// 
// 
// 	      t_PATH *pw = pa->LastPos(640);
// 	      if (pw != NULL)
// 	         {
// 		  mo2.Init(median2);
// 		  median2.x += (pw->way.x - median2.x)/1000.0;			      	
// 		  median2.y += (pw->way.y - median2.y)/1000.0;			      	
// 		  md2.Init(median2);
// 		  md2.Sub(&mo2);
//   	          fprintf(stdout,"%g %g %g %g\t",mdir3.x,mdir3.y,md3.x,md3.y);		
// 		  fprintf(stdout,"%g %g %g %g\t",mdir2.x,mdir2.y,md2.x,md2.y);		
// 		  mdir2.Add(&md2);
// 
// 
// //		  md.Init(md2);
// //		  md.Sub(&md3);
// 		  fprintf(stdout,"%g\n",md2.CosPhi(&md3));
// 
// // mdir.x,mdir.y,md.x,md.y);		
// //		  mdir.Add(&md);
// 	          
// 	         } 
// 
// //	      MS.Add(&ms);
//      	      if (v_start == false)
//                  {
//                   if ((v_s.x != 0.0) || (v_s.y != 0.0))
//                      {
// 	  	      v_start = true;
// 	              v_add.Init(v_s);
// //	              gamma_add = -KMap->phi;
// //	              fprintf(stderr,"start @ %g°\n",DEG(gamma_add));
// 		     } 
// //	           else
// //                     fprintf(stderr,"center: (%g,%g), (%g,%g)\n",
// //       				Scan_World.center.x,Scan_World.center.y,
// //					o_center_s.x,o_center_s.y); 
// 	         } 
//                else
//                  {
//                   if ((v_s.x == 0.0) && (v_s.y == 0.0))
//                      {
// 	  	      v_start = false;
// 	              gamma_add = KMap->v_gamma * (double)Scan_World.steps;
// 		      if (DEG(gamma_add) > 3.0)
// 	                 { 
// //	                  Scan_World.Shift(v_add);
// 			  v_add.Mult(0.5);
// 			  t_vector s;
// 		          s.Init(v_add);
// 	        	  double l = v_add.Length();
// 
// 	        	  // v_add.Rotate( M_PI/2.0 );
// 			  double x = v_add.x;
// 			  double y = v_add.y;
// 			  v_add.x = -y;
// 			  v_add.y = x;	
// 				
// 			  v_add.Normalize();
// 			  double M = l / (2.0 * tan(gamma_add));
// 			  v_add.Mult(M);
// 			  v_add.Add(&s);
// 			  v_add.Rotate(KMap->phi);
// 
// 			  fprintf(stderr,"M:%g, l:%g, steps;%ld, ga:%g°, va(%g,%g) s(%g,%g)\n",
//                              M,l,Scan_World.steps,DEG(gamma_add),v_add.x,v_add.y,s.x,s.y);		
// 
//    			  Sim.World.Expand(v_add);
//                           fprintf(stderr,"center: (%g,%g)\n",
//        				Sim.World.center.x,Sim.World.center.y);
// 			  Scan_World.steps = 0;
//                          } 
// 		     } 
//                    else 
//    		     {
// 	              v_add.Add(&v_s);
// //	              fprintf(stderr,"add (%g,%g) (%g,%g)\n",v_s.x,v_s.y,v_add.x,v_add.y);
//                      }
// 	  	 } 
// 
// 	     }
// //           else
// 
// 
//           o_center_s.Init(Scan_World.center); 	// scanned
// 
// 	  Scan_World.steps++;
//          t_vertex_2d ux;
//	  ux.x = -pa->next->way.x; 
//	  ux.y = -pa->next->way.y; 
//          Sim.World.Update(ux);
//      }
//   my_path->info = KMap->phi; 	 
//   if (my_path != NULL)
//      my_path->border = false;




#ifdef WITH_DELETE_LINES

t_deq *t_kurvemap2::Nearest(t_vertex_2i &center, t_vertex_2i &click_pos)
{
   double arg,dx,dy,max,d;
   t_deq *N,*R;
   
   dx = click_pos.x - center.x;
   dy = click_pos.y - center.y;
   
   arg = atan2(dy,dx);
   
   R = NULL;
   max = 8*M_PI;   // max shoudn't be larger than Pi per definition
   N = first;
   while (N != NULL)
     {
      if (N->type == e_line)
	 {
          d = N->angle_diff(arg);
          if (d < max)
             {
              max = d;  
              R = N;
             }
         } 
      
      N = N->next;   
     }
     
   return R;  
}


void KMap_Delete_Line(t_vertex_2i &click)
{
   if (KMap == NULL)
      return;
      
   t_deq *D = KMap->Nearest(VScr.center,click);
   KMap->DelLine(D);   
}



void KMap_Add_Line(t_vertex_2i &click)
{
   if (KMap == NULL)
      return;
      
//   t_diffgl *D = KMap->Nearest(VScr.center,click);
   
   t_vertex_2d ln;
   
   ln.x = -VScr.center.x + click.x;
   ln.y = -VScr.center.y + click.y; 
   
   KMap->AddLine(ln);   
}


#endif






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