#include <stdio.h>
#include <values.h>
#include "SuperHarpDmIF.h"
#include "CameraSerialIF.h"
#include "DmErrno.h"

/* Periodic task */
int main(int argc, char **argv)
{
  // Initialize Data Manager
  Errno ret;
  char errorBuf[100];
  
  if ((ret = dm_init(NULL)) != OK)
  {
    sprintDmError(ret, "dm_init()", errorBuf);
    fprintf(stderr, "%s\n", errorBuf);
    exit(1);
  }

  /* Create datamanager interface (pass NULL sem id, since this task
     is periodic and doesn't use semaphore). */
  SuperHarpServerDmIF *dmInterface = new SuperHarpServerDmIF(NULL);

  // Create serial interface
  CameraSerialIF *serialInterface = new CameraSerialIF(); // HYPOTHETICAL!

  // Setup structure for loop timing
  struct timeval timeout;
  timeout.tv_sec = 0;
  timeout.tv_usec = CameraPeriod * 1000;
  
  Int16 zoom;
  Int16 focus;
  Int16 iris;

  Int16 prevZoom = MAXSHORT;
  Int16 prevFocus = MAXSHORT;
  Int16 prevIris = MAXSHORT;
  
  /* Top of main control loop. For now, we assume no zoom, focus, or iris 
     feedback from camera itself, so give instantaneous feedback if write
     to serial interface succeeds. */

  while (TRUE)
  {
    DM_Time t;
    
    dmInterface->readZoomRequest(&zoom, &t);
    dmInterface->readFocusRequest(&focus, &t);
    dmInterface->readIrisRequest(&iris, &t);
    
    if (zoom != prevZoom)
    {
      // Write zoom request to camera
      if (serialInterface->writeZoom(zoom) != -1)
	dmInterface->writeZoomStatus(zoom);

      prevZoom = zoom;
    }

    if (focus != prevFocus)
    {
      // Write focus request to camera
      if (serialInterface->writeFocus(focus) != -1)
	dmInterface->writeFocusStatus(focus);

      prevFocus = focus;
    }
    
    if (iris != prevIris)
    {
      // Write iris request to camera
      if (serialInterface->writeIris(iris) != -1)
	dmInterface->writeIrisStatus(iris);

      prevIris = iris;
    }

    // Wait specified time
    select(0, 0, 0, 0, &timeout);
  }
}