void SetJoints(JointPos& j)
   {
     mutex.Wait();
     state.joints = j;
     last_neck_time = YARPTime::GetTimeAsSeconds(); 
     mutex.Post();
   }

 void SetGyro(double n_yaw, double n_pitch, double n_roll)
   { 
     mutex.Wait();
     state.yaw = n_yaw;  state.pitch = n_pitch;  state.roll = n_roll; 
     last_gyro_time = YARPTime::GetTimeAsSeconds();
     mutex.Post();
   }

void MarkTarget(int id)
{
  printf("Marking current region as target %d\n", id);
  state_mutex.Wait();
  target_manager.Set(id,global_theta,global_phi);
  state_mutex.Post();
}

int YARPSocketNameService::RegisterName(const char *name)
{

  int name_status = -1;
  int result = -1;

  //static YARPInputSocket sock;
  //sock.Register(name);
  //test_global = &sock;

  DBG(5) printf("^^^^^^^^ checking for previous definitions\n");
  if (GetThreadSocket()==NULL)
    {
      DBG(5) printf("^^^^^^^^ checks out okay\n");
      int pid = my_getpid();
      mutex.Wait();
      DBG(5) printf("^^^^^^^^ creating\n");
      YARPInputSocket *is = new YARPInputSocket;
      is_map[pid] = is;
      DBG(5) printf("^^^^^^^^ preparing to register\n");
      is->Register(name);
      result = is->GetAssignedPort();
      mutex.Post();
      DBG(5) printf("^^^^^^^^ Made an input socket handler\n");
      fflush(stdout);
    }

  return result;
}

 virtual void OnRead()
   {
     assert(Read(0));
     state_mutex.Wait();
     state_joint = Content();
     state_mutex.Post();
   }

 int Get(HeadState& dest)
   {
     int result = IsValid();
     mutex.Wait();
     dest = state;
     mutex.Post();
     return result;
   }

 void SetEyes(double n_tilt, double n_left_pan, double n_right_pan)
   { 
     mutex.Wait();
     state.tilt = n_tilt;  state.left_pan = n_left_pan;  
     state.right_pan = n_right_pan;
     last_eyes_time = YARPTime::GetTimeAsSeconds(); 
     mutex.Post();
   }

void ResetControl()
{
  mutex.Wait();
  HeadMessage& msg = out_cmd.Content();
  msg.type = HeadMsgNonValid;
  out_cmd.Write();
  mutex.Post();
}

  int IsValid()
    {
      double now = YARPTime::GetTimeAsSeconds();
      mutex.Wait();
      int result = /*(now-last_gyro_time<1)&&*/(now-last_eyes_time<1)&&
	(now-last_neck_time<1);
      mutex.Post();
      return result;
    }

// LATER: find out why this is commented out.
static void RemovePort(YARPPort *port)
{
	ACE_UNUSED_ARG(port);

	port_list_mutex.Wait();

//#ifndef __QNX__
	//  port_list.erase(port);
//#endif
	port_list_mutex.Post();
}

  void SetNeck(double n_tilt, double n_roll,
	       double n_lean, double n_pan)
    {
      mutex.Wait();
      state.neck_tilt = n_tilt;
      state.neck_roll = n_roll;
      state.neck_lean = n_lean;
      state.neck_pan = n_pan;
      //last_neck_time = YARPTime::GetTimeAsSeconds(); 
      mutex.Post();
    }

YARPInputSocket *GetThreadSocket()
{
  int pid = my_getpid();
  YARPInputSocket *result = NULL;
  mutex.Wait();
  if (is_map.find(pid)!=is_map.end())
    {
      result = is_map[pid];
    }
  mutex.Post();
  return result;
}

void YARPPort::DeactivateAll()
{
	port_list_mutex.Wait();

	PortList::iterator it(port_list);
	it.go_head();		// might not be required.

	while (!it.done())
	{
		(*it)->Deactivate();
		it++;
	}

	port_list_mutex.Post();
}

void RequestOrientation(double theta, double phi)
{
  mutex.Wait();
  HeadMessage& msg = out_cmd.Content();
  msg.type = HeadMsgMoveToOrientation;
  msg.j1 = theta;
  msg.j2 = phi;
  msg.j3 = 0;
  msg.j4 = theta;
  msg.j5 = phi;
  msg.j6 = 0;
  msg.j7 = 0;
  out_cmd.Write();
  mutex.Post();
}

int main()
{
  in_gyro.Register("/egomap/i:gyro");
  in_eye.Register("/egomap/i:eye");
  out_dir.Register("/egomap/o:dir");

  while (1)
    {
      wake_up_call.Wait();

      HeadState state;
      int ok = head_state_manager.Get(state);
      if (ok)
	{
	  CogGaze dir;
	  dir.Apply(state.joints);
	  
	  printf("LEFT %+8.2f %+8.2f %+8.2f      RIGHT %+8.2f %+8.2f %+8.2f\n", 
		 dir.theta_left*180.0/M_PI, 
		 dir.phi_left*180.0/M_PI,
		 dir.roll_left*180.0/M_PI,
		 dir.theta_right*180.0/M_PI, 
		 dir.phi_right*180.0/M_PI,
		 dir.roll_right*180.0/M_PI);
	}
      //YARPTime::DelayInSeconds(0.05);
    }

  return 0;
}

//--------------------------------------------------------------------------------------
//       Class: SoundResources 
//      Method: (public)_uninitialize
// Description:  
//--------------------------------------------------------------------------------------
int
SoundResources::_uninitialize (void)
{
	_bmutex.Wait ();
	
	m_InRecord = false;
    mixerClose(m_MixerHandle);   // Close mixer
    waveInReset(m_WaveInHandle); // Reset the wave input device

    if(_rawBuffer != NULL){
        delete[] _rawBuffer;     // Delete the shared buffer
        _rawBuffer = NULL;
    }
    _bmutex.Post ();

	return YARP_OK;
}

inline int PicoloResources::_init (const PicoloOpenParameters& params)
{
	/// copy params.
	int ret;
	_nRequestedSize = params._size_x;
	_nWidth = params._size_x;
	_nHeight = params._size_y;
	_nImageSize = params._size_x * params._size_y * 3;

	init_bttvx(params._video_type,params._unit_number,_nWidth,_nHeight, params._offset_x, params._offset_y);

	_bmutex.Wait ();
	_rawBuffer = new unsigned char [_nImageSize];
	ACE_ASSERT (_rawBuffer != NULL);
	_bmutex.Post ();

	return ret;
}

inline int PicoloResources::_uninitialize (void)
{
	_bmutex.Wait ();
	close_bttvx();

	if (_rawBuffer != NULL) delete[] _rawBuffer;
	_rawBuffer = NULL;

	_nRequestedSize = 0;
	_nWidth = 0;
	_nHeight = 0;
	_nImageSize = 0;
	_canpost = true;

	_bmutex.Post ();

	return YARP_OK;
}

  void BeginObservations()
    {
      if (!observing)
	{
	  mutex.Wait();
	  UpdateActivity();
	  observing = 1;
	}
    }

  virtual void OnRead()
    {
      assert(Read(0));
      JointPos& datum = Content();
//      printf("eye %g %g %g\n", datum(1), datum(2), datum(3));
      head_state_manager.SetEyes(datum(1), datum(2), datum(3));
//      head_state_manager.SetNeck(datum(6), datum(7), datum(4), datum(5));
      head_state_manager.SetNeck(datum(4), datum(5), datum(7), datum(6));
      head_state_manager.SetJoints(datum);
      wake_up_call.Post();
    }