#include <stdio.h>
#include <unistd.h>
#include "IviewControl.h"
#include "DmErrno.h"

#ifdef UNIX
# include <netdb.h>
#else
# include "hostLib.h"
# include "taskLib.h"
# include "sysLib.h"

#endif

#define dprintf if (debug) fprintf

extern "C" 
{
  STATUS semDelete( SEM_ID sem );
};

int IviewControl::_nIviewControls = 0;

IviewControl::IviewControl(IviewAxisMask axisMask, 
			   MBool startPeriodicRequests,
			   const char *name,
			   SEM_ID msgSem)
{
  MBool debug = FALSE;

  _processControlId = _nIviewControls++;

  Errno err;
  char errorBuf[errorMsgSize];
  
  // Create semaphore for detecting status updates
  if ((_statusSem = semBCreate(SEM_Q_FIFO, SEM_EMPTY)) == SEM_NULL)
  {
    setError("semBCreate() failed");
    return;
  }

  for (int srcIndex = 0; srcIndex < MaxControlSources; srcIndex++)
  {
    _dmControlSource = new DmControlSource(srcIndex);

    MBool used;
    _dmControlSource->hasClient(&used);

    if (!used) {

      /* Try to become client side of DmControlSource. Note that the
	 DmControlSource *might* be used by someone else by the time we 
	 get to the grabControlSource() call. */
      dprintf(stderr, 
	      "IviewControl::IviewControl() - Try to grab "
	      "control source #%d...\n", 
	      _dmControlSource->id());

      int status = grabControlSource();
      if (status == IviewSuccess) {
	  if (startPeriodicRequests)
	    status = startPeriodic();
      }
    
      if (status == IviewSuccess)
	break;
    
      if (debug) {
	prtErrorMsg();
	dprintf(stderr, 
		"IviewControl::IviewControl() - grab failed.\n");
      }
    }

    // Delete DmControlSource and try next one
    delete _dmControlSource;
  }


  if ((err = dm_start_consumer(_dmControlSource->statusItem(), 
			       DM_ASYNC, _statusSem)) 
      != SUCCESS)
  {
    sprintDmError(err, "IviewControl::IviewControl(), statusItem", errorBuf);
    setError(errorBuf);
  }

  if ((err = dm_start_consumer(_dmControlSource->messageItem(), DM_STATIC, 
			       msgSem)) 
      != SUCCESS)
  {
    sprintDmError(err, "IviewControl::IviewControl(), _messageItem", errorBuf);
    setError(errorBuf);
  }


#ifdef UNIX
  char hostName[256];
  gethostname(hostName, sizeof(hostName));
  struct hostent *hostEntry = gethostbyname(hostName);
  
  _aperiodicRequest.hostAddr = 
    ((struct in_addr *)(hostEntry->h_addr_list[0]))->s_addr;
  
#else
  BOOT_PARAMS bootParams;
  bootStringToStruct(sysBootLine, &bootParams);
  int netMask;
  bootNetmaskExtract(bootParams.ead, &netMask);
  _aperiodicRequest.hostAddr = inet_addr(bootParams.ead);
#endif

  _aperiodicRequest.controlId = _processControlId;
  
#ifdef UNIX
  _aperiodicRequest.pid = (int )getpid();
#else
  _aperiodicRequest.pid = taskIdSelf();
  
#endif

  if (name)
  {
    strncpy(_aperiodicRequest.controlName, name, MaxControlNameLen - 1);
    int len = strlen(_aperiodicRequest.controlName);
    _aperiodicRequest.controlName[len] = '\0';
  }
  else
    strcpy(_aperiodicRequest.controlName, "");
    
  _requestNo = 0;
  
  _axisMask = _aperiodicRequest.axisMask = axisMask;
  _aperiodicRequest.deviceMask = 0;
  _aperiodicRequest.refDevice = IviewPortCamera;
  _aperiodicRequest.sensitivity = IviewDefaultSensitivity;

  _periodicRequest.pan = _periodicRequest.tilt = _periodicRequest.focus = 
    _periodicRequest.zoom = _periodicRequest.iris = IviewNoChange;
  
  _started = FALSE;
  if (start() != IviewSuccess)
  {
    // Try again later
    clearError();
  }
}


