#include "CameraInterface.h"
#include "DmErrno.h"

#ifndef UNIX
extern "C" {
# include "usrTime.h"
};
#endif

#define dprintf if (debug) fprintf

#define ZoomPositionModeValue 0
#define ZoomRateModeValue 1


CameraInterface::CameraInterface(CameraZoomMode zoomMode, const char *dmPrefix)
{
  _zoomMode = zoomMode;
  _dmItems = new CameraInterfaceDm(zoomMode, dmPrefix);

  // Provide at 20 hz
  _providePeriod = 50000;
  _consumePeriod = 50000;
  _iHaveIt = FALSE;

  Errno err;
  char errorBuf[256];

  // Start consuming items
  if ((err = _dmItems->zoomMode->startConsume(DM_STATIC)) 
      != SUCCESS) {

    sprintDmError(err, "CameraInterface::CameraInterface() - zoomMode",
		  errorBuf);

    fprintf(stderr, "%s\n", errorBuf);
  }

  if ((err = _dmItems->zoomSetpoint->startConsume(_consumePeriod)) 
      != SUCCESS) {

    sprintDmError(err, "CameraInterface::CameraInterface() - zoomSetpoint",
		  errorBuf);

    fprintf(stderr, "%s\n", errorBuf);
  }

  if ((err = _dmItems->focusSetpoint->startConsume(_consumePeriod)) 
      != SUCCESS) {

    sprintDmError(err, "CameraInterface::CameraInterface() - focusSetpoint",
		  errorBuf);

    fprintf(stderr, "%s\n", errorBuf);

  }

  if ((err = _dmItems->irisSetpoint->startConsume(_consumePeriod)) 
      != SUCCESS) {

    sprintDmError(err, "CameraInterface::CameraInterface() - irisSetpoint",
		  errorBuf);

    fprintf(stderr, "%s\n", errorBuf);

  }

  if ((err = _dmItems->zoom->startConsume(_consumePeriod)) != SUCCESS) {

    sprintDmError(err, "CameraInterface::CameraInterface() - zoom",
		  errorBuf);

    fprintf(stderr, "%s\n", errorBuf);

  }

  if ((err = _dmItems->focus->startConsume(_consumePeriod)) != SUCCESS) {

    sprintDmError(err, "CameraInterface::CameraInterface() - focus",
		  errorBuf);

    fprintf(stderr, "%s\n", errorBuf);
  }

  if ((err = _dmItems->iris->startConsume(_consumePeriod)) != SUCCESS) {

    sprintDmError(err, "CameraInterface::CameraInterface() - iris",
		  errorBuf);

    fprintf(stderr, "%s\n", errorBuf);
  }

  if ((err = _dmItems->fieldWidth->startConsume(_consumePeriod)) != SUCCESS) {

    sprintDmError(err, "CameraInterface::CameraInterface() - fieldWidth",
		  errorBuf);

    fprintf(stderr, "%s\n", errorBuf);
  }

  if ((err = _dmItems->focalDistance->startConsume(_consumePeriod)) 
      != SUCCESS) {

    sprintDmError(err, "CameraInterface::CameraInterface() - focalDistance",
		  errorBuf);

    fprintf(stderr, "%s\n", errorBuf);
  }

  if ((err = _dmItems->zoomAngle->startConsume(_consumePeriod)) 
      != SUCCESS) {

    sprintDmError(err, "CameraInterface::CameraInterface() - zoomAngle",
		  errorBuf);

    fprintf(stderr, "%s\n", errorBuf);
  }

}


CameraInterface::~CameraInterface()
{
  delete _dmItems;
}


int CameraInterface::grab()
{
  MBool debug = TRUE;

  if (_iHaveIt)
    // Already have it
    return 0;

  char errorBuf[256];
  Period period = _providePeriod;
  MBool gotError = FALSE;
  Errno err;

  if ((err = _dmItems->zoomMode->startProvide(period, ExclusiveProvider))
      != SUCCESS) {
    sprintDmError(err, "CameraInterface::grab(), zoomMode", errorBuf);
    dprintf(stderr, "%s\n", errorBuf);
    gotError = TRUE;
  }

  if (!gotError &&
      (err = _dmItems->zoomSetpoint->startProvide(period, ExclusiveProvider))
      != SUCCESS) {
    sprintDmError(err, "CameraInterface::grab(), zoomSetpoint", errorBuf);
    dprintf(stderr, "%s\n", errorBuf);
    gotError = TRUE;
  }

  if (!gotError && 
      (err = _dmItems->focusSetpoint->startProvide(period, ExclusiveProvider))
      != SUCCESS) {
    sprintDmError(err, "CameraInterface::grab(), focusSetpoint", errorBuf);
    dprintf(stderr, "%s\n", errorBuf);
    gotError = TRUE;
  }

  if (!gotError && 
      (err = _dmItems->irisSetpoint->startProvide(period, ExclusiveProvider))
      != SUCCESS) {
    sprintDmError(err, "CameraInterface::grab(), irisSetpoint", errorBuf);
    dprintf(stderr, "%s\n", errorBuf);
    gotError = TRUE;
  }

  if (!gotError) {

    DM_Time t;
    gettimeofday(&t, 0);

    Int16 mode;
    // Set zoom mode
    if (_zoomMode == ZoomPosition)
      mode = ZoomPositionModeValue;
    else
      mode = ZoomRateModeValue;

    if ((err = _dmItems->zoomMode->write(mode, &t)) != SUCCESS) {
      sprintDmError(err, "CameraInterface::grab(), write zoomMode", errorBuf);
      dprintf(stderr, "%s\n", errorBuf);
      gotError = TRUE;
    }
  }

  if (gotError) {
    release();
    return -1;
  }

  _iHaveIt = TRUE;
  return 0;
}



