#include "pai.h"
#include <math.h>

int GRAB_BALL;
int FOUND_BALL;

int MinSonarDist(int low, int high) {
  int i;
  int closest = 5000;
  for(i = low; i <= high; i++)
    if(paiSonarRange(i) < closest)
      closest = paiSonarRange(i);
  return(closest);      
}

void rotfunc(void);
void velfunc(void);
void findfunc(void);
void grabprepfunc(void);
void grabfunc(void);

paiMAKE_PROCESS(GrabProc, grabfunc, 1)

paiMAKE_PROCESS(GrabPrepProc, grabprepfunc, 1)

paiMAKE_PROCESS(FindProc, findfunc, 1)

paiMAKE_PROCESS(TurnProc, rotfunc, 1)
     
paiMAKE_PROCESS(GoProc, velfunc, 0)

void rotfunc(void) {
  paiRotateRobot(0.05 * DEG_TO_RAD * paiVisBlobX(CHANNEL_A));
}

void velfunc(void) {
  char buffer[256];
  sprintf(buffer, "X: %d  Y: %d", paiVisBlobX(CHANNEL_A), paiVisBlobY(CHANNEL_A));
  sfMessage(buffer);
  if(MinSonarDist(1, 5) < 400) {
    paiSetRobotVelocity(0);
  }
  else {
    if(paiVisBlobArea(CHANNEL_A) > 20) {
      if(paiVisBlobY(CHANNEL_A) > 232) {
	FOUND_BALL = 1;
      }
      paiSetRobotVelocity(150);
    }
    else {
      paiSetRobotVelocity(0);
    }
  }
}

void findfunc(void) {
  if(paiVisBlobArea(CHANNEL_A) < 20) {
    paiRotateRobot(5.0 * DEG_TO_RAD);
  }
}

void grabprepfunc(void) {
  if(paiVisBlobY(CHANNEL_A) == 0 && FOUND_BALL) {
    GRAB_BALL = 1;
    paiSuspendProcess(FindProc);
    paiSuspendProcess(TurnProc);
    paiSuspendProcess(GoProc);
    paiSetGripperState(paiGRIPPEROPEN);
    paiActivateProcess(GrabProc);
  }
}

void grabfunc(void) {
  if(!paiGripperFrontBreakbeam() || !paiGripperRearBreakbeam()) {
    paiSetRobotVelocity(50);
  }
  else if(paiGripperFrontBreakbeam() && !paiGripperRearBreakbeam()) {
    paiSetRobotVelocity(20);
  }
  else if(paiGripperRearBreakbeam()) {
    paiSetRobotVelocity(0);
    paiSuspendProcess(GrabPrepProc);
    paiSetGripperState(paiGRIPPERUP);
    paiRobotShutdown();
  }
}


void main(int argc, char** argv) {
  if(paiRobotStartup(sfTTYPORT, sfCOM2)) {
    FOUND_BALL = 0;
    GRAB_BALL = 0;
    paiInitProcess(FindProc, "FIND", paiACTIVE);
    paiInitProcess(TurnProc, "ROT", paiACTIVE);
    paiInitProcess(GoProc, "VEL", paiACTIVE);
    paiInitProcess(GrabPrepProc, "GP", paiACTIVE);
    paiInitProcess(GrabProc, "GRAB", paiSUSPENDED);
  }
  else {
    printf("Dang!\n");
  }
  while(1);
}