IviewControl::~IviewControl()
{
  _aperiodicRequest.requestNo = _requestNo++;
  _aperiodicRequest.refDevice = IviewPortCamera;
  
  _dmControlSource->writeRequest(&_aperiodicRequest);
  
  _periodicRequest.requestNo = _requestNo++;
  
  _periodicRequest.pan = _periodicRequest.tilt = 
    _periodicRequest.focus = _periodicRequest.zoom = 
      _periodicRequest.iris = IviewNoChange;

  _dmControlSource->writeRequest(&_periodicRequest);

  delete _dmControlSource;

  semDelete(_statusSem);
}


int IviewControl::setDevices(IviewDeviceMask deviceMask, 
			     IviewDevice refDevice,
			     int timeoutTicks)
{
  _aperiodicRequest.refDevice = refDevice;
  _aperiodicRequest.deviceMask = deviceMask;
  _aperiodicRequest.requestNo = _requestNo++;

  int ret;
  
  if ((ret = _dmControlSource->writeRequest(&_aperiodicRequest)) 
      != IviewSuccess)
  {
    _dmControlSource->prtErrorMsg();
    setError("IviewControl::setDevices() - writeRequest() failed");
    return ret;
  }

  return requestStatus(_aperiodicRequest.requestNo, timeoutTicks);
}

  
int IviewControl::getDevices(IviewDeviceMask *deviceMask,
			     IviewDevice *refDevice)
{
  int ret;
  
  if ((ret = _dmControlSource->readMessage(&_message)) != IviewSuccess)
  {
    _dmControlSource->prtErrorMsg();
    setError("IviewControl::getDevice() - readMessage() failed");
    return ret;
  }
  *deviceMask = _message.deviceMask;

  // Note: Iview does NOT echo the reference device from the request!!! 
  if ((ret = _dmControlSource->readStatus(&_status)) != IviewSuccess)
  {
    _dmControlSource->prtErrorMsg();
    setError("IviewControl::getDevice() - readStatus() failed");
    return ret;
  }

  *refDevice = _status.refDevice;
  
  return IviewSuccess;
}


int IviewControl::setSensitivity(IviewSensitivity sensitivity,
				 int timeoutTicks)
{
  _aperiodicRequest.sensitivity = sensitivity;
  _aperiodicRequest.requestNo = _requestNo++;

  int ret;

  if ((ret = _dmControlSource->writeRequest(&_aperiodicRequest)) 
      != IviewSuccess)
  {
    _dmControlSource->prtErrorMsg();
    setError("IviewControl::setSensitivity() - writeRequest() failed");
    return ret;
  }

  return requestStatus(_aperiodicRequest.requestNo, timeoutTicks);
}


int IviewControl::setCoords(float pan, float tilt, float focus, 
			    float zoom, float iris,
			    int timeoutTicks)
{
  _periodicRequest.pan = pan;
  _periodicRequest.tilt = tilt;
  _periodicRequest.focus = focus;
  _periodicRequest.zoom = zoom;
  _periodicRequest.iris = iris;
  
  _periodicRequest.requestNo = _requestNo++;

  int ret;
  
  if ((ret = _dmControlSource->writeRequest(&_periodicRequest)) 
      != IviewSuccess)
  {
    _dmControlSource->prtErrorMsg();
    setError("IviewControl::setCoords() - writeRequest() failed");
    return ret;
  }  

  return requestStatus(_periodicRequest.requestNo, timeoutTicks);
}


int IviewControl::getMsg(IviewMessage *message)
{
  int ret;
  
  if ((ret = _dmControlSource->readMessage(&_message)) != IviewSuccess)
  {
    _dmControlSource->prtErrorMsg();
    setError("getMsg() - readMessage() failed");
    return ret;
  }

  // Update request to reflect current state
  _aperiodicRequest.deviceMask = _message.deviceMask;
  _aperiodicRequest.panoramic = _message.panoramic;
  
  message->id = _message.id;
  message->deviceMask = _message.deviceMask;
  message->panoramic = _message.panoramic;
  message->focusCaptured = _message.focusCaptured;
  message->serverStarting = _message.serverStarting;
  
  return IviewSuccess;
}


