
/* 
 * Thomas Pantzer: khep_cb.cc, v0.1b  23.9.97 22:29 
 * Thomas Pantzer: khep_cb.cc, v0.4.7  8.6.99 16:32 
 * 
 * 
 * Copyright 1997 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@aix520.informatik.uni-leipzig.de   (Thomas Pantzer) 
 * 
 */

 

#include <stdlib.h>
#include <unistd.h>
#include <time.h>
#include <fcntl.h>
#include <math.h>

#include "copyright.h"

#include "forms.h"
#include "graphix.h"
#include "new_panel.h"
#include "slBounds.h"

// #ifdef hpux
#include "timer.h"
#include "t_trigger.h"
//#endif

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

#include "homeo_demo.h"
#include "neuronal_demo.h"
#include "diffeq_demo.h"
#include "simpath_demo.h"



//#include "user.h"


float	bow_speed = 0.3;
float turn_speed = 1.2;
int pressed = false;
int ticks_L = 0;
int ticks_R = 0;

int active_sl		= 0;


/* callbacks for form panel */
void do_nothing(FL_OBJECT *ob, long data)
{
  /* fill-in code for callback */
}


#define algo_args_lo (cmd_algo * 100)
#define algo_args_hi ((cmd_algo * 100) + 16)

#define view_args_lo (cmd_view * 100)
#define view_args_hi ((cmd_view * 100) + 16)


inline void Toggle(char &B)
{
	if (B == true)
	   B = false;
	 else
	   B = true;	 	
}

void popup_cb(int arg)
{
  /* fill-in code for callback */
	int d;

	if ((algo_args_hi >= arg) && (arg >= algo_args_lo)) 
	   {
#ifdef DEBUG
	    fprintf(stderr,"trapped algo\n");
#endif
	    d = arg - algo_args_lo; 

	    Toggle(Callbacks.RunObj[d]->active_calc);

	    if (Callbacks.RunObj[d]->active_calc == false)	// show the coresponding
	       Callbacks.RunObj[d]->Interactions(true);		// parameter form
             else  
	       Callbacks.RunObj[d]->Interactions(false);	// hide it

	    return;
	   } 


	if ((view_args_hi >= arg) && (arg >= view_args_lo)) 
	   {
#ifdef DEBUG
	    fprintf(stderr,"trapped view\n");
#endif
	    d = arg - view_args_lo; 

	    Toggle(Callbacks.RunObj[d]->visible);

	    char need_canvas = Callbacks.NeedCanvas();

	    if (need_canvas == false) 
		VScr.Hide();
	     else
	       	VScr.Show("kcontrol canvas");

//		fd_panel->canvas = VScr.fd_xfbgi->canvas;
#ifdef DEBUG	
	    fprintf(stderr,"need: %d, visible %d\n",need_canvas,fd_panel->canvas->visible);
#endif
	    return;
	   } 

	fprintf(stderr,"popup_cb(%d)\n",arg);
}

void create_pup(FL_OBJECT *ob, long data)
{
  /* fill-in code for callback */
	int pup = fl_newpup(FL_ObjWin(ob));	
	char st[256];
	char toggle;
	int cd;

	switch(data)
	  {	
	   case cmd_algo:
	     fl_addtopup(pup,"Algorithm%F%t",popup_cb);
	     break;
	
	   case cmd_view:
	     fl_addtopup(pup,"View%F%t",popup_cb);
	     break;

	   default:
	     return;	 	
	  }

	cd = ((int)data * 100);

	for (int k=0; k<Callbacks.number; k++)
          {
	   switch(data)
	     {	
	      case cmd_algo:
//		fl_addtopup(pup,"SubSubM%F%t",popup_cb);
		if ((Callbacks.RunObj[k]->props & _p_calc) > 0)
		   {
		    if (Callbacks.RunObj[k]->active_calc == true)
			toggle = 'r';
		     else
			toggle = 'R';
	  	    
		    sprintf(st,"%s%%x%d%%%c%d",Callbacks.RunObj[k]->Name,cd+k,toggle,k);		
		    fl_addtopup(pup,st);
		   } 
		break;

	      case cmd_view:
		if ((Callbacks.RunObj[k]->props & _p_show) > 0)
		   {
		    if (Callbacks.RunObj[k]->visible == true)
			toggle = 'R';
		     else
			toggle = 'r';

		    sprintf(st,"%s%%x%d%%%c%d",Callbacks.RunObj[k]->Name,cd+k,toggle,k);		
		    fl_addtopup(pup,st);
		   } 
		break;
	     }

	  }

	fl_dopup(pup);
}




