#include "tracking.h"

extern VWHandle vwhandle;

int turn_left(VWHandle *vw_handle)
{
#ifdef	DEBUG
	LCDPrintf("turning left\n");
#endif
	if (VWDriveTurn(*vw_handle, TURN_ANGLE, TURN_SPEED)!=0) return false;
	if (VWDriveWait(*vw_handle) !=0) return false;
	return true;	
}

int turn_right(VWHandle *vw_handle)
{
#ifdef DEBUG
	LCDPrintf("turning right\n");
#endif
	if (VWDriveTurn(*vw_handle, -TURN_ANGLE, TURN_SPEED)!=0) return false;
	if (VWDriveWait(*vw_handle) !=0) return false;
	return true;	
}

int move_forward(VWHandle *vw_handle)
{
#ifdef DEBUG
	LCDPrintf("moving forward\n");
#endif
	if( VWDriveStraight(*vw_handle, DISTANCE, FORWARD_SPEED) !=0)return false;
	if(VWDriveWait(*vw_handle) !=0) return false;
	return true;
}

int move_backward(VWHandle *vw_handle)
{
#ifdef DEBUG
	LCDPrintf("moving forward\n");
#endif
	if( VWDriveStraight(*vw_handle, -DISTANCE, FORWARD_SPEED) !=0)return false;
	if(VWDriveWait(*vw_handle) !=0) return false;
	return true;
}
	
int initVW(VWHandle *vw_handle)
{
	 *vw_handle = VWInit(VW_DRIVE, 1);
        if (*vw_handle == 0) {
                LCDPrintf("VWInit() error\n");
                return false;
        }
        VWStartControl(*vw_handle, 7, 0.3, 7, 0.1); /*optimum drive control*/
        /* Vv: 7 (proportional component of the v-controller)
         * Tv: 0.3 (integrating component of the v-controller)
         * Vw: 7 (proportional component of the w-controller)
         * Tw: 0.1 (integrating component of the w-controller)
         */

	return true;
}

int releaseVW(VWHandle *vw_handle)
{
	if (VWStopControl(*vw_handle) == -1) 
	{
		LCDPrintf("error stopping VWControl...\n");
		return false;
	}

	if(VWRelease(*vw_handle)==-1)
	{
		LCDPrintf("error releasing VWHandle...\n");
		return false;
	}

	return true;
}
			

void initPSD(PSDHandle *psd_front, PSDHandle *psd_right, PSDHandle *psd_left, PSDHandle *psd_back)
{
	*psd_front = PSDInit(PSD_FRONT);
  	*psd_left  = PSDInit(PSD_LEFT);
  	*psd_right = PSDInit(PSD_RIGHT);
//	*psd_back = PSDInit(PSD_BACK);
  	PSDStart(*psd_front|*psd_left|*psd_right, /* | *psd_back */ TRUE); /* start all 3 psd in continuous measuring cycles*/
}

void releasePSD(void)
{
	PSDStop();
	PSDRelease();
}

void camInit(void)
{
  	int camera;		/* just used for checking camera status */

  	camera = CAMInit(NORMAL);	/* CAMInit is an eyebot library func */

  	if (camera == NOCAM)
    		LCDPrintf("Camera not detected!\n");
  	else if (camera == INITERROR)
      		LCDPrintf("Camera Initialisation Error!\n");
}

/**
 * @brief Releases camera resources.
 */ 
void camRelease(void)
{
  	if(CAMRelease()==-1)
    		LCDPrintf("Warning: Camera Resources Not Released\n");
}