// Emanuele Ercoli 702270, emanuele.ercoli@studenti.unimi.it
// Progetto per il corso di Sistemi Intelligenti A.A. 2006/2007
//

#include "AIBOCoccole.h"
#include <OPENR/OSyslog.h>
#include <OPENR/core_macro.h>
#include <OPENR/OPENRAPI.h>
#include "entry.h"
#include "../Motion/MotionInterface.h"

AIBOCoccole::AIBOCoccole()
{
};

OStatus AIBOCoccole::DoInit(const OSystemEvent& event)
{
  OSYSPRINT(("AIBOCoccole::DoInit()\n"));

  NEW_ALL_SUBJECT_AND_OBSERVER;
  REGISTER_ALL_ENTRY;
  SET_ALL_READY_AND_NOTIFY_ENTRY;

  //Apro le primitive di gestione dei sensori di tatto posteriore e per il calcolo della distanza
  const char *LOCATOR_DIST = "PRM:/r1/c1/c2/c3/p1-Sensor:p1";  //il sensore della testa (near) (5/50 cm)
  //const char *LOCATOR_DIST = "PRM:/r1/c1/c2/c3/p2-Sensor:p2";  //il sensore della testa (far) (20/150 cm)
  const char *LOCATOR_BACK = "PRM:/t2-Sensor:t2";  //ultimo dei tre bottoni sulla schiena

  //Accendo i sensori
  OStatus result1 = OPENR::OpenPrimitive(LOCATOR_BACK, &sensorIDBack);
  OStatus result2 = OPENR::OpenPrimitive(LOCATOR_DIST, &sensorIDDist);
  if (result1 != oSUCCESS)
    OSYSPRINT(("AIBOCoccole::DoInit(): ERROR - il sensore della schiena non si è avviato\n"));
  if (result2 != oSUCCESS)
    OSYSPRINT(("AIBOCoccole::DoInit(): ERROR - il sensore per la distanza non si è avviato\n"));

  return oSUCCESS;
}

OStatus AIBOCoccole::DoStart(const OSystemEvent& event)
{
  OSYSPRINT(("AIBOCoccole::DoStart()\n"));

  ENABLE_ALL_SUBJECT;
  ASSERT_READY_TO_ALL_OBSERVER;

  for (register short i = 0; i < N_AZIONI; i++)   //inizializzo le funzioni valore
    Q[i] = 0;
  reward = 0;   //e il reward

  //Avvio il timer per il ragionamento
  int dummy=0;
  EventID sincroEvent = EventID();
  RelativeTime period(10, 0);     //periodicamente ragiona su cosa è meglio fare
  TimeEventInfoWithRelativeTime timeInfo(TimeEventInfo::Periodic, period);
  sError error=SetTimeEvent(&timeInfo,myOID_,Extra_Entry[entryTimerMind],&dummy,sizeof(dummy),&sincroEvent);
  if(error!=sSUCCESS)
    OSYSPRINT(("AIBOCoccole::DoStart(): ERROR - TimerMind non si è avviato\n"));

  //Avvio il timer per il polling da sensore coccole
  int dummy2=0;
  EventID sincroEvent2 = EventID();
  RelativeTime period2(0, 300);    //polling coccole
  TimeEventInfoWithRelativeTime timeInfo2(TimeEventInfo::Periodic, period2);
  sError error2 = SetTimeEvent(&timeInfo2,myOID_,Extra_Entry[entryTimerSensor],&dummy2,sizeof(dummy2),&sincroEvent2);
  if(error2!=sSUCCESS)
    OSYSPRINT(("AIBOCoccole::DoStart(): ERROR - TimerSensor non si è avviato\n"));

  //Avvio il timer che mi guida nell'evitare gli ostacoli mentre cammino
  int dummy3=0;
  EventID sincroEvent3 = EventID();
  RelativeTime period3(1, 0);      //polling distanza
  TimeEventInfoWithRelativeTime timeInfo3(TimeEventInfo::Periodic, period3);
  sError error3 = SetTimeEvent(&timeInfo3,myOID_,Extra_Entry[entryTimerDist],&dummy3,sizeof(dummy3),&sincroEvent3);
  if(error3!=sSUCCESS)
    OSYSPRINT(("AIBOCoccole::DoStart(): ERROR - TimerDist non si è avviato\n"));

  //Scelgo azione iniziale a random
  register double l = (double)rand()/RAND_MAX;
  if (l <= 0.5)
    azione = FERMO;
  else
    azione = CAMMINO;
  
  controlloAzione();  //esegue l'azione scelta

  return oSUCCESS;
}