void loadfile_cb(const char *Title, const char *Mask)
{
  /* fill-in code for callback */
  const char *st;
  char st2[1024];
  char *s;
/*
  char se1[256],se2[256];
  
  strcpy(se1,"\0");
  strcpy(se2,"\0");
*/
  //strcpy(st,fl_get_input(ob));
  sprintf(st2,"%s/.kcontrol/",getenv("HOME"));

  fl_set_border_width(1);
  if((st = fl_show_file_selector(Title,st2,Mask,"")))
    {                                                                                           
//     if (! fl_load_browser(fdui->br,fname))                                                   
//        fl_add_browser_line(fdui->br,"NO SUCH FILE!");                                       
    }
   else
    return;                  
 
  fprintf (stderr,"about to load file [%s]\n",st); 
  s = strrchr(st,'.');
  
  if (s == NULL)
     return; 
     
  char ext[2][12] = {".net",".traj"};
  
  int i;
  i = 0;
  while (i<2)
    {
     if (strcmp(s,ext[i]) == 0)
        break;
     i++;  
    }

  fprintf(stderr,"loadmode (%d)\n",i);     
  switch(i)
    {
     case 0:
       {
	Neuronal_Net.Done();
	Neuronal_Net.Init(&Robot_,&robot_netspeed);

//	demo_mode = 0;
//	fl_set_choice(fd_panel->demo_ch,demo_mode+1);
	Neuronal_Net.Load(st);
       } 
       break;
       
     case 1: // fast trajectory mode
//       kmap_init(st);
       fprintf(stderr,"file: [%s]\n",st);	
       SimPath_Demo.ctl_fs = fopen(st,"r");
       fprintf(stderr,"simP_fs: 0x%lx\n",(long)SimPath_Demo.ctl_fs);
       break;
     
     case 2:
     default:
//       demo_mode = 1;  
       break;
    }   
}




/*
 = NULL;
long change_slBounds_data = 0;
double change_slTopVal = 1.0;
double change_slBottonVal = 0.0;
*/


