/* Drive-Left Maze Program   */
/* with compensation         */
/* Thomas Braunl, 1998, 2019 */
#include "eyebot.h"
#define sqr(x) ((x)*(x))
#define THRES 300
#define DIST  131
#define MSIZE 360
#define SPEED 100

int main ()
{ int F,L,R, Ffree,Lfree,Rfree;
  int x1,x2,y1,y2,phi1,phi2, drivedist;
  LCDPrintf("MAZE Left\n");
  LCDMenu("","","","END");

  do
  { // 1. Check walls
    Ffree = PSDGet(PSD_FRONT) > THRES;
    Lfree = PSDGet(PSD_LEFT)  > THRES; 
    Rfree = PSDGet(PSD_RIGHT) > THRES;
    LCDSetPrintf(4,0, "L%d F%d R%d", Lfree, Ffree, Rfree);
    
    // 2. Rotations
    if (Lfree)         { VWTurn(+90, 90); VWWait(); } // Turn left
     else if (Ffree)   {  }                           // NO turn
       else if (Rfree) { VWTurn(-90, 90); VWWait(); } // Turn right
         else          { VWTurn(180, 90); VWWait(); } // Dead end
         
    // Driving straight 1 square
    // VWStraight(360, 360); VWWait();
    VWGetPosition(&x1,&y1,&phi1);
    do
    { L=PSDGet(PSD_LEFT); F=PSDGet(PSD_FRONT); R=PSDGet(PSD_RIGHT);
      LCDSetPrintf(1,0, "L%d F%d R%d\n", L,F,R);
      if (100<L && L<180  && 100<R && R<180)    // space check L and R
      { VWSetSpeed(SPEED, L-R);  // drive left if left > right
        LCDSetPrintf(2,0, "Check L+R %d       ", L-R);
      }
      else
       if (100<L && L<180)         // space check - LEFT
       { VWSetSpeed(SPEED, L-DIST); // drive left if left > DIST
         LCDSetPrintf(2,0, "Check Left %d", L-DIST);
       }
       else
        if (100<R && R<180)         // space check - RIGHT
        { VWSetSpeed(SPEED, DIST-R); // drive left if DIST > right 
          LCDSetPrintf(2,0, "Check Right %d", DIST-R);
        }
        else 
        { VWSetSpeed(SPEED, 0);
          LCDSetPrintf(2,0, "Straight          ");
        }
       
      VWGetPosition(&x2,&y2,&phi2);
      drivedist = sqrt(sqr(x2-x1)+sqr(y2-y1));
      //OSWait(10);
    } while (drivedist < MSIZE && F > MSIZE/2 - 50);      // stop in time
    //VWSetSpeed(0,0);
    
  } while (KEYRead() != KEY4);
}