#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/time.h>
#include "IviewTest.h"

#define dprintf if (debug) fprintf

ControlSource::ControlSource(int i, DeviceRequestQueues *queues)
{
  // Enable/disable relinquish feature
  _relinquishEnabled = TRUE;
  
  _dmControlSource = new DmControlSource(i);
  _dmControlSource->setServerRole();
  _queues = queues;
  _index = i;
  _requestId = -1;
  reset();
  struct timezone tzone;
  tzone.tz_minuteswest = 0;
  tzone.tz_dsttime = 0;
  DM_Time t;
  gettimeofday(&t, &tzone);
  _requestTime = t.tv_sec + t.tv_usec / 1e6;
  _inUse = FALSE;
}


ControlSource::~ControlSource()
{
  delete _dmControlSource;
}


void ControlSource::reset()
{
  _update = FALSE;
  _msg.id = -1;
  _requestDevices = _actualDevices = 0;
}


void ControlSource::grab(IviewDevice device)
{
  _actualDevices |= device;
  _update = TRUE;
}


void ControlSource::relinquish(IviewDevice device)
{
  _actualDevices &= ~device;
  _update = TRUE;
}


int ControlSource::dispatchUpdate()
{
  MBool debug = TRUE;
  
  if (!_update)
    return IviewSuccess;
  
  // Send message to client
  _msg.id++;
  _msg.deviceMask = _actualDevices;
  _msg.refDevice = IviewNoDevice;
  _msg.code = IviewDeviceListModified;
  sprintf(_msg.text, "devices modified");
  int status;
  dprintf(stderr, "dispatch update message #%d for source %d\n", 
	  _msg.id, _index);

  if ((status = _dmControlSource->writeMessage(&_msg)) != IviewSuccess)
  {
    fprintf(stderr, "dispatchUpdate() - error\n");
    _dmControlSource->prtErrorMsg();
  }
  
  _update = FALSE;
  return status;
}


