#include <stdio.h>
#include <pmdsdk2.h>

#define SOURCE_PLUGIN "camcube"
#define SOURCE_PARAM ""
#define PROC_PLUGIN "camcubeproc"
#define PROC_PARAM ""

char err[128];

int main (int argc, char *argv[])
{
  PMDHandle hnd;
  int res;

  res = pmdOpen (&hnd, SOURCE_PLUGIN, SOURCE_PARAM, PROC_PLUGIN, PROC_PARAM);
  if (res != PMD_OK)
    {
      pmdGetLastError (0, err, 128);
      fprintf (stderr, "Could not connect: %s\n", err);
      return 1;
    }

  res = pmdUpdate (hnd);
  if (res != PMD_OK)
    {
      pmdGetLastError (hnd, err, 128);
      fprintf (stderr, "Could transfer data: %s\n", err);
      pmdClose (hnd);
      return 1;
    }

  PMDDataDescription dd;

  res = pmdGetSourceDataDescription (hnd, &dd);
  if (res != PMD_OK)
    {
      pmdGetLastError (hnd, err, 128);
      fprintf (stderr, "Could get data description: %s\n", err);
      pmdClose (hnd);
      return 1;
    }

  if (dd.subHeaderType != PMD_IMAGE_DATA)
    {
      fprintf (stderr, "Source data is not an image!\n");
      pmdClose (hnd);
      return 1;
    }

  float * dist = new float [dd.img.numRows * dd.img.numColumns];

  res = pmdGetDistances (hnd, dist, dd.img.numColumns * dd.img.numRows * sizeof (float));
  if (res != PMD_OK)
    {
      pmdGetLastError (hnd, err, 128);
      fprintf (stderr, "Could get distances: %s\n", err);
      pmdClose (hnd);
      return 1;
    }

  printf ("Middle distance: %f m\n", dist[(dd.img.numRows / 2) * dd.img.numColumns + dd.img.numColumns / 2]);

  pmdClose (hnd);

  return 0;
}


