/*
 * Filename:	nav.c
 *
 * Author:	Elliot Nicholls <nicho-ej@ciips.ee.uwa.edu.au>, UWA 1997
 *          Thomas Braunl, UWA 2000, update V3.0
 *
 * Description:	Drives EyeBot vehicle using front IR sensors and PSD sensor
 *		Will navigate with semi-intelligence
 *	        
 ****************************************************************************/

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

#define Velv 7.0
#define Timev 0.3
#define Velw 7.0
#define Timew 0.1
#define PSD_LIMIT_L 250
#define PSD_LIMIT_H 800
#define SPD_FWD 0.2
#define SPD_REV -0.1
#define PSD_TURN_CORRECT 12 /* degrees */
#define BACK_OFF_DIST 0.30 /* meters */

/*
 * Global variables
 **********************************************************************/
VWHandle vw;
static PSDHandle psd;
IRHandle irLF, irRF, irLM, irRM, irLB, irRB, irBA;
int lastTurnLeft = 0;
SpeedType s;

/*
 * Forward declations
 **********************************************************************/
void PSDTurnParallel(void);
void irEvade(void);
int  Detect(void);
int  sqr(int x);

/*
 * Function to stop the vehicle
 **********************************************************************/

void VWStop(VWHandle vw) {
  VWSetSpeed(vw, 0, 0);
  return;
}

/*
 * Function to turn the vehicle by number of degrees
 * Function uses VWDriveTurn and blocks
 **********************************************************************/
 
void VWTurn(VWHandle vw, float degrees, float speed, BOOL block) {

  float rad;

  rad = degrees * M_PI / 180;
  VWDriveTurn(vw, rad, speed);
  if(block == TRUE) {
    do {
    } while(VWDriveDone(vw) == 0);
  }
  return;
}


int SysInitialise(void) {
  /*
   * Initialise PSD controller
   **********************************************************************/

  psd = PSDInit(PSD_FRONT);
  if(psd == 0) {
    LCDPutString("PSDInit Error!\n");
    OSWait(200);
    return 0;
  }

  if(PSDStart(psd, TRUE)) {
    LCDPutString("PSD busy!\n");
    return 0;
  }

  /*
   * Initialise VW interface and get initial position data
   **********************************************************************/

  vw = VWInit(VW_DRIVE,1);
  if(vw == 0)
    {
      LCDPutString("VWInit Error!\n");
      OSWait(200);
      return 0;
    }
  VWStartControl(vw, Velv, Timev, Velw, Timew);

  /*
   * Initialise IR sensors
   **********************************************************************/

  irLF = IRInit(IR_LF);
  if(irLF == 0) {
    LCDPutString("IRInit error (LF)\n");
    return 0;
  }
  irRF = IRInit(IR_RF);
  if(irRF == 0) {
    LCDPutString("IRInit error (RF)\n");
    return 0;
  }
  irLM = IRInit(IR_LM);
  if(irLM == 0) {
    LCDPutString("IRInit error (LM)\n");
    return 0;
  }
  irRM = IRInit(IR_RM);
  if(irRM == 0) {
    LCDPutString("IRInit error (RM)\n");
    return 0;
  }
  AUBeep();

  return 1;
}

/*
 * Release all handles before exiting program
 **********************************************************************/

void SysRelease(void) {

  PSDRelease();
  VWRelease(vw);
  IRRelease(irLF + irRF + irLM + irRM);
  return;
  
}

/*
 * Routine to handle driving forwards
 **********************************************************************/

void DriveForward(void) {
  PositionType begin;
  int psdObject, irDetect;

  VWGetPosition(vw, &begin);
  
  do {
    VWSetSpeed(vw, SPD_FWD, 0);
    
    /*
     * Check to see if there is any PSD/IR action
     **********************************************************************/
    do {
      if(VWStalled(vw) == 1) {
	VWDriveStraight(vw, BACK_OFF_DIST, SPD_REV);
	if(lastTurnLeft == 1) VWTurn(vw, 90, 1.0, TRUE);
	else VWTurn(vw, -90, 1.0, TRUE);
	VWSetSpeed(vw, SPD_FWD, 0);
      }
      psdObject = PSDGet(psd);
      irDetect = Detect();
    } while((psdObject > PSD_LIMIT_L) && (irDetect == 0));
    
    if(psdObject < PSD_LIMIT_L) {
      PSDTurnParallel();
    }
    if(irDetect == 1) {
      irEvade();
    }
  } while(1 == 1);
  
  return;
}

			  
/*
 * Attempt to evade objects using IR sensors
 **********************************************************************/

