// Lidar Example, T. Braunl 2022
#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 getmin(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, n;
  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
  MOTORDrive(1, 15);  // start driving

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

    LIDARGet(scan);
    m = getmax(scan);
    n = getmin(scan);
    if (n<1000) MOTORDrive(1, 0);  // stop to avoid collision
    scale = m/150.0;
    LCDSetPos(14,0);
    LCDPrintf("min %4d \n", n);
    LCDPrintf("max %4d \n", m);
    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 previous line
    }
    LCDLine(180+POINTS/(2*SCL),0, 180+POINTS/(2*SCL),150, RED); // straight (0°)
  } while (k!=KEY4);
}
