/*
* Simple demo code which shows camera stream and distance sensors on screen, and can be enabled to make the robot drive back and forwards.
* Parham Bahrami, 2025
*/

#include "eyebot.h"

int motorfunction = 0;
BYTE image[QQVGA_SIZE];
int remaining = 0;

bool motoron = false;

void setup() {
}

void loop() {
  if(KEYRead() == KEY_LEFT){
    motoron = !motoron;
    delay(500);
    if(!motoron) VWStop();
  }

  if(!motorfunction && !VWDone() && motoron){
    VWStraight(500,100);
    motorfunction = 1;
  }
  if(motorfunction && !VWDone() && motoron){
    VWStraight(-500, 100);
    motorfunction = 0;
  }
  
  CAMGet(image);
  LCDImage(image);

  LCDSetPos(0, WIDTH+10);
  LCDPrintf("L: ");
  LCDPrintf("%d", PSDGet(2));
  LCDPrintf("   ");
  LCDSetPos(20, WIDTH+10);
  LCDPrintf("F: ");
  LCDPrintf("%d", PSDGet(1));
  LCDPrintf("   ");
  LCDSetPos(40, WIDTH+10);
  LCDPrintf("R: ");
  LCDPrintf("%d", PSDGet(3));
  LCDPrintf("   ");
  LCDSetPos(HEIGHT + 5, 0);
  LCDPrintf("Remaining: ");
  LCDPrintf("%d", VWRemain());
  LCDPrintf("   ");
  LCDSetPos(HEIGHT + 20, 0);
  LCDPrintf("Press left btn to stop \nor start motor");
  delay(10);

}