/* 
 * Thomas Pantzer: CrashTest_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 "copyright.h"

#include "t_runtime_obj.h"

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

#define _CrashTest_DEMO_G
#include "CrashTest_demo.h"


#include <unistd.h>
#include <stdio.h>
#include <math.h>




void t_avg_group::Init(void)
{
	Set.Init(3);
	Got.Init(3);
	LongSet.Init(20);
	LongGot.Init(20);
	Diff.Init(150);
}

void t_avg_group::Add(double s, double g, int phase)
{
	Set.Add(s);
	Got.Add(g);

	LongSet.Add(Set.AvgV);
	LongGot.Add(Got.AvgV);

	Diff.Add(LongSet.Get(phase) - LongGot.Get((int)0));
}

void t_avg_group::Done(void)
{
	Diff.Done();
	LongGot.Done();
	LongSet.Done();
}




#define CrashTest_nBoxes	3
#define CrashTest_nSliders	2
#define CrashTest_nChoices	0

#define	blocked_th	0.9
#define slipping_th	1.7
#define phase_avg_len	500.0


char CrashTest_boxes[CrashTest_nBoxes][32] = {"erster\0","zweite\0","drittes\0"};
char CrashTest_sliders[CrashTest_nSliders][16] = {"uno\0","dos\0"};


void t_CrashTest_demo::Init(RobotContext *robot)
{
	char *cb_pointers[CrashTest_nBoxes];
	char *sl_pointers[CrashTest_nSliders];

	for (int i=0; i<CrashTest_nBoxes; i++)
          cb_pointers[i]  = &CrashTest_boxes[i][0];

	for (int i=0; i<CrashTest_nSliders; i++)
          sl_pointers[i]  = &CrashTest_sliders[i][0];

	int pro = 0; 
	pro |= _p_control;	//  does something to control the robot
	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("CrashTest",robot,pro,
		CrashTest_nSliders,CrashTest_nBoxes,0,
		sl_pointers,cb_pointers,NULL);
	ds_avg = 0.0;

	LeftMed = new t_avg_group;
	LeftMed->Init();

	RightMed = new t_avg_group;
	RightMed->Init();

	DG.Init(50);
	DS.Init(50);
	DR.Init(100);

	ph = 6;
	Phase.Init(800);
	Phase.AvgV = 3.0;
	crashflags = 0;
}


void t_CrashTest_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_CrashTest_demo::slider_cb(FL_OBJECT *ob,long data)
{
	switch(data)
	  {
	   default:
	     break; 	
	  }
}

void t_CrashTest_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_CrashTest_demo::Save(int fd)
{
	t_runtime_object::Save(fd);	// call parent for generic savings

	fprintf(stderr,"t_CrashTest_demo::Save()\n");
	// then save our private stuff
}

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


void t_CrashTest_demo::Show(void)
{
}




void t_CrashTest_demo::make_logstring(void)
{
	char st2[256];

	sprintf(Logs.data,"%g %g %g %g %g %g %g %d %g %g %g %g\t",
			LeftMed->Set.AvgV,
			LeftMed->Got.AvgV,
			LeftMed->LongSet.Get(ph),
			LeftMed->LongGot.Get(0),
			LeftMed->LongSet.AvgV,
			LeftMed->LongGot.AvgV,
			LeftMed->Diff.AvgV,
			ph,
//			DGL.avg,
			DS.AvgV,
			DG.AvgV,
//			DSL.avg,DGL.avg,

			DR.AvgV,
			(double)crashflags/(-8.0)
			);
			
	sprintf(st2,"%g\n",Phase.AvgV);
	strcat(Logs.data,st2);
}

char t_CrashTest_demo::calculate_module(char allow_learn, char any_new)
{
	if (t_runtime_object::calculate_module(allow_learn,any_new) == false)
	   return false;
	
//	do some calculation in our spare time after this comment line
//	fprintf(stderr,"c ");

	Robot->MsgQueue.request_motorspeed();
	Robot->MsgQueue.request_coords();	

//	char st[256],st2[256];

	double sl = (double)Robot->set_speed_left;
	double sr = (double)Robot->set_speed_right;
	double gl = (double)Robot->speed_left;
	double gr = (double)Robot->speed_right;

	LeftMed->Add(sl,gl,ph);
	RightMed->Add(sr,gr,ph);
	
	DS.Add(LeftMed->Set.AvgV - RightMed->Set.AvgV);
	DG.Add(LeftMed->Got.AvgV - RightMed->Got.AvgV);

	DR.Add(DS.AvgV - DG.AvgV);

	if (gl > (LeftMed->Set.max*slipping_th))
	   {
//	    fprintf(stderr,"slip left\a\n");	// got more than expect, that was a crash
	    gl = LeftMed->Set.max;			// correct it for phase lookup
	    crashflags |= slip_left;
	   }

	if (gr > (RightMed->Set.max*slipping_th))
	   {
//	    fprintf(stderr,"slip right\a\n");
	    gr = RightMed->Set.max;
	    crashflags |= slip_right;	
	   }

	if (fabs(DR.AvgV) > blocked_th) 
	   {
//	    fprintf(stderr,"bl.");
	    crashflags |= wheels_blocked; 
	   } 
         else
	   {
	    if (fabs(LeftMed->Diff.AvgV) > blocked_th) 
	       {
//		if ((crashflags & hang_left) > 0)
//		   fprintf(stderr,"hang left\a\n");

		crashflags |= hang_left;
	       }

	    if (fabs(RightMed->Diff.AvgV) > blocked_th)
	       {
//		if ((crashflags & hang_right) > 0)
//		    fprintf(stderr,"hang right\a\n");
		crashflags |= hang_right;
	       } 
           } 

	if (crashflags == 0)
	   {	
            int g = 0;
	    double me = 0.0;

	    for (int i=0; i<18; i++)
	      {
	       double E = 0.0;

	       for (int j=0; j<80; j++)
        	 {
		  double d = DG.Get(j) * DS.Get(j+i);
		  E += d;	      	
		 }

	       if (E > me)
		  {
		   g = i;
		   me = E;
		  }
	      }

	    Phase.Add((double)g);

	    if (Phase.AvgV > 16.0)	// stack has only depth 10 
	       Phase.AvgV = 16.0;

//	    if ((Phase - floor(Phase)) > 0.8)
//	       ph = (int)(ceil(Phase));
//             else
            ph = (int)Phase.AvgV;
	   }	


	crashflags = 0;
	
        return false;	// no crash detected so far

//	return true;
}


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

//	place your control instructions after this comment line
//	if (crashflags == 0)
//	   return false;

	//crashflags = 0;

	return true;
}



char t_CrashTest_demo::start_module(void) 
{
	if (t_runtime_object::start_module() == false)
	   return false;

//	place after that
	old_path = NULL;
	Logs.wival = 1;
	Logs.average = 1.0;

	return true;
}


char t_CrashTest_demo::stop_module(void)
{
	if (t_runtime_object::stop_module() == false)
	   return false;
	
//	place after that

	return true;
}

void t_CrashTest_demo::Done(void)
{	
	DG.Done();
	DS.Done();

	RightMed->Done();
	LeftMed->Done();

	t_runtime_object::Done();
}







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