#include "Logica_Robot.h"
#include "Interficie_Mart.h"
#include "Comunicacio_Robot.h"

extern Simulador_Mart *simulador_mart;
extern Visualitzador_Mart *visor_mart;
extern Comunicacio_Robot *interficie_robot;

//int moviment_actiu;
USING_PTYPES

FILE *fitxerPosicions;

void Thread_Moviment_Robot::execute()
{
   int error;           // Variable per detectar errors de moviment.
   int i;
   cua resum;           // Llista de punts que s'han de recorrer.
   TPosicio posicio;    // Variable aux per a tractar les diferents posicions.
   TPosicio poscamara;  // Auxiliar per obtenir la posicio de cada camara
   
   float inc_x = 0;
   float inc_y = 0;
   float inc_z = 0;
//   FILE *fitxerPosicions;

   datetime DataHora;
   int tempsInici;
   int tempsFinal; 
   
   if ((fitxerPosicions = fopen("moviments", "w")) ==NULL){
       printf("Error opening file\n");
       exit(1); 
   }
        
   probot->get_posicio_actual(&posicio);
   fprintf(fitxerPosicions,
                  "X: %f, Y: %f, Z: %f, Orientaciķ: %i, Horitzo: %i\n",
                  posicio.x, posicio.y, posicio.z, posicio.angle_orientacio, 
                  posicio.angle_horitzo);

   //DataHora = now();
   //tempsInici = msecs(DataHora);

   probot->esborrar_posicions();

   while (probot->get_moviment() == 1) 
   {
       // Obtenim cua de punt a visitar;
       error = probot->calcular_seguent_posicio(&resum);
       if (error != -1)
       {
           /*DataHora = now();
           tempsFinal = msecs(DataHora);

           if (tempsFinal - tempsInici >= 10000)
           {
               relax(2000);
               DataHora = now();
               tempsInici = msecs(DataHora);
           } */                   

           TPosicio pos_anterior = posicio;
           if (resum.EsBuida() != 1)
           {  
               posicio = resum.llegir();
           }   

           //TPosicio pos_anterior = posicio;
           while (resum.EsBuida() != 1)
           {   
               relax(50);
               
               inc_x = (posicio.x - pos_anterior.x) / 10;
               inc_y = (posicio.y - pos_anterior.y) / 10;
               inc_z = (posicio.z - pos_anterior.z) / 10;

               // dividim el desplaįament d'un centímetre en 10 passos.              
               for(int num_frame = 0; num_frame < 10; num_frame++) 
               {
                  for (i = 1;i <= 9; i++) 
                  {
                      probot->get_posicio_camera(i,&poscamara);
                      visor_mart->actualitzar_camera(i, poscamara.x + (inc_x * num_frame), 
                                   poscamara.y + (inc_y * num_frame), poscamara.z + (inc_z * num_frame),
                                   (int) poscamara.angle_orientacio, (int) poscamara.angle_horitzo);
                      
                  }
                  relax(50);
               }   
               
               // Fixem següent posiciķ  
               probot->set_posicio_actual(&posicio);

               // Aquest printf s'ha desubstituir per reposicionament de camares
               fprintf(fitxerPosicions,
                      "X: %f, Y: %f, Z: %f, Orientaciķ: %i, Horitzo: %i\n",
                      posicio.x, posicio.y, posicio.z, posicio.angle_orientacio, 
                      posicio.angle_horitzo);
 
               pos_anterior = posicio;

               posicio = resum.llegir();
           }  
           relax(50);

           inc_x = (posicio.x - pos_anterior.x) / 10;
           inc_y = (posicio.y - pos_anterior.y) / 10;
           inc_z = (posicio.z - pos_anterior.z) / 10;

           for(int num_frame = 0; num_frame < 10; num_frame++) 
           {
              for (i=1;i<=9;i++) 
              {
                 probot->get_posicio_camera(i,&poscamara);

                 visor_mart->actualitzar_camera(i, poscamara.x + (inc_x * num_frame), 
                                         poscamara.y + (inc_y * num_frame), poscamara.z + (inc_z * num_frame),
                                        (int) poscamara.angle_orientacio, (int) poscamara.angle_horitzo);
              }
              relax(50);
           }    

           // Fixem següent posiciķ  
           probot->set_posicio_actual(&pos_anterior);
           
           // Aquest printf s'ha de substituir per reposicionament de camares
           fprintf(fitxerPosicions,
                  "X: %i, Y: %f, Z: %i, Orientaciķ: %i, Horitzo: %i\n",
                  posicio.x, posicio.y, posicio.z, posicio.angle_orientacio, 
                  posicio.angle_horitzo);
       }   
       else
       {
          probot->desactivar_moviment(); 
          // caldria activar algun senyal d'emergencia.
       }   
    }   
    fclose(fitxerPosicions);
}

