/* 
 * Thomas Pantzer: user.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 <sys/types.h>
#include <fcntl.h>
#include <math.h>
#include <stdlib.h>

#include "copyright.h"


#include "timer.h"

#include "t_trigger.h"

#include "filters.h"
#include "stuff.h"

#include "homeo_demo.h"
#include "gripper_demo.h"
#include "neuronal_demo.h"
#include "diffeq_demo.h"
#include "kompass_demo.h"
#include "simpath_demo.h"
#include "CrashTest_demo.h"
#include "speedy_demo.h"
#include "Predict_demo.h"
#include "HTest_demo.h"

// #include "VHideo_demo.h"


#define MasterCtl CrashTest_Demo



#define _USER_G
#include "user.h"

#include "new_panel.h"


/*
char view_text[view_modes][64] = {"path","obstacle map","free map","control net","diff.Eq.","kompass"};
char demo_text[demo_modes][64] = {"neuronal","gripper","follow","kompass","homeo"};
*/

long t_st, c_st, o_st;
int frame_num = 0;
char tset;

t_average US;

void user_cb_init(void)
{
#ifdef DEBUG
	fprintf(stderr,"user_cb_init()\n");
#endif

	Callbacks.Init();
	
	Callbacks.Add(&Gripper_Demo);
	Gripper_Demo.Init(&Robot_);

	Callbacks.Add(&Neuronal_Demo);
	Neuronal_Demo.Init(&Robot_);

	Callbacks.Add(&SimPath_Demo);
	SimPath_Demo.Init(&Robot_);

	Callbacks.Add(&Homeo_Demo);
	Homeo_Demo.Init(&Robot_);

	Callbacks.Add(&Predict_Demo);
	Predict_Demo.Init(&Robot_);

	Callbacks.Add(&HTest_Demo);
	HTest_Demo.Init(&Robot_);

	Callbacks.Add(&Kompass_Demo);
	Kompass_Demo.Init(&Robot_);

	Callbacks.Add(&DiffEq_Demo);
	DiffEq_Demo.Init(&Robot_);

	Callbacks.Add(&speedy_Demo);
	speedy_Demo.Init(&Robot_);

	MasterCtl.Init(&Robot_);
	MasterCtl.start_module();

	run_user_cb = false;
	tset = false;


	Trigger.Init(8);

	Trigger.Register(Canvas_trg,200);
	Trigger.Register(Cam_trg,100);
	Trigger.Register(Odo_trg,30);
	Trigger.Register(LED_trg,50);


	US.Init(200.0);
//	US.AvgV = 0.0;
	update_freq = 20.0;
}


void user_cb_done(void)
{
//	MasterCtl.stop_module();
#ifdef DEBUG
	fprintf(stderr,"user_cb_done()\n");
#endif
//	Callbacks.stop();
	MasterCtl.Done();
	Callbacks.Done();

	Trigger.Done();
}

void user_start(void)
{
	frame_num = 0;
}

// double delay = 0.0;



double self_delay = 0.0;

long cbo_st, cb_st1, cb_st2,send_time,receive_time;



int tell_frame = 0;

double send_delay = 0.0;
double receive_delay = 0.0;


int vvv = 0;

