// Highlevel Drive functions on Raspberry, using only
// Lowlevel Functions on IO-Board 
// T. Bräunl, June 2019
#include "eyebot.h"
#define DEBUG    1
#define NOCOM    0
#define STRAIGHT 1
#define TURN     2

int vvx, vvy, vvphi, vvcom, vvdist, vvangle; //globals


void VVCheck()                    // Periodic check for drive completion
{ int x,y,phi, dist,angle;

  switch (vvcom)
  { case STRAIGHT:
      VWGetPosition(&x, &y, &phi);
      dist = sqrt((x-vvx)*(x-vvx) + (y-vvy)*(y-vvy));
      if (dist >= abs(vvdist))
      { VWSetSpeed(0,0);
        vvcom = NOCOM;
      }
      if (DEBUG) printf("check Straight dist %d vvdist %d\n", dist, vvdist);
      break;
      
    case TURN:
      VWGetPosition(&x, &y, &phi);
      if (phi<0) phi += 360;
      angle = phi - vvphi;
      if ((vvangle>=0 && (angle >= vvangle))
       || (vvangle< 0 && (angle <= vvangle)))
      { VWSetSpeed(0,0);
        vvcom = NOCOM;
      }
      if (DEBUG) printf("check Turn angle %d (%d - %d) vvangle %d\n", angle, phi,vvphi, vvangle);
      break;
  }    
}

int VVDone()
{ return (vvcom == NOCOM);
}

void VVWait()
{ int x, y, p;
  while (!VVDone())
  { VWGetPosition(&x,&y,&p);
    LCDPrintf("%6d x, %6d y, %6d phi\n", x,y,p);              // wait until completed
  }
}


int VVInit()
{ vvcom = NOCOM;
  return OSAttachTimer(100, VVCheck); // Check every 0.1s for completion
}


void VVStraight(int dist, int speed)
{ vvcom  = STRAIGHT;
  if (dist<0) {dist = -dist; speed = -speed;} // reversing
  vvdist = dist;
  VWGetPosition(&vvx, &vvy, &vvphi);
  VWSetSpeed(speed, 0);  // Drive straight at desired SPEED
}


void VVTurn(int angle, int speed)
{ vvcom   = TURN;
  if (angle<0) speed = -speed;
  vvangle = angle;
  VWGetPosition(&vvx, &vvy, &vvphi);
  if (vvphi<0) vvphi += 360;
  VWSetSpeed(0, speed);  // Turn at desired SPEED
}


int main()
{ LCDPrintf("Init Timer\n");
  VVInit();
  LCDPrintf("Drive straight\n");
  VVStraight(500, 250);
  VVWait();              // wait until completed

  LCDPrintf("Rotate 180 deg.\n");
  VVTurn(180, 60);       // half turn in ca. 3s
  VVWait();              // wait until completed

  LCDPrintf("Drive straight\n");
  VVStraight(500, 250);
  VVWait();              // wait until completed

  LCDPrintf("Rotate 180 deg.\n");
  VVTurn(-180, 60);       // half turn in ca. 3s
  VVWait();              // wait until completed
}