int CameraInterface::release()
{
  _dmItems->zoomMode->stopProvide();
  _dmItems->zoomSetpoint->stopProvide();
  _dmItems->focusSetpoint->stopProvide();
  _dmItems->irisSetpoint->stopProvide();
  _iHaveIt = FALSE;
  return 0;
}


MBool CameraInterface::inUse()
{
  DmPrvdrType provider = dmPrvdrStatus(_dmItems->irisSetpoint->getHandle());

  if (provider == NOT_PRVDR || _iHaveIt)
    // Either we have it or nobody does
    return FALSE;
  else
    // Somebody else has it
    return TRUE;
}


int CameraInterface::setZoom(Int16 value)
{
  if (!_iHaveIt)
    return -1;

  DM_Time t;
  gettimeofday(&t, 0);

  Errno err;

  if ((err = _dmItems->zoomSetpoint->write(value, &t)) != SUCCESS) {
    char errorBuf[256];
    sprintDmError(err, "CameraInterface::setZoom()", errorBuf);
    fprintf(stderr, "%s\n", errorBuf);
    return -1;
  }
  else
    return 0;
}


int CameraInterface::setFocus(Int16 value)
{
  if (!_iHaveIt)
    return -1;

  DM_Time t;
  gettimeofday(&t, 0);

  Errno err;
  if ((err = _dmItems->focusSetpoint->write(value, &t)) != SUCCESS) {
    char errorBuf[256];
    sprintDmError(err, "CameraInterface::setFocus()", errorBuf);
    fprintf(stderr, "%s\n", errorBuf);
    return -1;
  }
  else
    return 0;
}


int CameraInterface::setIris(Int16 value)
{
  if (!_iHaveIt)
    return -1;

  DM_Time t;
  gettimeofday(&t, 0);

  Errno err;
  if ((err = _dmItems->irisSetpoint->write(value, &t)) != SUCCESS) {
    char errorBuf[256];
    sprintDmError(err, "CameraInterface::setIris()", errorBuf);
    fprintf(stderr, "%s\n", errorBuf);
    return -1;
  }
  else
    return 0;
}


int CameraInterface::zoomSetpoint(Int16 *value)
{
  DM_Time t;
  Errno err;

  if ((err = _dmItems->zoomSetpoint->read(value, &t)) != SUCCESS)
    return -1;
  else
    return 0;
}


int CameraInterface::zoomMode(CameraZoomMode *mode)
{
  Int16 value;
  DM_Time t;
  Errno err;

  if ((err = _dmItems->zoomMode->read(&value, &t)) != SUCCESS)
    return -1;

  if (value == ZoomPositionModeValue)
    *mode = ZoomPosition;
  else
    *mode = ZoomRate;

  return 0;
}


int CameraInterface::zoom(Int16 *value)
{
  DM_Time t;
  Errno err;

  if ((err = _dmItems->zoom->read(value, &t)) != SUCCESS)
    return -1;
  else
    return 0;
}


int CameraInterface::focusSetpoint(Int16 *value)
{
  DM_Time t;
  Errno err;

  if ((err = _dmItems->focusSetpoint->read(value, &t)) != SUCCESS)
    return -1;
  else
    return 0;
}


int CameraInterface::focus(Int16 *value)
{
  DM_Time t;
  Errno err;

  if ((err = _dmItems->focus->read(value, &t)) != SUCCESS)
    return -1;
  else
    return 0;
}


int CameraInterface::irisSetpoint(Int16 *value)
{
  DM_Time t;
  Errno err;

  if ((err = _dmItems->irisSetpoint->read(value, &t)) != SUCCESS)
    return -1;
  else
    return 0;
}


int CameraInterface::iris(Int16 *value)
{
  DM_Time t;
  Errno err;

  if ((err = _dmItems->iris->read(value, &t)) != SUCCESS)
    return -1;
  else
    return 0;
}


int CameraInterface::fieldWidth(Flt64 *value)
{
  DM_Time t;
  Errno err;

  if ((err = _dmItems->fieldWidth->read(value, &t)) != SUCCESS)
    return -1;
  else
    return 0;
}


int CameraInterface::focalDistance(Flt64 *value)
{
  DM_Time t;
  Errno err;

  if ((err = _dmItems->focalDistance->read(value, &t)) != SUCCESS)
    return -1;
  else
    return 0;
}


int CameraInterface::zoomAngle(Flt64 *value)
{
  DM_Time t;
  Errno err;

  if ((err = _dmItems->zoomAngle->read(value, &t)) != SUCCESS)
    return -1;
  else
    return 0;
}


CameraInterfaceDm *CameraInterface::dmItems()
{
  return _dmItems;
}