void irEvade() {
  if(IRRead(irLM) == 0) {
    VWSetSpeed(vw, SPD_FWD, -1.0);

    do {
      if(VWStalled(vw) == 1) {
	VWStop(vw);
	VWSetSpeed(vw, SPD_REV, -1.0);
      }
    } while((IRRead(irLM) == 0) && (PSDGet(psd) > PSD_LIMIT_L));
    VWSetSpeed(vw, SPD_FWD, 0.0);
    return;
  }
  if(IRRead(irRM) == 0) {
    VWSetSpeed(vw, SPD_FWD, 1.0);
    
    do {
      if(VWStalled(vw) == 1) {
	VWStop(vw);
	VWSetSpeed(vw, SPD_REV, 1.0);
      }
    } while((IRRead(irRM) == 0) && (PSDGet(psd) > PSD_LIMIT_L));
    VWSetSpeed(vw, SPD_FWD, 0.0);
    return;
  }

  /*
   * Back off if too close
   **********************************************************************/
   
  if(IRRead(irLF) == 0 || IRRead(irRF) == 0) {
    VWStop(vw);
    OSWait(30);
    VWSetSpeed(vw, SPD_REV, 0.0);
    do {
    } while(IRRead(irLF) == 0 || IRRead(irRF) == 0);
    VWStop(vw);
    PSDTurnParallel();
    return;
  }
}

/*
 * Detect object using IR sensors
 **********************************************************************/

int Detect(void) {
  if(IRRead(irLF) == 0) return 1;
  if(IRRead(irRF) == 0) return 1;
  if(IRRead(irLM) == 0) return 1;
  if(IRRead(irRM) == 0) return 1;
  return 0;
}

/*
 * Turn parallel to wall using PSD sensor on front of eyebot
 * Only works effectively with walls and accurate PSD readings
 **********************************************************************/

void PSDTurnParallel(void) {
  int psdStart, psdCurrent;
  float radRemain, radTurned;
  PositionType start, end;
  
  LCDPrintf("Starting PSDTurnParallel\n");
  VWStop(vw);
  psdStart = PSDGet(psd);
  if(psdStart > PSD_LIMIT_L) return;
  VWGetPosition(vw, &start);

  LCDPrintf("Got starting position\n");
  if(lastTurnLeft == 1) {
    VWSetSpeed(vw, 0.0, 1.0);
  } else {
    VWSetSpeed(vw, 0.0, -1.0);
  }

  LCDPrintf("Calling OSWait\n");
  OSWait(40);
  psdCurrent = PSDGet(psd);

  LCDPrintf("Entering test loop\n");
  if(psdCurrent > psdStart) {

    LCDPrintf("In loop...\n");
    do {
      LCDPrintf("Waiting\n");
      VWGetSpeed(vw, &s);
      LCDPrintf("%d, %d\n", s.v, s.w);
    } while(PSDGet(psd) < PSD_LIMIT_H);
    VWStop(vw);
    psdCurrent = PSDGet(psd);
    
    VWGetPosition(vw, &end);
    if(lastTurnLeft == 1) {
      radTurned = end.phi - start.phi;
      if(radTurned < 0) radTurned += 2*M_PI;
    } else {
      radTurned = start.phi - end.phi;
      if(radTurned < 0) radTurned += 2*M_PI;
    }

    radRemain = asin((psdStart * sin(radTurned)) / sqrt(sqr(psdStart) + sqr(psdCurrent) - (2 * psdStart * psdCurrent * cos(radTurned))));
    radRemain = radRemain + (PSD_TURN_CORRECT * M_PI / 180);

    LCDPrintf("Calling VWDriveTurn\n");
    if(lastTurnLeft == 1) {
      VWDriveTurn(vw, radRemain, 1.0);
    } else {
      VWDriveTurn(vw, -radRemain, 1.0);
    }

    do {
      LCDPrintf("Waiting to finish\n");
    } while(VWDriveDone(vw) == 0);

  } else {
    VWStop(vw);
    if(lastTurnLeft == 1) lastTurnLeft = 0;
    else lastTurnLeft = 1;
    LCDPrintf("Going a level deeper\n");
    PSDTurnParallel();
  }

  return;
}

/*
 * Square function
 **********************************************************************/

int sqr(int x) {
  return x*x;
}

/*
 * Main routine
 **********************************************************************/

int main(void) {

  if(SysInitialise() == 0) {
    LCDPutString("Init error!\n");
    return 1;
  }

  LCDPutString("Init OK!\n");

  DriveForward();

  SysRelease();
  return 0;
}