//************************************
// Thread controlador de temperatura
//************************************
void Thread_Control_Termic::execute()
{
   // En construccio...
   TPosicio pos_actual;
   int robot_actiu = interficie_robot -> get_Funcionament();
   while(robot_actiu) {
      relax(PERIODE_CONTROL_TEMPERATURA);
      probot -> get_posicio_actual(&pos_actual);
      int temp = simulador_mart -> get_temperatura((int)pos_actual.x,(int)pos_actual.z);
      // Si la temperatura es menor a -20ēC s'activen els escalfadors
      // i es desactiven els radiadors.
      if(temp < -20) {
          if (!probot -> escalfadors_activats()) {
             probot -> activar_escalfadors();
             visor_mart -> set_Missatge("Escalfadors Activats");
          }   
          if (probot -> radiadors_activats()) {
             probot -> desactivar_radiadors();
             visor_mart -> set_Missatge("Radiadors Desactivats");
          }
      }
      // Si la temperatura esta entre -20ēC i 20ēC es desactiven els 
      // radiadors i els escalfadors.
      else if (temp > -20 && temp < 20) {
          if (probot -> escalfadors_activats()) {
             probot -> desactivar_escalfadors();
             visor_mart -> set_Missatge("Radiadors Desactivats");              
          }    
          if (probot -> radiadors_activats()) {
             probot -> desactivar_escalfadors();
             visor_mart -> set_Missatge("Escalfadors Desactivats");              
          }
      }
      // Si la temperatura supera els 20ēC i es de dia s'activen
      // els radiadors per dissipar la calor.
      else if (temp > 20 && simulador_mart -> esDeDia()) {
          if (!probot -> radiadors_activats()) {
             probot -> activar_radiadors();
             visor_mart -> set_Missatge("Radiadors Activats");
          }   
      }
      // Si la temperatura supera els 20ēC i es de dia s'activen
      // els radiadors per dissipar la calor.
      else if (temp > 20 && !simulador_mart -> esDeDia()) {
          if (probot -> radiadors_activats()) {
             probot -> desactivar_radiadors();
             visor_mart -> set_Missatge("Radiadors Desactivats");
          }          
      }    
      robot_actiu = interficie_robot -> get_Funcionament();      
   }   
}

//*************************************
// Thread controlador de les bateries
//*************************************
void Thread_Control_Bateries::execute()
{
   int robot_actiu = interficie_robot -> get_Funcionament();
   int dia = simulador_mart->esDeDia();
   int increment = 0;
   while(robot_actiu)
   {
      int moviment = interficie_robot -> get_moviment();
      if (simulador_mart->esDeDia()) {
         if(moviment) {
            increment = -1;  // Bateria baixa poc gracies als panells solars
         }
         else {
            increment = 2;   // Es carreguen els panells solars
         }
         if(!dia) {
            probot -> set_DegradaBat(probot -> get_DegradaBat() + 1); // Degrada un 1% la bateria cada mig dia
         }   
         dia = 1;
      }   
      else {
         if(moviment) {
            increment = -3;  // Bateria es descarrega mes de nit amb moviment
         }
         else {
            increment = 0;   // Augment nul de la bateria durant la nit
         }
         if(dia) {
            probot -> set_DegradaBat(probot -> get_DegradaBat() + 1); // Degrada un 1% la bateria cada mig dia
         }   
         dia = 0;
      }
      probot -> set_NivellBateria(probot-> get_NivellBateria() + increment);
      if (probot-> get_NivellBateria() > 100 - probot -> get_DegradaBat()) {
         probot-> set_NivellBateria(100 - probot -> get_DegradaBat());
      }
      relax(PERIODE_CONTROL_BATERIA);      
      robot_actiu = interficie_robot -> get_Funcionament();
   }
}

