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

extern Comunicacio_Robot *interficie_robot;

void Thread_Moviment_Robot::execute()
{
   Logica_Robot *logica = interficie_robot -> get_Logica_Robot();
   int moviment_actiu = logica -> get_moviment();
   int moviment_possible = 1;
   while (moviment_actiu) {
      
      // IMPLEMENTAR...
      
      if (moviment_possible) {
         moviment_actiu = logica -> get_moviment();
      }
      else {
         moviment_actiu = false;
         interficie_robot -> tractar_Emergencia("moviment_impossible");
      }      
   }   
}   

void Thread_Control_Termic::execute()
{
   
}   

// --------------------------------------
// Implementar la classe Logica_Robot...
// --------------------------------------

Logica_Robot::Logica_Robot()
{
   robot_en_moviment = 0;
}   

int Logica_Robot::get_moviment()
{
   return robot_en_moviment;
}

void Logica_Robot::desactivar_moviment()
{
   robot_en_moviment = 0;    
} 

void Logica_Robot::activar_moviment()
{
   robot_en_moviment = 1;    
}    

TPosicio *Logica_Robot::get_posicio_actual()
{
   return &posicio_actual;
}   

void Logica_Robot::girar_robot(int graus)
{
   if (posicio_actual.angle_orientacio + graus >= 360) {
      posicio_actual.angle_orientacio = posicio_actual.angle_orientacio + graus - 360;
   }
   else if (posicio_actual.angle_orientacio + graus < 0) {
      posicio_actual.angle_orientacio = posicio_actual.angle_orientacio + graus + 360;
   }
   else {
      posicio_actual.angle_orientacio = posicio_actual.angle_orientacio + graus;
   }   
}   

void Logica_Robot::girar_cameres(int graus)
{
   if (angle_cameres + graus >= 360) {
      angle_cameres = angle_cameres + graus - 360;
   }
   else if (angle_cameres + graus < 0) {
      angle_cameres = angle_cameres + graus + 360;
   }
   else {
      angle_cameres = angle_cameres + graus;
   }
}   

int Logica_Robot::get_angle_cameres()
{
   return angle_cameres;
}   

int Logica_Robot::get_nivell_bateria()
{
   return nivell_bateria;
}   

