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

#include "copyright.h"

#define _KOMPASS_DEMO_G
#include "kompass_demo.h"

#include "graphix.h"
//#include "trigonom.h"


double gwidth = 1.1;
double zwidth = 1.1;


double deg,deg2,deg5,deg6,info,truth,reli;

t_vertex_2i old[5];
	
void t_kompass_demo::Init(RobotContext *robot)
{
	t_runtime_object::Init("Kompass",robot);
//	props |= _p_control;
	props |= _p_calc;
	props |= _p_show;
}



char t_kompass_demo::start_module(void) 
{
	if (t_runtime_object::start_module() == false)
	   return false;
	
        Robot->MsgQueue.send_motorspeed(-1,0); 	

	return true;
}

char t_kompass_demo::stop_module(void)
{
	if (t_runtime_object::stop_module() == false)
	   return false;
	
        Robot->MsgQueue.send_motorspeed(0 ,0); 	

	return true;
}


void t_kompass_demo::make_logstring(void)
{
	double v = angle_average(RAD(deg),RAD(deg2));

//	char st[2048];

	sprintf(Logs.data,"%g %g %g %g %g %g\n",
	            Robot->alpha, 
		RAD_NORM(M_PI - angle_difference(Robot->alpha,RAD(deg))), 
		angle_difference(Robot->alpha,v),
		v, 
		angle_difference(RAD(deg),RAD(deg2)),
		angle_difference(Robot->alpha,deg5));
}

char t_kompass_demo::calculate_module(char allow_learn, char any_new)
{
	if (any_new == false)
	   return false;	

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

	// put your calculations here
//	fprintf(stderr,"calc_module(Kompass)\n");
	Robot->MsgQueue.request_coords();
	Robot->MsgQueue.request_ambient();
	Robot->MsgQueue.request_proximity();

	deg2 = Robot->Kompass_old(AMBL,true,zwidth,info);
	deg = Robot->Kompass_old(AMBL,false,gwidth,truth);

	deg5 = Robot->Kompass(AMBL,reli);
//	deg6 = Robot->Kompass(IRED,reli);

	return true;
}


double Information(RobotContext *Robot)
{
   return 1.0;   
}

void DrawAngle(double deg,double length,int thickness/*, int &lx, int &ly*/)
{
	int dx2,dy2;
        
	dy2 = -(int)(sin(RAD(deg)) * length);
	dx2 = (int)(cos(RAD(deg)) * length);
	dx2 += VScr.center.x;
	dy2 += VScr.center.y;

        VScr.LineWidth = thickness;
	VScr.DrawLine(VScr.center.x,VScr.center.y,dx2,dy2);
}



void DrawGaussian(double deg, char down)
{
//      SetColor(color);
//      VScr.LineWidth = 1;
   	int ox,oy;

	ox = 0;
	oy = 0;
        for (int i=0; i<VScr.width; i++)
          {
           double x = (((double)i/(double)VScr.width)*4.0*M_PI) - M_PI;
           double m;
           int y;
           
           m = RAD_NORM(-x + RAD(deg));
           
           if (down == true)
              y = ((VScr.height*3)/4) - 22 + (int)(gaussian(m,zwidth,1.0)*(double)VScr.height/4.0);
            else  
              y = VScr.height - 20 - (int)(gaussian(m,gwidth,1.0)*(double)VScr.height/4.0);
           
           if (i != 0)
              drawline(VScr.width - ox,oy,VScr.width - i,y);
              
           ox = i;
           oy = y;   
          } 

}



	int w_idx[16] = {5,12,4,11,3,10,2,9,1,8,0,15,7,14,6,13}; 


void DrawBars(int sensor_type, RobotContext *Robot)
{
	char st[256];

	t_SensorGroup *SensGrp;
	if (sensor_type == IRED)
	   SensGrp = Robot->_Ired_;
         else
	   SensGrp = Robot->_Ambl_; 
   
	for (int k=0; k<32; k++)
	  {
           int i = w_idx[k % 16];
	   int x,y,w,h;
           
	   w = 5;
	   h = (int)(SensGrp->Sensor[i]->ClipV * 100.0);

	   y = VScr.height - h;
	   y -= 20;

	   double X = (SensGrp->Sensor[i]->Loc - M_PI)/(-2.0*M_PI);
           X *= (double)(VScr.width/2);
	   x = (int)X;

	   if ((k/16) > 0)
	      x += (VScr.width/2);
           

	// gelbe Sensoren werden ausgewertet
	// gefüllte Sensoren sind echte

	   if (Robot->DistSensor(i) == true)
	      SetColor(FL_YELLOW);
	    else
	      SetColor(FL_DARKORANGE);

	   if (i >= 8)
 	      { DrawRectangle(x,y,w,h); }
	    else  
  	      { FillRectangle(x,y,w,h); }

	   sprintf(st,"%d",i);
           SetColor(FL_CYAN);

	   if ((i < 8) && ((k/16) == 0))
	      OutTextXY(x,VScr.height,st);

	   if ((i >= 8) && ((k/16) > 0))
	      OutTextXY(x,VScr.height,st);
	  }
	
}

void t_kompass_demo::Show(void)
{
//	if (visible == false)
//	   return;
	
	char st[256];
	double al,north,n2;//,deg3;
	
	al = DEG(RAD_NORM(Robot->alpha));
        
	north = (al + deg) / 2.0;
	n2 = 180.0 - deg;
	
   	SetColor(FL_WHEAT);
          double length = (double)VScr.center.y/2.0;
          DrawGaussian(180.0 - deg,false);
   	  DrawAngle(deg+180.0,length,3/*,old[0].x,old[0].y*/);
  	  sprintf(st,"deg: %4.1f°, gwidth: %2.3f, truth: %g",deg,gwidth,truth);
	  OutTextXY(10,40,st);

   	SetColor(FL_BLUE);
          DrawGaussian(-deg2,true);
   	  DrawAngle(deg2+180.0,length,3/*,old[1].x,old[1].y*/);
  	  sprintf(st,"deg2: %4.1f°, zwidth: %2.3f, level: %g",deg2,zwidth,0.333333 * log(8));
	  OutTextXY(10,60,st);
        
   	SetColor(FL_RED);
          n2 = (deg2 - deg)/2.0;
          if (fabs(n2) > 90.0)
              n2 = DEG_NORM(n2 + deg + 180);
           else   
              n2 = DEG_NORM(n2 + deg);
          
  	  DrawAngle(n2,length,2/*,old[3].x,old[3].y*/);
  	  sprintf(st,"north: %4.1f°",n2);
	  OutTextXY(10,100,st);

   	SetColor(FL_DARKCYAN);
          length = (double)VScr.center.y*2.0/3.0;
  	  DrawAngle(al,length,1/*,old[2].x,old[2].y*/);
  	  sprintf(st,"al: %4.1f° rounds: %3.1f",al,DEG(Robot->alpha)/360.0);
	  OutTextXY(10,80,st);

        DrawBars(AMBL,Robot);

	SetColor(FL_WHITE);
	DrawAngle(DEG(deg5),100.0,5);
	SetColor(FL_DARKORANGE);
	DrawAngle(DEG(deg6),80.0,7);
}











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