//****************************************
// Constructor de la classe Logica_Robot
//****************************************
Logica_Robot::Logica_Robot() 
{
   Parcela_Mart Parcela;      // Parcela del mapa a visitar coordenades (i,j).
   TPosicio poscamara;
   int i;
   
   // insert your code here
	Rcoord_x = 30;     // Posiciķ inicial en x del robot.  
	Rcoord_z = 30;     // Posiciķ inicial en y del robot.

   Rorientacio = 45; // Orientaciķ inicial del robot.
   Rhoritzo = 0;     // Inclinacio sobre l'horitzo.          

   Cap_H = 0;    // Posiciķ horitzontal (angle 0..360) del cap del robot.
   Cap_V = 0;    // Posiciķ vertical (angle 90..-90) del cap del robot.

   //simulador_mart -> get_parcela(Rcoord_x, Rcoord_z, &Parcela);
   Rcoord_y = simulador_mart -> get_altura((int)Rcoord_x, (int)Rcoord_z);
   
   // Inicialitzem cāmares de 1 a 9.
   for (i=1;i<10;i++)
   {
      get_posicio_camera(i,&poscamara);
      visor_mart->inicialitzar_cameres(i, poscamara.x, poscamara.y, poscamara.z, 
                                    (int) poscamara.angle_orientacio,(int) poscamara.angle_horitzo);
   }   

   Radiadors = 0;    // valor inicial de funcionament.
   Escalfadors = 0;  // valor inicial de funcionament.

   Moviment = 0; // determina si el robot estā o no en moviment '0'/'1'.
        
  
   NivellBateria = 100;
   
   DegradaBat = 0;
}

// class destructor
Logica_Robot::~Logica_Robot() {
	// insert your code here
}

//*****************************************************************************
// Valida que la posiciķ (i,j) es trobi dins l'angle de visiķ del robot si-
// tuat a (x,y), amb orientaciķ "orientacio".
// retorna: nova orientaciķ que tindrā el robot.
//*****************************************************************************
int Logica_Robot::puntValid(int x, int z, int i, int j, int orientacio)
{
int difX, difZ;
int penGraus;
int angleSup, angleInf;

difX = i-x;
difZ = j-z;

if (difZ > 0)
{
   if (difX == 0) penGraus = 90;
   else if (difX < 0) penGraus = 135;
   else penGraus = 45;
}
else if (difZ == 0)
{  
   if (difX < 0) penGraus = 180;
   else penGraus = 0;
}
else
{
   if (difX == 0) penGraus = 270;
   else if (difX < 0) penGraus = 225;
   else penGraus = 315;
}  

// Calculem Angles d'obertura, valors entre 0 i 359 graus;
angleSup = (orientacio + (OberturaHazcam / 2)) % 360;
angleInf = (orientacio - (OberturaHazcam / 2) + 360) % 360;

if (angleInf > angleSup)
{
   if ((penGraus >= angleSup) && (penGraus <= angleInf))
       return -1;
   else 
       return penGraus;
}
else // (angleInf <= angleSup)
{
   if ((penGraus <= angleSup) && (penGraus >= angleInf))
       return penGraus; 
   else 
       return -1;
}
}

