// Lidar Example, T. Braunl 2018
#include "eyebot.h"
#include <stdio.h>
#define POINTS 180
#define PLOT   180
#define SCL    (POINTS/PLOT)
#define CONES  4
#define SPEED  1000

int getmax(int a[])
{ int i, pos = 0;
  for (i=1; i<POINTS; i++)
    if (a[i] > a[pos]) pos = i;
  return a[pos];
}


int main ()
{ int   i, k, m;
  int   scan[POINTS];
  float scale;
  BYTE  img[QQVGA_SIZE];
  //int   pos[CONES], dist[CONES];

  LCDPrintf("Formula SAE Cone-Testing\n");
  LCDMenu("", "", "", "END");
  CAMInit(QQVGA);
  LIDARSet(180, 0, POINTS); // range, tilt, points

  do
  { k = KEYRead();
    CAMGet(img);
    LCDImage(img);

    LIDARGet(scan);
    m = getmax(scan);
    scale = m/150.0;
    LCDSetPos(13,0);
    LCDPrintf("max %d scale %3.1f\n", m,scale);
    for (i=0; i<10; i++) LCDPrintf("%4d ",scan[i*(POINTS/10)]);
    LCDPrintf("\n");

    // plot distances
    for (i=0; i<PLOT; i++)
    { LCDLine(180+i,150-scan[SCL*i]/scale, 180+i,150, BLUE);
      LCDLine(180+i,150-scan[SCL*i]/scale, 180+i,  0, BLACK);   // remove any previous line
    }
    LCDLine(180+POINTS/(2*SCL),0, 180+POINTS/(2*SCL),150, RED); // straight (0°)
  } while (k!=KEY4);
}