void slider_cb(FL_OBJECT *ob, long data)
{
	
//	if (fd_panel->slBorder_bt)
    double d;	
//    char st[256];	

    switch(fl_mouse_button())
      {
       case FL_MIDDLE_MOUSE:	
  	 break;

       case FL_RIGHTMOUSE: 
	 {
	  char st[256];
	  int ro_n;//,sl_n;
	  char **nptr;
	
	   
	  nptr = Callbacks.find_slidername(ob,ro_n);
	  if (nptr != NULL)
     	     sprintf(st,"change slider bounds for\n%s_demo->%s",
			Callbacks.RunObj[ro_n]->Name, nptr[0]);
   	   else
     	     sprintf(st,"change slider bounds");
	  
	  fl_set_object_label(fd_slBounds_Dialog->banner,st);
   	  fl_show_form(fd_slBounds_Dialog->slBounds_Dialog,FL_PLACE_CENTER,FL_TRANSIENT,"Khepera Slider Bounds Dialog");
//	  fl_deactivate_form(fd_panel->panel);
	  fl_set_focus_object(fd_slBounds_Dialog->slBounds_Dialog,fd_slBounds_Dialog->topval_in); 	
	  
	  change_slBounds.obj = ob;
	  change_slBounds.data = data;
	  
//	  fl_get_slider_precision(ob);
		
	  fl_get_slider_bounds(ob,&change_slBounds.TopVal,&change_slBounds.BottomVal);
	  sprintf(st,"%3.4f",change_slBounds.TopVal);
	  fl_set_input(fd_slBounds_Dialog->topval_in,st);
	  sprintf(st,"%3.4f",change_slBounds.BottomVal);
	  fl_set_input(fd_slBounds_Dialog->bottomval_in,st);
/*
	  FL_OBJECT *o; 
	  do {
	      o = fl_do_forms();
             } while ((o != fd_slBounds_Dialog->ok_bt) && 
                      (o != fd_slBounds_Dialog->dismiss_bt));	

	  fl_hide_form(fd_slBounds_Dialog->slBounds_Dialog);
	  fl_activate_form(fd_panel->panel);
*/
	  return;
	 }
//         break;

       case FL_LEFTMOUSE:
         d = fl_get_slider_value(ob); 	

  switch (data)
    {
/*
     case 0:
       active_sl = (active_sl +1) % 3;
       
       for (int i=0; i<4; i++)
         for (int j=0; j<64; j++)
            if (i!=active_sl)
               fl_hide_object(fd_panel->lcd_sl[1+j+(i*64)]);
              else 
               fl_show_object(fd_panel->lcd_sl[1+j+(i*64)]);
       break;
*/
     case cmd_amblsl:
       //Toggle(
       ambl_avg_bool++;
       ambl_avg_bool %= 3;		
       break;	
   
     case cmd_iredsl:
       //Toggle(ired_avg_bool);	
       ired_avg_bool++;
       ired_avg_bool %= 3;		
       break;		

     case cmd_speed:
       turn_speed = fl_get_slider_value(ob);
       robot_netspeed = turn_speed;	
       break;

/*       
     case cmd_pathlen:
       max_path_len = (int)d;	
       break;
*/	
     case cmd_update:
	Trigger.Register(Canvas_trg,(long int)fl_get_slider_value(fd_panel->update_sl));	       
       break;	
       
    }
	 break;
	}
}



void button_cb(FL_OBJECT *ob, long data)
{
        fl_activate_form(fd_panel->panel);

	int i = fl_get_button(ob);

	switch (data)
	  {
	   case -1:
	     DoneKhepera();
	     exit(0);

	   case cmd_run:    // run_cb
	     if (i == 0)
		{
//		 if (control_mode != _cm_SIMULATOR)
//        	    Robot_.SetMotorSpeed(0,0);

		 run_user_cb = false;
//		 fl_set_idle_delta(10);
		}
	      else
		{
		 run_user_cb = true;   
		 fl_set_idle_delta(0);
		}
             break; 

	   case cmd_grip: 
	     if (i)
        	Robot_.Grip();
              else  
        	Robot_.Release();
	     break;

/*
	   case 2: // Move_cb
//	     fprintf(stderr,"check_button returned %d\n",i);
	     if (i == 0)
        	 bow_speed = 0.0;
	      else
        	 bow_speed = fl_get_slider_value(fd_panel->speed_sl) / 2.0; 
*/


	   case cmd_move7:  
	   case cmd_move8:  
	   case cmd_move9:  
	   case cmd_move4:  
	   case cmd_move1:  
	   case cmd_move3:  
	   case cmd_move6:  
	   case cmd_move2:  
	     fprintf(stderr,"[%ld] returned %d\n",data,i);
	     break;

	   case cmd_OKslBounds:
	     if ((change_slBounds.topc == true) || (change_slBounds.botc == true))
                 fl_set_slider_bounds(change_slBounds.obj,
				   change_slBounds.TopVal,
				    change_slBounds.BottomVal); 

	     if (change_slBounds.stepc == true) 
                 fl_set_slider_step(change_slBounds.obj,
				   change_slBounds.StepVal);

	     if (change_slBounds.precic == true) 
                {
//	         char *S = change_slBounds.obj->spec;

//		 for (int j=0; j<16; j++)
//		   fprintf(stderr,"0x%02x ",S[j]);  
//		 fprintf(stderr,"\n");

		 fl_set_slider_precision(change_slBounds.obj,
				   change_slBounds.PreciVal);
//		 for (int j=0; j<16; j++)
//		   fprintf(stderr,"0x%02x ",S[j]);  
//		 fprintf(stderr,"\n");
	        }

           case cmd_CancelslBounds:
	     change_slBounds.topc = false;
	     change_slBounds.botc = false;
	     change_slBounds.stepc = false;
	     change_slBounds.precic = false;
             fl_hide_form(fd_slBounds_Dialog->slBounds_Dialog);
             break;
	  }
}