//*****************************************************************************
// Calcula una llista de possibles veins
// retorna: una cua amb els punts veins que compleixen les restriccions. 
//*****************************************************************************
void Logica_Robot::CalcularVeins(int x, float y, int z, 
                                 int orientacio, cua *veins)
{
int i,j;

// Variables per cālcul orientacio del robot, resultant del moviment
int auxOrientacio; 

// Coordenades dels punts del mapa a evaluar
int mapX, mapZ;
float mapY;

float distan2D,penRadians; // Variables per al cālcul de la pendent.
int   penGraus;

struct TPosicio vei;       // coordenades posicio veina a visitar.
int MaxFiles;              // Maxim files del mapa.
int MaxColumnes;           // Maxim columnes del mapa.

float altaux = 0; // Variable aux per simular alįada.

MaxFiles = simulador_mart->get_num_files_mapa();
MaxColumnes = simulador_mart->get_num_columnes_mapa();

// Comenįa codi del mčtode.
for (i=x-1;i<=x+1;i++)
{
   mapX = (i + MaxFiles) % MaxFiles;
   for (j=z-1;j<=z+1;j++)
   
   {
      if (i!=x || j!=z)
      {
         mapZ = (j + MaxColumnes) % MaxColumnes;
                 
         // En cas de tractar-se d'un punt valid retorna la nova orientaciķ del 
         // robot.
         auxOrientacio = puntValid(x,z,i,j,orientacio);
         if (auxOrientacio != -1)
         {
             // Calculem la pendent.
             // *******************************************************
             // PROBABLE PUNT D'ERROR EN ARRIBAR A UN EXTREM DEL MAPA
             // *******************************************************
             //std::cout << "\nMAP_X: " << mapX << ", MAP_Z: " << mapZ << "\n";
             mapY = simulador_mart -> get_altura(mapX, mapZ); 

             if (y-mapY != 0)
             {
                 distan2D = (float) sqrt(((x-i)*(x-i)) + 
                                         ((z-j)*(z-j)));
                 if (distan2D == 0) penRadians = PI / 2;                       
                 else penRadians = atan((mapY-y)/distan2D);
             }
             else penRadians = 0.0;
       
             penGraus = (int) ((penRadians * 180) / PI);

             // Si la pendent no supera el maxim es guarda.
             if (penGraus < PendentMaxima)
             {         
                 vei.x = (float) mapX;
                 vei.y = mapY;
                 vei.z = (float) mapZ;
                 vei.angle_orientacio = auxOrientacio;
                 vei.angle_horitzo    = penGraus;
                 veins->afegir(&vei);
             }   
         }         
      }  
   }
}  

}