void user_cb(char run, char learn)
{
	cbo_st = cb_st1;
	cb_st1 = GetTime();
	
	update_freq += (((double)(cb_st1 - cbo_st) - update_freq) / 100.0);
	self_delay += (((double)(end_cb_stamp - cbo_st) - self_delay) / 100.0);

	long t1,t2,t3;
	int d,e;

	if (control_mode != _cm_SIMULATOR)
           {
	    t1 = GetTime();
	    e = Robot_.MsgQueue.transfer_packet();	t2 = GetTime();
	    d = Robot_.MsgQueue.receive_packet();	t3 = GetTime();

	    receive_time = (t3 - t2);
	    send_time = (t2 - t1);
    //	fprintf(stderr,"r:%2ld s:%2ld d:%2d e:%2d\n",receive_time,send_time,d,e);
           }
	  else
	   {
	    d = 1;
	    e = 1;    
	   }

	if (d > 0)
	   receive_delay += (((double)d - receive_delay) / 50.0);
	if (e > 0)
	   send_delay += (((double)e - send_delay) / 50.0);

	char some_new = (d > 0);

	char crash = MasterCtl.calculate_module(run , some_new);

	if (crash == true)
	    MasterCtl.control_module();
	 else	
	   {	
//	    if (some_new)
//	       {
//		fprintf(stderr,"d ");
		int v = 0;
		v += Callbacks.Execute_Calc(run, learn && ~crash , some_new);
		v += Callbacks.Execute_Cntl(run, false);
        				//     ^^ temporarly paused
#ifdef DEBUG
		if (vvv != v)
		   {
		    fprintf(stderr,"%d\n",v);
		    vvv = v;	
		   }
#endif

		if (v == 0)
		   usleep(40000);	
//	       }		
//	     else
//	       {
//		fprintf(stderr,"i ");       
			    // ^^ idle, nothing was received
//		Callbacks.Execute_Calc(run, learn && crash , false);
//	       }	 
	   } 	

	Update_Display(d);

	US.Add(receive_delay + send_delay - update_freq);	

//	int us = (int)(((send_delay+receive_delay) - update_freq) * 1000.0);
//	if (run == false)
//	   usleep(40000);
//	 else
//	   if (us > 0)
//	      usleep(us*3);
//	c_st = GetTime();

//	if (d == 0)
	end_cb_stamp = GetTime();

	if (control_mode != _cm_SIMULATOR)
           {
	    if (US.AvgV > 0.0)
	       usleep((long int)(US.AvgV*10000.0));
	     else
		US.AvgV = 0.0; 	

	    if (d == 0)
	       usleep(4000); 

	    usleep(1000);
	   }
}






void Display_Odo(void)
{ 
	char st[256];
	
	if (Trigger.Snapped(Odo_trg) == false)
	   return;

	Robot_.MsgQueue.request_coords();	

	if (Robot_.With_Gripper == true)
	   {
	    Robot_.MsgQueue.request_gripwidth();
	    Robot_.MsgQueue.request_resistor();

	     //show if something in gripper 
	    if (Robot_.catched == true)
	       fl_set_button(fd_panel->contact_bt,1);  
	     else  
	       fl_set_button(fd_panel->contact_bt,0);  

	    //show gripwidth
	    fl_set_slider_size(fd_panel->grip_sl,(Robot_.gr_width/255.0));

	     //show electrical resistor
	     /* c = robot.ReadGripOhmMeter(); */

	    sprintf(st,"%d",Robot_.ohm);
	    fl_set_input(fd_panel->resistor_in,st);
	   } 
	
	//show kompass 
	fl_set_dial_value(fd_panel->alpha_dl,Robot_.alpha);   
	fl_set_dial_value(fd_panel->aspect_dl,Robot_.Loc.aspect);   

	//show coords   scal between 0..1
	t_vector scal;
	scal.x = (Robot_.Loc.P.x - Robot_.World.min.x) / Robot_.World.size.x;
	scal.y = (Robot_.Loc.P.y - Robot_.World.min.y) / Robot_.World.size.y;

	fl_set_positioner_xvalue(fd_panel->world_pos,scal.x);
	fl_set_positioner_yvalue(fd_panel->world_pos,scal.y);
} 



void Display_IR(void)
{
     double d;

	if (Trigger.Snapped(LED_trg) == false)
	   return;
     
     Robot_.MsgQueue.request_proximity();
     Robot_.MsgQueue.request_ambient();

     for (int i=0; i<8; i++)
       {
	switch (ired_avg_bool) 
	  {
	    case 0: d = Robot_.Ired->Sensor[i]->Value;	break; 
	    case 1: d = Robot_.Ired->Sensor[i]->AvgV;	break;
	   default: d = Robot_._Ired_->Sensor[i]->AvgV;	break;
	  }        

	fl_set_slider_value(fd_panel->ired_sl[i],d);

	switch (ambl_avg_bool) 
	  {
	    case 0: d = Robot_.Ambl->Sensor[i]->Value;	break;
	    case 1: d = Robot_.Ambl->Sensor[i]->AvgV;	break;
	   default: d = Robot_._Ambl_->Sensor[i]->AvgV;	break;
	  }        

        fl_set_slider_value(fd_panel->ambl_sl[i],d);
       }
}