OStatus AIBOCoccole::DoStop(const OSystemEvent& event)
{
  OSYSPRINT(("AIBOCoccole::DoStop()\n"));

  DISABLE_ALL_SUBJECT;
  DEASSERT_READY_TO_ALL_OBSERVER;
  OPENR::SetMotorPower(opowerOFF);

  //Spengo i sensori
  OPENR::ClosePrimitive(sensorIDBack);
  OPENR::ClosePrimitive(sensorIDDist);

  return oSUCCESS;
}

OStatus AIBOCoccole::DoDestroy(const OSystemEvent& event)
{
  OSYSPRINT(("AIBOCoccole::DoDestroy()\n"));

  DELETE_ALL_SUBJECT_AND_OBSERVER;

  return oSUCCESS;
}

//Calcola il valore delle azioni e sceglie la migliore seguendo una politica epsilon-greedy
void AIBOCoccole::TimerMind(void *msg)
{
  //OSYSPRINT(("AIBOCoccole::TimerMind()\n"));

  if (reward == 0)	
    reward = -2;	//ottengo un reward negativo in caso di non ricezione reward

  //Aggiorno la funzione valore corrente: Q_new = Q_old + step_size * (reward - Q_old)
  if (azione == FERMO)
    Q[0] = Q[0] + ALFA * (reward - Q[0]);
  if (azione == CAMMINO)
    Q[1] = Q[1] + ALFA * (reward - Q[1]);

  OSYSPRINT(("\nAIBOCoccole::TimerMind(): valore delle azioni: FERMO: %lf, CAMMINO: %lf\n", Q[0],Q[1]));

  register double l = (double)rand()/RAND_MAX;    //numero pseudocasuale in [0,1]
  OSYSPRINT(("AIBOCoccole::TimerMind(): l: %lf, EPSILON: %lf\n", l,EPSILON));

  if (l >= EPSILON) //l'azione prossima è effettivamente la più performante, sto massimizzando, azione greedy
  {
    OSYSPRINT(("AIBOCoccole::TimerMind(): azione greedy (eccetto che per valori di azioni uguali)\n"));

    if (Q[0] > Q[1])
      azione = FERMO;
    else if (Q[0] < Q[1])
      azione = CAMMINO;
    else    //in caso di parità lo scelgo a random
    {
      if (l <= 0.5)
        azione = FERMO;
      azione = CAMMINO;
    }
  }
  else      //simile a prima ma con i valori invertiti, sto esplorando
  {
    OSYSPRINT(("AIBOCoccole::TimerMind(): azione esplorativa (eccetto che per valori di azioni uguali)\n"));

    if (Q[0] > Q[1])
      azione = CAMMINO;
    else if (Q[0] < Q[1])
      azione = FERMO;
    else
    {
      if (l <= 0.5)
        azione = FERMO;
      azione = CAMMINO;
    }
  }

  reward = 0;  //reinizializzo il reward per la prossima azione
  controlloAzione();   //effettuo fisicamente il cambiamento di azione
}

//Controlla il valore del sensore posteriore
void AIBOCoccole::TimerSensor(void *msg)
{
  //OSYSPRINT(("AIBOCoccole::TimerSensor()\n"));

  OPENR::GetSensorValue(sensorIDBack, &sensorValueBack);   //leggo il valore dal sensore posteriore

  //Se ho avuto le coccole
  if(sensorValueBack.value >= 10)
    {
      OSYSPRINT(("AIBOCoccole::TimerSensor(): coccole ricevute :-)\n"));
      
      reward++;                               //il reward è uguale a 1 in caso di coccole

      //Sono felice!
      Motion::MotionCommand goal;
      memset(&goal, 0, sizeof(goal));
      goal.head_cmd = Motion::HEAD_ANGLES;
      goal.head_tilt2 = 45;	//testa su giù (-15,45)
      goal.mouth = 100;
      goal.sound_cmd = Motion::SOUND_FILE;
      //goal.sound_file = Motion::SOUND_FILE_TEST;      //io
      goal.sound_file = Motion::SOUND_FILE_TADA;      //il Tilli!!!!
      goal.setBackLEDs(Motion::LED_BACK_FRONT_WHITE | Motion::LED_BACK_MIDDLE_WHITE | Motion::LED_BACK_REAR_COLOR, Motion::LED_BRIGHT);
      goal.setFaceLEDs(Motion::LED_GREEN_EYES, Motion::LED_BRIGHT, Motion::MODE_B);
      subject[sbjMotionControl]->SetData(&goal, sizeof(goal));
      subject[sbjMotionControl]->NotifyObservers();

      Wait(static_cast<longword>(1000000000));       //tempo di festeggiare...in nanosecondi!

      controlloAzione();                             //continua con l'agire normalmente
    }
}