//*****************************************************************************
// Calcula una trajectoria per als propers moviments.
// omple la cua resultat amb un cami de "ProfPlanif2" punts
// retorna -1 si no troba cami.
//*****************************************************************************
int Logica_Robot::calcular_seguent_posicio(cua *resultat)
{
int    profAux    = 0;
int    trobatCami = 0;
cua    llistaNodes;
float  pendentMinima;
struct TPosicio AuxCoord, segPunt;

// Obtenir punts veins a partir posiciķ del robot.
CalcularVeins((int)Rcoord_x, Rcoord_y, (int) Rcoord_z, Rorientacio, &llistaNodes);
if (llistaNodes.EsBuida() == 1)
  trobatCami = -1;

while ((profAux < ProfPlanif) && (trobatCami == 0))
{
   // llegir del primer punt;
   AuxCoord = llistaNodes.llegir();
   pendentMinima = AuxCoord.angle_horitzo;
   segPunt.x = AuxCoord.x;
   segPunt.y = AuxCoord.y;
   segPunt.z = AuxCoord.z;
   segPunt.angle_orientacio = AuxCoord.angle_orientacio;
   segPunt.angle_horitzo    = AuxCoord.angle_horitzo;

   int fillTrobat = 0;
   int visitat = 0;

   std::vector<TPosicio>::const_iterator itFills;
   // Mentre quedin punts veins fer
   while (llistaNodes.EsBuida() == 0)
   {
      // Es comprova si la posicio actual s'ha visitat previament
      //std::vector<TPosicio>::const_iterator itFills;
      itFills = pos_visitades.begin();
      while((itFills != pos_visitades.end()) && (visitat == 0)) {
         TPosicio pos_actual = *itFills;
         if((pos_actual.x == AuxCoord.x) && (pos_actual.y == AuxCoord.y)) {
            visitat = 1;
            fprintf(fitxerPosicions,
                  "Posicio visitada X: %f, Y: %f, Z: %f, Orientaciķ: %i, Horitzo: %i\n",
                  pos_actual.x, pos_actual.y, pos_actual.z, pos_actual.angle_orientacio, 
                  pos_actual.angle_horitzo);
         }   
         itFills++;
      }

      // Obtenim el punt amb pendent minima que no hagi estat visitat anteriorment
      if ((AuxCoord.angle_horitzo < pendentMinima) && (!visitat))
      {
          fillTrobat = 1;
          pendentMinima = AuxCoord.angle_horitzo;
          // Copiem el punt amb pendent mínima
          segPunt.x = AuxCoord.x;
          segPunt.y = AuxCoord.y;
          segPunt.z = AuxCoord.z;
          segPunt.angle_orientacio = AuxCoord.angle_orientacio;
          segPunt.angle_horitzo    = AuxCoord.angle_horitzo;
      }

      // Pop del primer punt;
      AuxCoord = llistaNodes.llegir();
   }
   
   // tractem darrer element recuperat.
   // Es comprova si la posicio actual s'ha visitat previament
   visitat = 0;
   //std::vector<TPosicio>::const_iterator itFills;
   itFills = pos_visitades.begin();
   while((itFills != pos_visitades.end()) && (visitat == 0)) {
      TPosicio pos_actual = *itFills;
      if((pos_actual.x == AuxCoord.x) && (pos_actual.y == AuxCoord.y)) {
         visitat = 1;
         fprintf(fitxerPosicions,
                 "Posicio visitada X: %f, Y: %f, Z: %f, Orientaciķ: %i, Horitzo: %i\n",
                 pos_actual.x, pos_actual.y, pos_actual.z, pos_actual.angle_orientacio, 
                 pos_actual.angle_horitzo);
      }   
      itFills++;
   }
   
   if ((AuxCoord.angle_horitzo <= pendentMinima) && (!visitat))
   {
      pendentMinima = AuxCoord.angle_horitzo;
      // Copiem el punt amb pendent mínima
      segPunt.x = AuxCoord.x;
      segPunt.y = AuxCoord.y;
      segPunt.z = AuxCoord.z;
      segPunt.angle_orientacio = AuxCoord.angle_orientacio;
      segPunt.angle_horitzo    = AuxCoord.angle_horitzo;
      fillTrobat = 1;
   }

   // Si tots els veins ja s'han visitat s'esborra la llista de posicions visitades
   if(fillTrobat == 0) {

      fprintf(fitxerPosicions,"No s'ha trobat fill!\n");
   }
   else {
      pos_visitades.push_back(segPunt);
      resultat->afegir(&segPunt);
      profAux = profAux + 1; 
      fprintf(fitxerPosicions,"Guarda punt al vector: %f, Y: %f, Z: %f, Orientaciķ: %i, Horitzo: %i\n",
                 segPunt.x, segPunt.y, segPunt.z, segPunt.angle_orientacio, 
                 segPunt.angle_horitzo);
   }   

   if (profAux < ProfPlanif)
   {
      CalcularVeins((int)segPunt.x, segPunt.y, (int)segPunt.z, 
                    segPunt.angle_orientacio, &llistaNodes);
      if (llistaNodes.EsBuida() == 1)
         trobatCami = -1;
   }   
}

return trobatCami;

}