void Display_Camera(void)
{
	if (Robot_.With_Camera == false)
	   return;

	if (control_mode == _cm_SIMULATOR)
	   return;	

//    if (Rob.ImageRead() == false)
//        return;

	if (Trigger.Snapped(Cam_trg) == false)
	   return;

	Robot_.MsgQueue.request_image();
//	fprintf(stderr,"image-req\n");	  	
       
	FilterPass();

    float x[128],y[128];

    for (int i=0; i<128; i++)
      {
       x[i] = (float)(i % 64);	 
       y[i] = ((float)Robot_.Image[i])/255.0;	
      } 		

//    fl_set_xyplot_data(fd_panel->lcd_sl[1], x, y, 64, "","","");

    for (int i=0; i<64; i++)
      {
       fl_set_slider_value(fd_panel->lcd_sl[i],y[i]); 
//       fl_add_chart_value(fd_panel->lcd_sl[1],y[i],"",FL_WHITE);
      }

    fl_set_xyplot_data(fd_panel->filter_plot, &x[64], &y[64], 64, "","","");

    Robot_.MsgQueue.request_brightness(); 
    fl_set_slider_value(fd_panel->iris_sl,log(Robot_.Iris.Value));
}



/*
void Display_Gadgets(void)
{
}
*/



void Update_Display(int d)
{
	Trigger.Update();

	if (fl_get_button(fd_panel->visual_bt) != 0)
	   Display_Camera();

	if (fl_get_button(fd_panel->odo_bt) != 0)
	   Display_Odo();

	if (fl_get_button(fd_panel->led_bt) != 0)
	   Display_IR();

	if (Trigger.Snapped(Canvas_trg) == false)
	   return;
  

	char st[256];
//	int plen;

	MasterCtl.Show();

	sprintf(st,"%6.2fms",send_delay);
	fl_set_object_label(fd_panel->send_delay,st);

	sprintf(st,"%6.2fms",receive_delay);
	fl_set_object_label(fd_panel->receive_delay,st);

	sprintf(st,"%6.2fms",update_freq);
	fl_set_object_label(fd_panel->update,st);

	sprintf(st,"%3.1f%%",(update_freq - self_delay) *100.0 / update_freq);
//	sprintf(st,"%6.2fms",self_delay);
	fl_set_object_label(fd_panel->unfair,st);

	sprintf(st,"%6.2fms",US.AvgV);
//	sprintf(st,"%6.2fms",self_delay);
	fl_set_object_label(fd_panel->upsleep,st);

	sprintf(st,"%6.2f pts",MasterCtl.Phase.AvgV);
//	sprintf(st,"%6.2fms",self_delay);
	fl_set_object_label(fd_panel->upphase,st);

	if (VScr.fd_xfbgi->canvas->visible)
	   {
#ifdef DEBUG
	    fprintf(stderr,"showing 0x%lx\n",VScr.fd_xfbgi->canvas);
#endif
	    VScr.SetPage(1);

	    VScr.Fill(FL_BLACK);
	    XFlush(VScr.dpy);

	    Callbacks.Show();

	    VScr.SetPage(0);

	    if (fl_get_button(fd_panel->mpeg_bt) != 0)
	       {
		char *s = getenv("HOME");
		if (s !=	NULL)	
        	   sprintf(st,"%s/.kcontrol/screenshots/ss-%05d.rle",s,frame_num);
		long t1 = GetTime();
		VScr.Save(st);
		long t2 = GetTime();

		frame_num++;
		sprintf(st,"%ldms",t2-t1);
	        fl_set_object_label(fd_panel->save_costs,st);
	       } 
	   }
}















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