int IviewControl::start()
{
  Errno err;
  char errorBuf[errorMsgSize];
  
  // This task may not be provider of this item, so open it urgently
  dm_urgent_open(_dmControlSource->periodicRequestItem(), DM_STATIC);
  
  setCoords(0., 0., 0., 0., 0.);
  
  dm_urgent_close(_dmControlSource->periodicRequestItem());
  
  // This task may not be provider of this item, so open it urgently
  dm_urgent_open(_dmControlSource->aperiodicRequestItem(), DM_STATIC);

  setDevices(0, IviewPortCamera);

  dm_urgent_close(_dmControlSource->aperiodicRequestItem());

  _started = TRUE;
  return IviewSuccess;
}


DM_Item IviewControl::aperiodicRequestItem()
{
  return _dmControlSource->aperiodicRequestItem();
}


DM_Item IviewControl::periodicRequestItem()
{
  return _dmControlSource->periodicRequestItem();
}


DM_Item IviewControl::statusItem()
{
  return _dmControlSource->statusItem();
}

DM_Item IviewControl::msgItem()
{
  return _dmControlSource->messageItem();
}



int IviewControl::requestStatus(int requestNo, int timeoutTicks)
{
  MBool debug = FALSE;
  
  if (timeoutTicks == NoWait)
    return IviewTimedOut;
  
  MBool timedOut = FALSE;

  while (!timedOut)
  {
    if (semTake(_statusSem, timeoutTicks) == ERROR)
      timedOut = TRUE;

    if (_dmControlSource->readStatus(&_status) != IviewSuccess)
      return IviewDmError;
    else
    {
      if (_status.requestNo < requestNo)
      {
	dprintf(stderr, "IviewControl::requestStatus(); Waiting for request "
		"%d, got %d\n",
		requestNo, _status.requestNo);
	
	// Status from an earlier request
	continue;
      }	
      if (_status.requestNo == requestNo)
      {
	dprintf(stderr, "IviewControl::requestStatus() - code = %d\n", 
		_status.code);
	  
	return _status.code;
      }
      else 
	// Somehow missed status?
	return IviewMissedStatus;
    }

  }

  return IviewTimedOut;
}



IviewAxisMask IviewControl::axisMask()
{
  return _axisMask;
}


int IviewControl::initialize()
{
  return start();
}


int IviewControl::grabControlSource()
{
  char errorBuf[errorMsgSize];
  Errno err;
  
  if ((err = dm_start_provider(_dmControlSource->aperiodicRequestItem(), 
			       DM_STATIC)) != SUCCESS)
  {
    sprintDmError(err, "grabControlSource(), aperiodicRequestItem", errorBuf);
    setError(errorBuf);
    return IviewDmError;
  }

  return IviewSuccess;
}


int IviewControl::startPeriodic()
{
  char buf[100];
  char errorBuf[errorMsgSize];
  Errno err;

  if ((err = dm_start_provider(_dmControlSource->periodicRequestItem(), 
			       IviewPeriod)) != SUCCESS)
  {
    sprintf(buf, "startPeriodic(), _periodicRequestItem for source #%d",
	    id());

    sprintDmError(err, buf, errorBuf);
    setError(errorBuf);
    return IviewDmError;
  }

  return IviewSuccess;
}


int IviewControl::setPanoramic(MBool on, int timeoutTicks)
{
  unsigned short deviceMask;
  
  if (on)
  {
    _aperiodicRequest.panoramic = TRUE;

    deviceMask = IviewPortCamera & IviewPortLight & 
      IviewStbdCamera & IviewStbdLight;
  }
  else
  {
    _aperiodicRequest.panoramic = FALSE;

    deviceMask = _aperiodicRequest.deviceMask;
  }
  
  return setDevices(deviceMask, _aperiodicRequest.refDevice, timeoutTicks);
}