//*****************************************************************************
// Posiciona el robot a una localitzacio determinada.
//*****************************************************************************
void Logica_Robot::set_posicio_actual(struct TPosicio *posicio_actual)
{
   Rcoord_x = posicio_actual->x;                  // Posiciķ en x del robot.
   Rcoord_y = posicio_actual->y;                  // Posiciķ en y del robot.
   Rcoord_z = posicio_actual->z;                  // Posiciķ en z del robot.   
   
   Rorientacio = posicio_actual->angle_orientacio;// Orientaciķ del robot.
   Rhoritzo = posicio_actual->angle_horitzo;      // Inclinacio sobre l'horitzo.  
}

//*****************************************************************************
// Retorna la posiciķ en la que es troba el robot.
//*****************************************************************************
void Logica_Robot::get_posicio_actual(struct TPosicio *posicio_actual)
{
    posicio_actual->x = Rcoord_x;
    posicio_actual->y = Rcoord_y;
    posicio_actual->z = Rcoord_z;
    posicio_actual->angle_orientacio = Rorientacio;
    posicio_actual->angle_horitzo = Rhoritzo;
    
    /*std::cout << "\nPOSICIO_ACTUAL.X = " << posicio_actual->x;
    std::cout << "\nPOSICIO_ACTUAL.Y = " << posicio_actual->y;
    std::cout << "\nPOSICIO_ACTUAL.Z = " << posicio_actual->z;
    std::cout << "\nPOSICIO_ACTUAL.ORIENTACIO = " << posicio_actual->angle_orientacio;
    std::cout << "\nPOSICIO_ACTUAL.HORITZO = " << posicio_actual->angle_horitzo;*/
}

// retorna l'estat de cārrega de la bateria.
int Logica_Robot::get_nivell_bateria()
{
    return NivellBateria;
}   

// Permet modificar l'orientaciķ del robot.
void Logica_Robot::girar_robot(int graus)
{
    Rorientacio = abs(Rorientacio + graus) % 360;
    TPosicio poscamara;
    for (int i= 1; i <= 9; i++)
    {
       get_posicio_camera(i,&poscamara);
       visor_mart->actualitzar_camera(i, poscamara.x, poscamara.y, poscamara.z, 
                                       (int) poscamara.angle_orientacio,(int) poscamara.angle_horitzo);
    }     
}

// Permet posicionar el cap del robot segons els angles de gir vertical
// i horitzontal. En cas de no poder assolir la posiciķ dessitjada re- 
// torna -1.
int Logica_Robot::posicionar_cap(int Pos_H) //, int Pos_V)
{
    /*if ((Pos_V < -90) | (Pos_V > 90))
        return -1;*/
        
    //Cap_V = Pos_V;
    
    /*if ((Pos_H < 0) || (Pos_H > 360))
        return -1;*/
    
    Cap_H = abs(Cap_H + Pos_H) % 360;

    TPosicio poscamara;
    for (int i= 1; i <= 9; i++)
    {
       get_posicio_camera(i,&poscamara);
       visor_mart->actualitzar_camera(i, poscamara.x, poscamara.y, poscamara.z, 
                                       (int) poscamara.angle_orientacio,(int) poscamara.angle_horitzo);
    }     
    
    return 0;  
}

