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

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 a semaphore for aperiodic datamanager objects
  SEM_ID wakeupSem;
  
  if ((wakeupSem = semBCreate(SEM_Q_FIFO, SEM_EMPTY)) == SEM_NULL)
  {
    fprintf(stderr, "Couldn't create _monitorSem\n");
    exit(1);
  }

  // Create datamanager interface
  SuperHarpServerDmIF *dmInterface = new SuperHarpServerDmIF(wakeupSem);  

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

  CameraGain gain;
  MBool colorBars;

  DM_Time t;
  
  while (TRUE)
  {
    // Wait on semaphore for DM item changes
    semTake(wakeupSem, WAIT_FOREVER);

    if (dmInterface->newGainRequest())
    {
      dmInterface->readGainRequest(&gain, &t);
      
      // Write gain request to camera
      if (serialInterface->writeGain(gain) != -1)
        dmInterface->writeGainStatus(gain);
    }

    if (dmInterface->newColorBarsRequest())
    {
      dmInterface->readColorBarsRequest(&colorBars, &t);
      
      // Write colorbars request to camera
      if (serialInterface->writeColorBars(colorBars) != -1)
        dmInterface->writeColorBarsStatus(colorBars);
    }
  }
}