//Controllo che non ci siano ostacoli mentre cammina
void AIBOCoccole::TimerDist(void *msg)
{
  //OSYSPRINT(("AIBOCoccole::TimerDist()\n"));

  if (azione == CAMMINO)
  {
    OPENR::GetSensorValue(sensorIDDist, &sensorValueDist);   //leggo il valore dal sensore anteriore per la distanza
    register slongword dist = sensorValueDist.value;

    if (dist < LIMITE)
    {
      OSYSPRINT(("AIBOCoccole::TimerDist(): ho trovato un ostacolo, giro per evitarlo\n"));

      Motion::MotionCommand gira;
      memset(&gira, 0, sizeof(gira));
      gira.motion_cmd = Motion::MOTION_WALK_TROT;
      gira.vx = vel;  //mm/s
      gira.va = RAD(90);  //rad/s, giro sempre da una sola parte
      gira.head_cmd = Motion::HEAD_ANGLES;
      gira.head_tilt2 = -15;	//testa su giù (-15,45)
      gira.setBackLEDs(Motion::LED_BACK_FRONT_WHITE | Motion::LED_BACK_MIDDLE_WHITE | Motion::LED_BACK_REAR_COLOR, Motion::LED_BRIGHT);
      gira.setFaceLEDs(Motion::LED_RED_EYES_LOW, Motion::LED_BRIGHT, Motion::MODE_A);
      subject[sbjMotionControl]->SetData(&gira, sizeof(gira));
      subject[sbjMotionControl]->NotifyObservers();
    }
    else                                           //altrimenti continuo/torno ad andare dritto
    {
      Motion::MotionCommand dritto;
      memset(&dritto, 0, sizeof(dritto));
      dritto.motion_cmd = Motion::MOTION_WALK_TROT;
      dritto.vx = vel;  //mm/s
      dritto.va = RAD(0);
      dritto.head_cmd = Motion::HEAD_ANGLES;
      dritto.head_tilt2 = -15;	//testa su giù (-15,45)
      dritto.setBackLEDs(Motion::LED_BACK_FRONT_WHITE | Motion::LED_BACK_MIDDLE_WHITE | Motion::LED_BACK_REAR_COLOR, Motion::LED_BRIGHT);
      dritto.setFaceLEDs(Motion::LED_RED_EYES_LOW, Motion::LED_BRIGHT, Motion::MODE_A);
      subject[sbjMotionControl]->SetData(&dritto, sizeof(dritto));
      subject[sbjMotionControl]->NotifyObservers();
    }
  }
}

//Si occupa di far eseguire all'AIBO i movimenti specificati per ciascun azione
void AIBOCoccole::controlloAzione()
{
  switch(azione)
  {
    case FERMO:
    {
      OSYSPRINT(("AIBOCoccole::controlloAzione(): FERMO\n"));

      //Resto fermo ad aspettare le coccole
      Motion::MotionCommand stop;
      memset(&stop, 0, sizeof(stop));
      stop.motion_cmd = Motion::MOTION_STAND_NEUTRAL;
      stop.head_cmd = Motion::HEAD_ANGLES;
      stop.head_tilt2 = -15;	//testa su giù (-15,45)
      stop.setBackLEDs(Motion::LED_BACK_FRONT_WHITE | Motion::LED_BACK_MIDDLE_WHITE | Motion::LED_BACK_REAR_COLOR, Motion::LED_BRIGHT);
      stop.setFaceLEDs(Motion::LED_RED_EYES_LOW, Motion::LED_BRIGHT, Motion::MODE_A);
      subject[sbjMotionControl]->SetData(&stop, sizeof(stop));
      subject[sbjMotionControl]->NotifyObservers();
    }
    break;

    case CAMMINO:
    {
      OSYSPRINT(("AIBOCoccole::controlloAzione(): CAMMINO\n"));

      //Comandi di movimento
      Motion::MotionCommand go;
      memset(&go, 0, sizeof(go));
      go.motion_cmd = Motion::MOTION_WALK_TROT;
      go.vx = vel;  //specificato nel .h
      go.head_cmd = Motion::HEAD_ANGLES;       //abbasso la testa per usare meglio il sensore della distanza
      go.head_tilt2 = -15;	//testa su giù (-15,45)
      go.setBackLEDs(Motion::LED_BACK_FRONT_WHITE | Motion::LED_BACK_MIDDLE_WHITE | Motion::LED_BACK_REAR_COLOR, Motion::LED_BRIGHT);
      go.setFaceLEDs(Motion::LED_RED_EYES_LOW, Motion::LED_BRIGHT, Motion::MODE_A);
      subject[sbjMotionControl]->SetData(&go, sizeof(go));
      subject[sbjMotionControl]->NotifyObservers();
    }
    break;

    default:
      OSYSPRINT(("AIBOCoccole::controlloAzione(): default\n"));
    break;
  }
}