// Metode per llegir la posiciķ de una camara.
//
// Situaciķ cāmeres del cap
//        1           2
//            3   4
//
// Cāmeres HAZCAMS.
//              5     7
//         <-
//              6     8
int Logica_Robot::get_posicio_camera(int numCam,struct TPosicio *posicio_actual)
{
int error = 0;   
   
    posicio_actual->x = Rcoord_x;
    posicio_actual->y = Rcoord_y;
    posicio_actual->z = Rcoord_z;
    posicio_actual->angle_horitzo = Rhoritzo;  
  
    switch (numCam)
    {
      case 1: posicio_actual->x = posicio_actual->x + (distNavcam/2);
              posicio_actual->y = posicio_actual->y + AlsadaCap + distNavPan;
              posicio_actual->angle_orientacio = Rorientacio + Cap_H;
              posicio_actual->angle_horitzo += Cap_V;
              break;
      case 2: posicio_actual->x = posicio_actual->x - (distNavcam/2);
              posicio_actual->y = posicio_actual->y + AlsadaCap + distNavPan;
              posicio_actual->angle_orientacio = Rorientacio + Cap_H;              
              posicio_actual->angle_horitzo += Cap_V;
              break;
      case 3: posicio_actual->x = posicio_actual->x + (distPancam/2);
              posicio_actual->y = posicio_actual->y + AlsadaCap;
              posicio_actual->angle_orientacio = Rorientacio + Cap_H;              
              posicio_actual->angle_horitzo += Cap_V;
              break;
      case 4: posicio_actual->x = posicio_actual->x - (distPancam/2);
              posicio_actual->y = posicio_actual->y + AlsadaCap;
              posicio_actual->angle_orientacio = Rorientacio + Cap_H;              
              posicio_actual->angle_horitzo += Cap_V;
              break;
      case 5: posicio_actual->x = posicio_actual->x + (Hazcam_H/2);
              posicio_actual->z = posicio_actual->z + (Robot_F/2);
              posicio_actual->y = posicio_actual->y + AlsadaRod;
              posicio_actual->angle_orientacio = Rorientacio;              
              break;                                          
      case 6: posicio_actual->x = posicio_actual->x - (Hazcam_H/2);
              posicio_actual->z = posicio_actual->z + (Robot_F/2);
              posicio_actual->y = posicio_actual->y + AlsadaRod;
              posicio_actual->angle_orientacio = Rorientacio;
              break;
      case 7: posicio_actual->x = posicio_actual->x + (Hazcam_H/2);
              posicio_actual->z = posicio_actual->z - (Robot_F/2);
              posicio_actual->y = posicio_actual->y + AlsadaRod;
              posicio_actual->angle_orientacio = abs(Rorientacio - 180);              
              break;
      case 8: posicio_actual->x = posicio_actual->x - (Hazcam_H/2);
              posicio_actual->z = posicio_actual->z - (Robot_F/2);
              posicio_actual->y = posicio_actual->y + AlsadaRod;
              posicio_actual->angle_orientacio = abs(Rorientacio - 180);              
              break;
      case 9: posicio_actual->y = posicio_actual->y + AlsadaRod;
              posicio_actual->angle_orientacio = Rorientacio + Cap_H;
              posicio_actual->angle_horitzo += Cap_V; 
              break;
      default:error = -1;
    }                                
   
    return error;
}

// Permet consultar si el robot es troba en moviment, retorna 0 o 1.
int Logica_Robot::get_moviment()
{
    return(Moviment);
}

// Activem el moviment del robot.
void Logica_Robot::activar_moviment()
{
    Moviment = 1;    
}

// Desactivar el moviment del robot.
void Logica_Robot::desactivar_moviment()
{
    Moviment = 0;    
}

void Logica_Robot::activar_radiadors()
{
    Radiadors = 1;
}    

void Logica_Robot::activar_escalfadors()
{
    Escalfadors = 1;
}    
      
void Logica_Robot::desactivar_radiadors()
{
    Radiadors = 0;
}      

void Logica_Robot::desactivar_escalfadors()
{
    Escalfadors = 0;    
}    

int Logica_Robot::radiadors_activats()
{
    return Radiadors;
}    

int Logica_Robot::escalfadors_activats()
{
    return Escalfadors;
}    

int Logica_Robot::get_DegradaBat()
{
   return DegradaBat;
}   

void Logica_Robot::set_DegradaBat(int valor)
{
   DegradaBat = valor;
}

int Logica_Robot::get_NivellBateria()
{
   return NivellBateria;
}   

void Logica_Robot::set_NivellBateria(int valor)
{
   NivellBateria = valor;
}   

void Logica_Robot::esborrar_posicions()
{
   pos_visitades.clear();
}   


