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

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

char err[128];
PMDDataDescription dd;
char * sourceData;

// Open a source plugin, get data and close
void getData ();
// Open a processing plugin, calculate and display distances from source data and close
void calcDistance ();

int main (int argc, char *argv[])
{
  getData ();
  calcDistance ();

  delete[] sourceData;

  return 0;
}


void getData ()
{
  PMDHandle hnd;
  int res;

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

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

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

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

  sourceData = new char[dd.size];
  res = pmdGetSourceData (hnd, sourceData, dd.size);
  if (res != PMD_OK)
    {
      pmdGetLastError (hnd, err, 128);
      fprintf (stderr, "Could get source data: %s\n", err);
      pmdClose (hnd);
      exit (1);
    }

  pmdClose (hnd);
}

void calcDistance ()
{
  PMDHandle hnd;
  int res;

  res = pmdOpenProcessingPlugin (&hnd, PROC_PLUGIN, PROC_PARAM);
  if (res != PMD_OK)
    {
      pmdGetLastError (0, err, 128);
      fprintf (stderr, "Could not open processing plugin: %s\n", err);
      exit (1);
    }

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

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

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

  delete[] dist;

  pmdClose (hnd);
}