void browser_proc(FL_OBJECT *ob, long data)
{
  /* fill-in code for callback */
}

void choice_cb(FL_OBJECT *ob, long data)
{
  /* fill-in code for callback */
  int /*i,*/j;
//  char st[1024];

  j = fl_get_choice(ob);
  switch(data)
    {
     case cmd_control:
	control_mode = j - 1;	
	for (int n=0; n<Callbacks.number; n++)
	   Callbacks.RunObj[n]->active_ctl = true;

	Callbacks.RunObj[control_mode]->active_ctl = false;
	
	switch(control_mode)
	  {
	   case _cm_NEURONAL:
             loadfile_cb("Load Network Description","*.net");
	     break;

	   case _cm_SIMULATOR: 	
	     loadfile_cb("Load Trajectory","*.traj");
	     break;
	  } 	
	break;
    }
}

void input_cb(FL_OBJECT *ob, long data)
{
     char st[128];
     float d;
     int i;	
	
     strcpy(st,fl_get_input(ob));

     switch(data)
       {
	case cmd_device:
	  Robot_.Done();
	  Robot_.Init(st);
	  break;

	case cmd_slTopVal:
	  if (sscanf(st,"%f",&d) == 1)
	     {
  	      change_slBounds.TopVal = d;
	      change_slBounds.topc = true;	
	     }
	  break;

	case cmd_slBottomVal:
	  if (sscanf(st,"%f",&d) == 1)
	     {
	      change_slBounds.BottomVal = d;
	      change_slBounds.botc = true;	
	     } 	
	  break;

	case cmd_slPreciVal:
	  if (sscanf(st,"%d",&i) == 1)
	     {
  	      change_slBounds.PreciVal = i;
	      change_slBounds.precic = true;	
	     }
	  break;

	case cmd_slStepVal:
	  if (sscanf(st,"%f",&d) == 1)
	     {
	      change_slBounds.StepVal = d;
	      change_slBounds.stepc = true;	
	     } 	
	  break;
       } 		
}


void dial_cb(FL_OBJECT *ob, long data)
{
	double d = fl_get_dial_value(ob);
	
	switch(data)
	  {
	   case cmd_turn:
	     Robot_.SetOrientation(d);
#ifdef DEBUG
	     fprintf(stderr,"dial:%g\n",d);
#endif    
	     break; 

	   case cmd_armpos: 
	     Robot_.SetArmPos((int)d);
/*	     Robot[current_robot]. */
	     break;
	  }
}
	

void pos_cb(FL_OBJECT *ob, long data)
{
       /*
               x = fl_get_positioner_xvalue(ob);
               y = fl_get_positioner_yvalue(ob);
               u = y - Robot[current_robot].Y;
               v = x - Robot[current_robot].X;
               Robot[current_robot].Turn(deg(atan2(u,v)));
               Robot[current_robot].Move(sqrt((u*u) + (v*v)));
        */
}






int idle_cb(XEvent *,void *)
{
	user_cb(run_user_cb, true);
	return 0;
}



//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//    Goodies
//________________________________________________________



/* which event to take over is better kept in a state varible even though
   query the status via fl_get_button is cheap
 */
