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

ProvaCM::ProvaCM(){
  Motion::MotionCommand cmd;

  cmd.motion_cmd=Motion::MOTION_WALK_TROT;       
  cmd.head_cmd=Motion::HEAD_ANGLES;
  cmd.tail_cmd=Motion::TAIL_FOLLOW;
  cmd.bound_mode;       
  cmd.vx=150;
  cmd.vy=0;
  cmd.va=0;      
  cmd.head_lookat=vector3d(0,0,0);  
  cmd.head_tilt_offset=0;  
  cmd.head_tilt=-20.0; 
  cmd.head_pan=0;  
  cmd.head_roll=0; 
  cmd.head_tilt2=0; 
  cmd.tail_pan=60;       
  cmd.tail_tilt=0;   
  cmd.mouth=0;          
  cmd.sound_cmd=Motion::SOUND_FILE;
  cmd.sound_file=Motion::SOUND_FILE_RECTANGLE;
  cmd.setFaceLEDs(0,0,Motion::MODE_A);
  codaComandi.push(cmd);


  cmd.motion_cmd=Motion::MOTION_WALK_TROT;       
  cmd.head_cmd=Motion::HEAD_LOOKAT;
  cmd.tail_cmd=Motion::TAIL_FOLLOW;
  cmd.bound_mode;       
  cmd.vx=150;
  cmd.vy=0;
  cmd.va=0.5;      
  cmd.head_lookat=vector3d(15,15,15);  
  cmd.head_tilt_offset=0;  
  cmd.head_tilt=0; 
  cmd.head_pan=100;  
  cmd.head_roll=0; 
  cmd.head_tilt2=0; 
  cmd.tail_pan=-60;       
  cmd.tail_tilt=60;   
  cmd.mouth=0;          
  cmd.sound_cmd=Motion::SOUND_FILE;
  cmd.sound_file=Motion::SOUND_FILE_TADA;
  cmd.setFaceLEDs(0,0,Motion::MODE_A);
  cmd.setBinLEDs(Motion::LED_TOP_ORANGE);
  codaComandi.push(cmd);


  cmd.motion_cmd=Motion::MOTION_WALK_TROT;       
  cmd.head_cmd=Motion::HEAD_LOOKAT;
  cmd.tail_cmd=Motion::TAIL_NO_CMD;
  cmd.bound_mode;       
  cmd.vx=-125;
  cmd.vy=0;
  cmd.va=0;      
  cmd.head_lookat=vector3d(150,150,0);  
  cmd.head_tilt_offset=0;  
  cmd.head_tilt=0; 
  cmd.head_pan=0;  
  cmd.head_roll=100; 
  cmd.head_tilt2=0; 
  cmd.tail_pan=50;       
  cmd.tail_tilt=50;   
  cmd.mouth=0;          
  cmd.sound_cmd=Motion::SOUND_FILE;
  cmd.sound_file=Motion::SOUND_FILE_L;
  cmd.setFaceLEDs(0,0,Motion::MODE_A);
  cmd.setBinLEDs(Motion::LED_TOP_WHITE);
  codaComandi.push(cmd);


  cmd.motion_cmd=Motion::MOTION_GOAL_HAPPY2;       
  cmd.head_cmd=Motion::HEAD_ANGLES;
  cmd.tail_cmd=Motion::TAIL_NO_CMD;
  cmd.bound_mode;       
  cmd.vx=0;
  cmd.vy=0;
  cmd.va=2;      
  cmd.head_lookat=vector3d(0,0,0);  
  cmd.head_tilt_offset=0;  
  cmd.head_tilt=0; 
  cmd.head_pan=0;  
  cmd.head_roll=0; 
  cmd.head_tilt2=100; 
  cmd.tail_pan=50;       
  cmd.tail_tilt=50;   
  cmd.mouth=0;          
  cmd.sound_cmd=Motion::SOUND_NOTE;
  cmd.sound_frequency=1000;
  cmd.sound_duration=150000;
  cmd.setFaceLEDs(0,0,Motion::MODE_A);
  cmd.setBinLEDs(Motion::LED_TINY);
  codaComandi.push(cmd);


  cmd.motion_cmd=Motion::MOTION_TURN_WITH_BALL_R;       
  cmd.head_cmd=Motion::HEAD_NO_CMD;
  cmd.tail_cmd=Motion::TAIL_FOLLOW;
  cmd.bound_mode;       
  cmd.vx=0;
  cmd.vy=0;
  cmd.va=-2;      
  cmd.head_lookat=vector3d();  
  cmd.head_tilt_offset=0;  
  cmd.tail_pan=0;       
  cmd.tail_tilt=-100;   
  cmd.mouth=0;          
  cmd.sound_cmd=Motion::SOUND_NOTE;
  cmd.sound_frequency=10000;
  cmd.sound_duration=150000;
  cmd.sound_file=Motion::SOUND_FILE_TADA;
  cmd.setFaceLEDs(0,0,Motion::MODE_A);
  cmd.setBinLEDs(Motion::LED_EARS_CYAN);
  codaComandi.push(cmd);
}