int ControlSource::processRequest()
{
  MBool debug = TRUE;
  
  MBool inUse;
  _dmControlSource->hasClient(&inUse);

  if (!inUse)
  {
    // No client right now
    if (_inUse)
    {
      // No longer in use
      reset();
      _requestId = -1;
      _inUse = FALSE;
      dprintf(stderr, "Client finished\n");
    }
    return IviewSuccess;
  }
  else
  {
    // Has a client now
    if (!_inUse)
    {
      _inUse = TRUE;
      dprintf(stderr, "Got a new client\n");  
    }
  }  

  int ret;
  if ((ret = _dmControlSource->readRequest(&_request)) != IviewSuccess)
  {
    dprintf(stderr, "readRequestFailed\n");
    return ret;
  }
  
  double requestTime = _dmControlSource->requestTime();

/* ***
  dprintf(stderr, "src %d: requestTime=%.3f, _requestTime=%.3f\n", 
	  _index, requestTime, _requestTime);
*** */
  
  if (requestTime <= _requestTime)
    return IviewSuccess;

  _requestTime = requestTime;

  dprintf(stderr, 
	  "processRequest() - New _request.id for source %d = %d, name=\"%s\"\n", 
	  _index, _request.id, _request.controlName);

  _requestId = _request.requestNo;
  
  DmControlSource::Status status;
  unsigned short relinquishedDevices;
  unsigned short newlyRequestedDevices;
  
  switch (_request.type)
  {
    case IviewStartRequest:
    reset();
    status.requestId = _request.id;
    status.code = IviewSuccess;
    status.deviceMask = 0;
    status.refDevice = IviewNoDevice;
    
    if ((ret = _dmControlSource->writeStatus(&status)) != IviewSuccess)
      return ret;

    _msg.deviceMask = 0;
    _msg.refDevice = IviewNoDevice;
    _msg.id++;
    _msg.code = IviewClientStarted;
    sprintf(_msg.text, "client started");
    dprintf(stderr, "write start msg (#%d) for source %d, request %d\n", 
	    _msg.id, _index, _request.id);

    ret = _dmControlSource->writeMessage(&_msg);

    return ret;
    
    break;
    
    case IviewFinishRequest:
    dprintf(stderr, "Got finish request\n");

    if (_relinquishEnabled)
    {
      // Relinquish devices held by control 
      if (_requestDevices & IviewPortLight)
	_queues->relinquish(this, IviewPortLight);

      if (_requestDevices & IviewPortCamera)
	_queues->relinquish(this, IviewPortCamera);

      if (_requestDevices & IviewStbdLight)
	_queues->relinquish(this, IviewStbdLight);

      if (_requestDevices & IviewStbdCamera)
	_queues->relinquish(this, IviewStbdCamera);
    }
    
    _requestId = -1;
    reset();
    return IviewSuccess;
    break;
    
    case IviewRateRequest:
    status.requestId = _request.id;
    status.code = IviewSuccess;
    status.deviceMask = _request.deviceMask;
    status.refDevice = _request.refDevice;
    
    if ((ret = _dmControlSource->writeStatus(&status)) != IviewSuccess)
      return ret;
    
    dprintf(stderr, "Got rate request:\n");
    dprintf(stderr, 
	    "Pan: %.3f  Tilt: %.3f  Focus: %.3f  Zoom: %.3f  Iris: %.3f\n",
	    _request.pan, _request.tilt, _request.focus, 
	    _request.zoom, _request.iris);
    
    break;
    
    case IviewPositionRequest:
    status.requestId = _request.id;
    status.code = IviewSuccess;
    status.deviceMask = _request.deviceMask;
    status.refDevice = _request.refDevice;

    if ((ret = _dmControlSource->writeStatus(&status)) != IviewSuccess)
      return ret;
    
    dprintf(stderr, "Got position request:\n");
    dprintf(stderr, 
	    "Pan: %.3f  Tilt: %.3f  Focus: %.3f  Zoom: %.3f  Iris: %.3f\n",
	    _request.pan, _request.tilt, _request.focus, 
	    _request.zoom, _request.iris);
    
    break;
    
    case IviewOffsetRequest:
    // Do nothing for now
    return IviewSuccess;  
    
    case IviewDeviceRequest:
    status.requestId = _request.id;
    status.code = IviewSuccess;
    status.deviceMask = _request.deviceMask;
    
    status.refDevice = _request.refDevice;
    
    if ((ret = _dmControlSource->writeStatus(&status)) != IviewSuccess)
      return ret;
    
    dprintf(stderr, "Requested device mask: %d\n", _request.deviceMask);

    if (_relinquishEnabled)
    {
      relinquishedDevices = 
	_requestDevices & ~_request.deviceMask;

//      newlyRequestedDevices = 
//	_request.deviceMask & ~_requestDevices;

      newlyRequestedDevices = 
	_request.deviceMask & ~_actualDevices;
    
      _requestDevices = _request.deviceMask;

      if (newlyRequestedDevices & IviewPortLight)
	_queues->grab(this, IviewPortLight);

      if (newlyRequestedDevices & IviewPortCamera)
	_queues->grab(this, IviewPortCamera);

      if (newlyRequestedDevices & IviewStbdLight)
	_queues->grab(this, IviewStbdLight);

      if (newlyRequestedDevices & IviewStbdCamera)
	_queues->grab(this, IviewStbdCamera);


      if (relinquishedDevices & IviewPortLight)
	_queues->relinquish(this, IviewPortLight);

      if (relinquishedDevices & IviewPortCamera)
	_queues->relinquish(this, IviewPortCamera);

      if (relinquishedDevices & IviewStbdLight)
	_queues->relinquish(this, IviewStbdLight);

      if (relinquishedDevices & IviewStbdCamera)
	_queues->relinquish(this, IviewStbdCamera);
    }
    else
    {
      _actualDevices = _request.deviceMask;
      _update = TRUE;
    }
    
    dprintf(stderr, "Actual device mask: %d\n", _actualDevices);
  }

  return IviewSuccess;
}


void ControlSource::printRequest()
{
  switch (_request.type)
  {
    case IviewStartRequest:
    printf("'Start' "); 
    break;
    
    case IviewDeviceRequest:
    printf("'Device' "); 
    break;
    
    case IviewRateRequest:
    printf("'Rate' "); 
    break;
    
    case IviewPositionRequest:
    printf("'Position' "); 
    break;

    case IviewFinishRequest:
    printf("'Finish' ");
    break;
    
    default:
    printf("Unknown "); 
  }
  
  printf("request from host '%d', pid %d, controlName %s\n", 
	 _request.hostAddr, _request.pid, _request.controlName);

  printf("id = %d\n", _request.id);
  printf("axisMask = %d, deviceMask = %d\n", 
	 _request.axisMask, _request.deviceMask);

  printf("refFrame = %d  pan = %.3f  tilt = %.3f  focus = %.3f\n",
	 _request.refDevice, _request.pan, _request.tilt, _request.focus);

  printf("\n");
}



