// Lidar Drive: select path towards max. freespace
// T. Braunl, May 2021
#include "eyebot.h"
#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* angle)
{ int i, pos=0;
  for (i=1; i<POINTS; i++)
    if (a[i] > a[pos]) pos = i;
  *angle = pos-90; // range -90 .. +90 (left is neg.)
  return a[pos];
}


int main ()
{ int   i, k, m, angle;
  int   scan[POINTS];
  float scale;
  BYTE  img[QQVGA_SIZE];

  LCDPrintf("Lidar Drive\n");
  LCDMenu("", "", "", "END");
  CAMInit(QQVGA);
  LIDARSet(180, 0, POINTS); // range, tilt, points

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

    LIDARGet(scan);
    m = getmax(scan,&angle);
    if (angle < -15)     VWSetSpeed(30, 10); // turn left
    else if (angle > 15) VWSetSpeed(30,-10); // turn right 
      else               VWSetSpeed(30,  0); // drive straight

    // display lidarvalues 
    scale = m/150.0;
    LCDSetPos(13,0);
    LCDPrintf("max %d angle %df\n", m,angle);
    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);
}