int preemptive_handler(FL_OBJECT *ob, int event,
                          FL_Coord mx, FL_Coord my, int key, void *xev)
{
//    int override = fl_get_button(fd_form0->override);
//    int i; 
    int override = 1;
//    char buf[128];
//    double mm,w,z;
    
//    char *what = override ? "preempted":"detected";

    switch(event)
    {
      case FL_ENTER:
        if (fl_get_button(ob)) 
        {
          /* ok, want to handle enter */
//            sprintf(buf,"%s %s", "FL_ENTER",what);
//            fl_set_object_label(fd_form0->event, buf);
            return (override ? FL_PREEMPT: 0);
        }
        break;
      case FL_LEAVE:
        if (fl_get_button(ob))
        {
//            sprintf(buf,"%s %s", "FL_LEAVE",what);
//            fl_set_object_label(fd_form0->event, buf);
            return (override ? FL_PREEMPT: 0);
        }
        break;
      case FL_PUSH:
      case FL_MOUSE: /* one of the quirks of the button class */
        if (fl_get_button(ob))
           {
//            sprintf(buf,"%s %s", "FL_PUSH",what);
//            fl_set_object_label(fd_form0->event, buf);
//            i = fl_get_choice(fd_panel->select_ch)-1;
//            Robot[current_robot].UpdateSensors();
//            Check_Kohonen(&Robot[current_robot]); 
      
//            if (Robot[current_robot].GetMotorSpeed(0) == 0)
            if (pressed == false)
//               {
                Robot_.MsgQueue.request_coords(); //SolveCoords(true);
//                ticks_L = Robot_.gpl;
//                ticks_R = Robot_.gpr;                 
//               }
            
            pressed = true;   
                        
            switch(ob->argument)
              {
               case cmd_move7: Robot_.SetMotorSpeed(bow_speed,turn_speed);	 break;  
               case cmd_move8: Robot_.SetMotorSpeed(turn_speed,turn_speed);	 break;  
               case cmd_move9: Robot_.SetMotorSpeed(turn_speed,bow_speed);	 break;  
               case cmd_move4: Robot_.SetMotorSpeed(-bow_speed,bow_speed);	 break;  
               case cmd_move6: Robot_.SetMotorSpeed(bow_speed,-bow_speed);	 break;  
               case cmd_move1: Robot_.SetMotorSpeed(-bow_speed,-turn_speed);	 break;  
               case cmd_move2: Robot_.SetMotorSpeed(-turn_speed,-turn_speed);	 break;  
               case cmd_move3: Robot_.SetMotorSpeed(-turn_speed,-bow_speed);	 break;
              }
               
            return (override ? FL_PREEMPT: 0);
        }
        break;
        
      case FL_RELEASE:
        if(fl_get_button(ob))
        {
//            sprintf(buf,"%s %s", "FL_RELEASE",what);
//            fl_set_object_label(fd_form0->event, buf);
//            i = fl_get_choice(fd_panel->select_ch)-1;

            Robot_.Stop();
            Robot_.MsgQueue.request_coords(); //SolveCoords(true);
            
            pressed = false;
            
//            fprintf(stderr,"diffs left:%d  right:%d\n",
//                           Robot_.gpl - ticks_L,
//                           Robot_.gpr - ticks_R); 
            
//            Robot_.SolveCoords(true);
 
            fl_set_button(ob,0);  
            return (override ? FL_PREEMPT: 0);
        }
        break;
    }
    return 0;
}








