// 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  5000

int getleftcone(int a[], int mid)
{ int i;
  for (i=mid-1; i>20; i--)
    if (a[i] < 9000) return i; // cone detected !
  return -1; // no cone
}

int getrightcone(int a[], int mid)
{ int i;
  for (i=mid+1; i<160; i++)
    if (a[i] < 9000) return i; // cone detected !
  return -1; // no cone
}

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, l,r,middle=90, drive=0, dir=0;
  int   scan[POINTS];
  float scale;
  BYTE  img[QQVGA_SIZE];

  LCDMenu("DRIVE", "STOP", "", "END");
  CAMInit(QQVGA);
  LIDARSet(180, 0, POINTS); // range, tilt, points

  do
  { k = KEYRead();
    if (k==KEY1) drive=1;
    if (k==KEY2) drive=0;
    if (drive) VWSetSpeed(SPEED, dir); //MOTORDrive(1, SPEED);
      else VWSetSpeed(0,0); //MOTORDrive(1, 0);
    OSWait(100);  // reduce main loop to 10Hz
    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");

    l = getleftcone (scan, middle);  // left-most cone
    r = getrightcone(scan, middle);  // right-most cone
    if (l>0 && r>0 && l<r)
    { middle = (l+r)/2;          // middle position of [0..POINTS]
      dir = (POINTS/2 - middle)*5; // range +/-POINTS/2 * factor
      //dir = middle * 255 / POINTS; // range [0..255]
      //SERVOSet(1, 128-onebyte);  // 128 is middle
    }
    LCDPrintf("L %3d, R %3d, dir %3d   \n", 90-l, r-90, dir);

    // 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); // rem. prev. line
    }
    LCDLine(180+middle/SCL,0, 180+middle/SCL,150, RED); // middle line
  } while (k!=KEY4);
}