OStatus ProvaCM::DoInit(const OSystemEvent& event){
  OSYSDEBUG(("ProvaCM::DoInit()\n"));
  
  NEW_ALL_SUBJECT_AND_OBSERVER;
  REGISTER_ALL_ENTRY;
  SET_ALL_READY_AND_NOTIFY_ENTRY;

  return oSUCCESS;
}


/*DoStart: fa partire Telepati*/
OStatus ProvaCM::DoStart(const OSystemEvent& event){
  OSYSDEBUG(("ProvaCM::DoStart()\n"));

  ENABLE_ALL_SUBJECT;
  ASSERT_READY_TO_ALL_OBSERVER;

  OStatus st = OPENR::SetMotorPower(opowerON);   

  int dummy=0;
  EventID sincroEvent = EventID();
  RelativeTime period(10, 0);
  TimeEventInfoWithRelativeTime timeInfo(TimeEventInfo::Periodic, period);
  sError error=SetTimeEvent(&timeInfo,myOID_,Extra_Entry[entryTimerEnd],&dummy,sizeof(dummy),&sincroEvent);
  if(error!=sSUCCESS) 
    OSYSLOG1((osyslogERROR, "::DoStart() : ERROR: Sincronization Timer not started\n"));
  
  
  return oSUCCESS;
}  //DoStart() END


/*DoStop: arresta Telepati*/
OStatus ProvaCM::DoStop(const OSystemEvent& event){
  OSYSDEBUG(("ProvaCM::DoStop()\n"));

  DISABLE_ALL_SUBJECT;
  DEASSERT_READY_TO_ALL_OBSERVER;
  OStatus st = OPENR::SetMotorPower(opowerOFF); 

  return oSUCCESS;
}  //DoStop() END


/*DoDestroy: libera la memoria occupata da Telepati*/
OStatus ProvaCM::DoDestroy(const OSystemEvent& event){
  OSYSDEBUG(("ProvaCM::DoDestroy\()\n"));

  DELETE_ALL_SUBJECT_AND_OBSERVER;

  return oSUCCESS;
}  //DoDestroy() END

void ProvaCM::TimerEnd(void* msg){
  if(!codaComandi.empty())
    {
      subject[sbjMotionControl]->SetData(&(codaComandi.front()),sizeof(Motion::MotionCommand));
      subject[sbjMotionControl]->NotifyObservers();
      codaComandi.pop();
      
    }
}

void ProvaCM::Notify(const ONotifyEvent& event){
  OSYSDEBUG(("ProvaCM:: ricevuto un notify\n"));
}

void ProvaCM::Ready(const OReadyEvent& event){
  OSYSDEBUG(("ProvaCM:: ricevuto Assert Ready\n"));
}
