/* 
 * Thomas Pantzer: simpath_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 <unistd.h>

#include "copyright.h"

#include "t_runtime_obj.h"


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

#define _SIMPATH_DEMO_G
#include "simpath_demo.h"

#define simpath_nBoxes		0
#define simpath_nSliders	2

// #define _sl_num 	0

#define _sl_pathlen	(0)
#define _sl_stepping	(1)
//#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 simpath_boxes[simpath_nBoxes+2][16] = {"\0"
				     ,"\0"
				     };

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

char simpath_sliders[simpath_nSliders][16] = {
		"pathlen.\0",
		"steps\0"};


void t_simpath_demo::Init(RobotContext *robot)
{
	char *cb_pointers[simpath_nBoxes];
	char *sl_pointers[simpath_nSliders];

	for (int i=0; i<simpath_nBoxes; i++)
          cb_pointers[i]  = &simpath_boxes[i][0];

	for (int i=0; i<simpath_nSliders; i++)
          sl_pointers[i]  = &simpath_sliders[i][0];

	int pro = 0; 
	pro |= _p_control;	//  does something to control the robot
	pro |= _p_calc;		//  needs calculations
	pro |= _p_show;		//  has show method on canvas
	pro |= _p_extras;	//  has parameter view with sliders and checkboxes

	t_runtime_object::Init("SimPath",robot,pro,
			simpath_nSliders,simpath_nBoxes,0,
			sl_pointers,cb_pointers,NULL);
//	t_runtime_object::Init("SimPath",robot,pro,simpath_nSliders,NULL,sl_pointers,NULL);

	max_path_len = 2000;
	stepping = 1;
}

void t_simpath_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:
	   default:
	     t_runtime_object::button_cb(ob,data);
	     break; 	
	  }
}

void t_simpath_demo::slider_cb(FL_OBJECT *ob,long data)
{
	switch(data)
	  {
	   case _sl_pathlen:
	     max_path_len = (int)fl_get_slider_value(ob);
	     break;

	   case _sl_stepping:
	     stepping = (int)fl_get_slider_value(ob);
	     break;

	   default:
	     break; 	
	  }
}

void t_simpath_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      

	// put your loading stuff here and 
	int j;
//	double d;

	if (sscanf(to_parse,"stepping=%d",&j) == 1)
	   {
	    stepping = j;
	    return;
	   }

	if (sscanf(to_parse,"pathlen=%d",&j) == 1)
	   {
	    max_path_len = j;
	    return;
	   }


	t_runtime_object::Load(to_parse);
}

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

//	fprintf(stderr,"t_simpath_demo::Save()\n");
//	Robot->Trajectory->Save(fd);
	// then save our private stuff

	char st[1024];
	sprintf(st,"stepping=%d\n",stepping);
	write(fd,st,strlen(st));
	sprintf(st,"pathlen=%d\n",max_path_len);
	write(fd,st,strlen(st));

	fprintf(stderr,"t_simpath_demo::save\n");

	Logs.fd = openfile("DATA_PATH","data","log",Name,false);
	Robot->Trajectory->Save(Logs.fd);
	close(Logs.fd);

}

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

char t_simpath_demo::start_module(void) 
{
//	log_fd = open("simpath_log.txt",O_RDWR|O_CREAT|O_TRUNC,0644);
	if (t_runtime_object::start_module() == false)
	   return false;

	t_vector center = {0.0,0.0};

	Robot->World.Init();
	Robot->Trajectory->Init(&center);

	return true;
}


char t_simpath_demo::stop_module(void)
{
	if (t_runtime_object::stop_module() == false)
	   return false;
	
//	char st[256];
//	sprintf(st,"log_%s.txt",Name);

//	fprintf(stderr,"t_simpath_demo::stop\n");
//	Logs.fd = openfile("DATA_PATH","data","log",Name,false);
//	Robot->Trajectory->Save(Logs.fd);
//	close(Logs.fd);


	fclose(ctl_fs);
	ctl_fs = NULL;
	Robot->Trajectory->Done();
	
	return true;
}



void t_simpath_demo::Show(void)
{
        
//        int plen = (int)fl_get_slider_value(fd_panel->pathlen_sl);
//	if (Robot->Trajectory != NULL)
//	   {

	if (control_mode != _cm_SIMULATOR)
	   Robot->MsgQueue.request_coords();
	    	
//	fprintf(stderr,"SimPath.Show()\n");

	SetColor(FL_GREEN);
	Robot->Trajectory->Draw(&Robot->World,max_path_len);

	SetColor(FL_WHITE);
	Robot->World.Draw();
//	   }
}

char t_simpath_demo::calculate_module(char allow_learn, char any_new)
{
	if (t_runtime_object::calculate_module(allow_learn,any_new) == false)
	   return false;
	
	if (control_mode != _cm_SIMULATOR)
	   Robot->MsgQueue.request_coords();

	if (state == _s_started)
	   state = _s_running;

	return true;
}


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

	t_PATH *tp;
	double d = 0.0; 

//	if (stepping < 1)
//	   stepping = 1;

//	for (int i=0; i<stepping; i++)
//	  {
	   do {
	       tp = Robot->Trajectory;

	       int k = 0;	
	       do {		
	           tp = Robot->Trajectory->NextPos(ctl_fs);
		   k++;
		  } while ((tp == Robot->Trajectory) && (k < 5));
					// up to 5 errors in path file allowed
	       if (k >= 5)
		  {
		   pause_module(true);
		   return false;
		  } 
	
	       if (tp != Robot->Trajectory)
		  {
	           d = Robot->Trajectory->way.Distance(&tp->way);
	    	   if (d < 0.000001)
	              {
#ifdef DEBUG
	               fprintf(stderr,"skipping near point, dist:%g\n",d);
#endif
	              } 
		  }
		else 
		  d = 1.0;	

	      } while ((d < 0.000001) && (tp->end == false));

/* obsoleted by secondary while

	    if (tp == Robot->Trajectory)
	       return true; 
*/

	    Robot->World.Update(tp->way);   
	    Robot->Trajectory = tp;
//	   }
	usleep(stepping*1000);
	

	if (state == _s_started)
	   state = _s_running;

	return true;
}










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