int post_handler(FL_OBJECT *ob, int event,
            FL_Coord mx, FL_Coord my, int key, void *xev)
{
    char *s = 0;

          if(ob == fd_panel->run_bt)       s = "execute user\ncallbacks";
//     else if(ob == fd_panel->save_cfg_bt)  s = "not omplemented"; 
//     else if(ob == fd_panel->robos_ch) s = "set number of\nused robots"; 
//     else if(ob == fd_panel->select_ch)    s = "set shown robot"; 
     
//     else if(ob == fd_panel->mode_ch)      s = "switch between\npiped and real mode";
      
     else if(ob == fd_panel->control_ch)      s = "select controller-\nmode for the robot"; 

     else if(ob == fd_panel->filter_ch[1])  s = "choose a filter function"; 
     else if(ob == fd_panel->filter_ch[2])  s = "choose a filter function"; 
     else if(ob == fd_panel->filter_ch[3])  s = "choose a filter function"; 
     else if(ob == fd_panel->filter_ch[4])  s = "choose a filter function"; 

     else if(ob == fd_panel->gripper_dl)     s = "set angle for\ngrip arm"; 
     else if(ob == fd_panel->turn_dl)       s = "turn robot\nto direction"; 
     else if(ob == fd_panel->device_in)       s = "specify\nserial line"; 
//     else if(ob == fd_panel->file_bt)      s = "not imlemented"; 
//     else if(ob == fd_panel->filename_inp)  s = "enter filename\nfor snns-network"; 

     else if(ob == fd_panel->move_bt[7]) s = "move robot\ncounterclockwise"; 
     else if(ob == fd_panel->move_bt[8]) s = "move robot\nforward"; 
     else if(ob == fd_panel->move_bt[9]) s = "move robot\nclockwise"; 
     else if(ob == fd_panel->move_bt[4]) s = "turn robot\ncounterclockwise"; 
     else if(ob == fd_panel->move_bt[6]) s = "turn robot\nclockwise"; 
     else if(ob == fd_panel->move_bt[1]) s = "move robot\nclockwise back"; 
     else if(ob == fd_panel->move_bt[2]) s = "move robot\nbackward"; 
     else if(ob == fd_panel->move_bt[3]) s = "move robot\ncounterclockwise\nback"; 

     else if(ob == fd_panel->grip_bt)  {
                                         if (fl_get_button(ob))
                                               s= "open the grip";
                                            else   
                                               s= "close the grip";
                                        }
     else                               s= "Want to quit ?\nPress me";

     if(event == FL_ENTER && s)
        fl_show_oneliner(s, ob->form->x + ob->x + 20,
                        ob->form->y + ob->y + ob->h);
     else if(event != FL_MOTION)
       fl_hide_oneliner();

     return 0;
}


int canvas_expose(FL_OBJECT *ob, Window win, int w, int h, XEvent *ev, void *d)
{
    // save window variable
    VScr.Init(fl_get_canvas_id(fd_panel->canvas),w,h);
//    fl_add_canvas_handler(fd_panel->canvas, Expose, canvas_expose, fd_panel);


//    fprintf(stderr,"width: %d\theight:%d\tsize:%d\n",VScr.width,VScr.height,VScr.size);

/*
    // clear screen
    SetColor(FL_BLACK);
    FillRectangle(0, 0, w, h);
    SetTextStyle(DefaultFont);
*/
    switch (ev->type)
      {
       case ButtonPress:
	 {
          switch (ev->xbutton.button)
            {
/*
             case 1: 
               view_mode = (view_mode +1) % view_modes;
               break;
*/
             case 2:
               if (DiffEq_Demo.visible == true)
        	  {
                     t_vertex_2i click;
                     click.x = ev->xbutton.x;
                     click.y = ev->xbutton.y;

                     fprintf(stderr,"click (%d,%d)\n",click.x,click.y);

#ifdef WITH_DELETE_LINE
                     if (DiffEq_Demo.AddLine() == false)
                	 KMap_Delete_Line(click);
                      else
                	 KMap_Add_Line(click);   
#endif
        	  }
               break;

             case 3: 
               break; 

             default:
               break;
            } // switch (ev->xbutton.button) 
	 }
	 break;

       case KeyRelease:
         {
	  char st[256];
	  KeySym xKsym;
	  XComposeStatus xcStatus;

#ifdef DEBUG
	  fprintf(stderr,"keycode: 0x%x\t",ev->xkey.keycode);
#endif
 	  int len = XLookupString(&(ev->xkey),st,32,&xKsym,&xcStatus);
          if (len > 0)
	     {
	      st[len] = '\0';
#ifdef DEBUG
              fprintf(stderr,"buffer [%s]\t",st);
#endif
	     }

#ifdef DEBUG
          fprintf(stderr,"keysym: 0x%04lx\n",xKsym);
#endif
	  switch (xKsym)
            {
	     case XK_F22:
	       VScr.Save("kcontrol-canvas");
               break;
	    }  
         }
	 break;
      } // switch (ev->type)
    
    // user drawing routines
    Update_Display(1);
    
//    Neuronal_Net->Show();
//    fl_addto_browser(ui->br, "Expose");
    return 0;
}









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