// 
// File name: Sensor.cpp
//
// Discription: Base classes
//
// Programmer: Vladimir P. Stasevich (SV)
//
// History: 
//    Created 30.01.2000 (SV)
//

#include "Sensor.h"
#include "Robot.h"
#include "Configur.h"

TForces TSensor::GetCurrentForces( )
{
   TForces res;

   return res;
}

TForces TSensor::GetDeltaForces( TTouchType type )
{
   TForces res;

//   res = GetCurrentForces() - prevForces;

   switch( type )
   {
      case NOTOUCH :
      case LSURFACETOUCH :
      case RSURFACETOUCH :
      case LFINALTOUCH :
      case RFINALTOUCH :

         res.Fx = 0;
         res.Fz = 0;
         res.My = 0;
         break;

      case LFACETTOUCH:
      case RFACETTOUCH:

         res.Fx = 0.6;
         res.Fz = 1.2;
         res.My = 0.06;
         break;
      
      case LONEPOINTTOUCH:
      case RONEPOINTTOUCH:

         res.Fx = 0.9;
         res.Fz = 8;
         res.My = 0.025;
         break;

      case LTWOPOINTTOUCH:
      case RTWOPOINTTOUCH:

         res.Fx = 0.2;
         res.Fz = -6;
         res.My = -0.04;
         break;

      case LJAMMINGTOUCH:
      case RJAMMINGTOUCH:

         res.Fx = 0.5;
         res.Fz = 15;
         res.My = 0.5;
         break;
   }

   res.Fx /= MAXFX;
   res.Fz /= MAXFZ;
   res.My /= MAXMY;

   unsigned fx, fz, my;

   config.GetMistakes( fx, fz, my );

   res.Fx = res.Fx - fx/2/100*res.Fx + res.Fx*random(fx)/100;
   res.Fz = res.Fz - fz/2/100*res.Fz + res.Fz*random(fz)/100;
   res.My = res.My - my/2/100*res.My + res.My*random(my)/100;
   
   return res;
}