DeviceRequestQueues::DeviceRequestQueues() :
  portLightSources(_portLightSourceList),
  portCameraSources(_portCameraSourceList),
  stbdLightSources(_stbdLightSourceList),
  stbdCameraSources(_stbdCameraSourceList)
{
}


void DeviceRequestQueues::grab(ControlSource *source, IviewDevice device)
{
  SListIter<ControlSource *> *sources;
  SList<ControlSource *> *list;
  
  switch (device)
  {
    case IviewPortLight:
    sources = &portLightSources;
    list = &_portLightSourceList;
    break;

    case IviewPortCamera:
    sources = &portCameraSources;
    list = &_portCameraSourceList;
    break;

    case IviewStbdLight:
    sources = &stbdLightSources;
    list = &_stbdLightSourceList;
    break;

    case IviewStbdCamera:
    sources = &stbdCameraSources;
    list = &_stbdCameraSourceList;
    break;

    case IviewNoDevice:
    return;
  }
  
  ControlSource **src;
  sources->reset();
  int nDeleted = 0;
  
  for (int index = 0; src = sources->next(); index++)
  {
    int len = list->length();  // For debug
    
    if (*src == source)
    {
      // remove earlier instance of this source
      sources->remove();
      nDeleted++;
    } 
    else if ((index + 1) - nDeleted == list->length())
      // last source in list loses device
      (*src)->relinquish(device);
  }
  source->grab(device);
  list->append(source);
}


void DeviceRequestQueues::relinquish(ControlSource *source,
				     IviewDevice device)
{
  SListIter<ControlSource *> *sources;
  SList<ControlSource *> *list;
  
  switch (device)
  {
    case IviewPortLight:
    sources = &portLightSources;
    list = &_portLightSourceList;
    break;

    case IviewPortCamera:
    sources = &portCameraSources;
    list = &_portCameraSourceList;
    break;

    case IviewStbdLight:
    sources = &stbdLightSources;
    list = &_stbdLightSourceList;
    break;

    case IviewStbdCamera:
    sources = &stbdCameraSources;
    list = &_stbdCameraSourceList;
    break;

    case IviewNoDevice:
    return;
  }

  ControlSource **src, **prevSrc = NULL;
  sources->reset();

  for (int index = 0; src = sources->next(); index++)
  {
    if (*src == source)
    {
      if ((index + 1) == list->length())
      { 
        // Source is last in list, grant device to previous source
	if (prevSrc)  
	  (*prevSrc)->grab(device);
      }
      (*src)->relinquish(device);
      
      // Source no longer interested in device - remove it from list
      sources->remove();
    }
    prevSrc = src;
  }
}


IviewTest::IviewTest()
{
  _queues = new DeviceRequestQueues();
  
  for (int i = 0; i < MaxControlSources; i++)
  {
    _source[i] = new ControlSource(i, _queues);
  }
}


int IviewTest::processControlSources()
{
  int i;
  
  for (i = 0; i < MaxControlSources; i++)
  {
    if (_source[i]->processRequest() != IviewSuccess)
    {
      fprintf(stderr, 
	      "IviewTest::processControlSources() - "
	      "error from processRequest()\n");
    }
  }

  for (i = 0; i < MaxControlSources; i++)
  {
    if (_source[i]->dispatchUpdate() != IviewSuccess)
    {
      fprintf(stderr, 
	      "IviewTest::processControlSources() - "
	      "error from dispatchUpdate()\n");
    }
  }

  return 0;
}



main(int argc, char **argv)
{
  MBool debug = TRUE;
  
  dm_init(NULL);
  
  IviewTest *iview = new IviewTest();
  struct timeval timeout;
  timeout.tv_sec = 0;
  timeout.tv_usec = 10000;
  
  while (TRUE)
  {
    iview->processControlSources();
    select(0, 0, 0, 0, &timeout);
  }

  exit(0);
}