int wallfound;


wallfound=1;

configureCollisionDetection(1, 20.0, 50.0, 20.0, 50.0, 0.5);

float _sqrt(float value)
{
    if (value <= 0)
   {
     return 0;
   }
   return value ^ 0.5;
}

float asinInDegrees(float x)
{
    float negate = 0;
    if (x < 0)
    {
    	x = x*-1;
    	negate = 1;
    }
    float pi = 3.14159265;
    float radians = -0.0187293;
    radians = radians * x;
    radians = radians + 0.0742610;
    radians = radians * x;
    radians = radians - 0.2121144;
    radians = radians * x;
    radians = radians + 1.5707288;
    radians = pi * 0.5 - ((1.0 - x) ^ 0.5) * radians;
    radians = radians - 2 * negate * radians;
    return radians * 180 / pi;
}

void _doRoll(float duration, float speed, float direction)
{
   if (duration > 0)
   {
     controlSystemTargetYaw = direction;
     controlSystemTargetSpeed = speed;

     float finishTime = currentRobotTime + duration;
     while (currentRobotTime < finishTime)
     {
       wait;
     }
   }
   controlSystemTargetSpeed = 0;
}

void find_wall()
{
	//reset direction

	controlSystemTargetYaw = 0;
	setCalibration = 0;

	//move until collision
	controlSystemTargetSpeed = 100;

	//find collision orientation
	//reset heading
}

void find_orientation()
{
	setRgbLed(0,255,0);
	//current position
	float startX = locatorPositionX;
	float startY = locatorPositionY;

	//move heading 0
	_doRoll(1,20,0);

	//new position
	float endX = locatorPositionX;
	float endY = locatorPositionY;

	//calculate deviation from calibration
	float offsetAngle = 0;
	float deltaX = endX - startX;
	float deltaY = endY - startY;

	float deltaHeading = asinInDegrees((deltaY/deltaX)/_sqrt((deltaX^2 + deltaY^2)));
	int TEMP = controlSystemTargetYaw - 90 + deltaHeading;
	setCalibration = TEMP;
	setRgbLed(0,255,255);
	delay(1);

	//set calibration
}

void change_direction(int change)
{
	controlSystemTargetYaw = controlSystemTargetYaw - change;
	setCalibration = controlSystemTargetYaw;
}

void turn_left()
{
	//stop
	controlSystemTargetSpeed = 0;
	//turn direction
	change_direction(-90);
	//maybe find wall again
}


void turn_right() {
	//stop
	controlSystemTargetSpeed = 0;
	//turn direction
	change_direction(90);
	//find wall again
}


void handleCollision()
{
	setRgbLed(255,0,0);

	if (wallfound == 1)
	{
		controlSystemTargetSpeed = 0;

		wallfound = 0;
		delay(3);
		find_orientation();


	}
	turn_right();
}


void follow_wall()
{
	while (accelSensorXRight > -1)
	{
		//move on heading -10
		controlSystemTargetImuYaw = -10;
		controlSystemTargetSpeed = 80;
	}

	turn_left();

}





onCollisionEvent = &handleCollision;

void startProgram()
{
	find_wall();
	while (1)
	{
		follow_wall();
	}

}