task main()
{
	clearTimer(timer1);
	bool toggle1 = false;
	int toggletime = 200;
	bool togglecheck = false;
	int X1 = 1;
	int Y1 = 1;
	while (1 == 1)
	{
		if (time1[timer1] >= toggletime)
		{
			togglecheck = true;
			clearTimer(timer1);
		}
		if (togglecheck == true)
		{
			if (vexRT[Btn8D] == 1)
			{
				if (toggle1 == false)
					toggle1 = true;
				else
					if (toggle1 == true)
					toggle1 = false;
			}
		}
		if (toggle1 == true)
		{
			X1 = X1 * -1;
			Y1 = Y1 * -1;
		}
		togglecheck = false;
	}
}

////////////////////////////RPM CALCULATION TASK
task RPMCALC()
{
	int count = 0;
	int status = 0;
	clearTimer(T1);
	while(1)
	{
		if (SensorValue[TAC] == 0 && status == 0)
		{
			count++;
			status = 1;
		} else if (SensorValue[TAC] == 1)
		{
			status = 0;
		}
		if (count == 5)
		{
			CurrentSpeed = (1000*5*30)/time1[T1];
			clearTimer(T1);
			count = 0;
		}
		if (time1[T1] > 500)
		{
			CurrentSpeed = 0;
		}
	}
}

void roboControl(void){ //For autonomous control
	setLauncherSpeed(30);
	launcherSpeed_new = 127;//Lets start this shoot speed
	goForwardFor_time(10);//Go forward 10 seconds
	goBackwardFor_time(1);
	clearTimer(T1);
	while(time1[T1] < 20000){setMotor(vertBelt,80);setMotor(hozBelt,100);updateLauncherSpeed(2);}



	/*goBackwardFor_time(1);
	turnRight(0.1);
	setMotor(hozBelt,-100);
	goForwardFor_time(.5);
	turnRight(0.1);
	goForwardFor_time(5);*/
	goForwardFor_time(7);//Go forward 7 seconds
	goBackwardFor_time(1); //Get to launching distance
	clearTimer(T1);
	while(time1[T1] < 15000){setMotor(vertBelt,80);setMotor(hozBelt,100);updateLauncherSpeed(2);}
	goBackwardFor_time(1);
	turnRight(1);
	setMotor(hozBelt,-100);
	goForwardFor_time(.5);
	turnRight(0.25);
	goForwardFor_time(5);
}

void ofxGenericAnimatedImageView::timer_fired( ofPtr< ofxGenericTimer > timer )
{
    if ( timer == _frameTimer )
    {
        int newFrame = _currentFrame + _animationDirection;
        
        if ( newFrame >= (int) _frames.size() )
        {
            if ( _delegate )
            {
                _delegate.lock()->animatedImage_animationEnded( dynamic_pointer_cast< ofxGenericAnimatedImageView >( _this ) );
            }
            
            if ( _loopMode == ofxGenericAnimatedImageLoopTypeWrap )
            {
                newFrame = 0;
            }
            else if ( _loopMode == ofxGenericAnimatedImageLoopTypePingPong )
            {
                _animationDirection = -1;
                newFrame = _frames.size() - 1;
            }
            else if ( _loopMode == ofxGenericAnimatedImageLoopTypeClamp )
            {
                newFrame = _frames.size() - 1;
                clearTimer();
            }
            else if ( _loopMode == ofxGenericAnimatedImageLoopTypeOnce )
            {
                newFrame = 0;
                clearTimer();
            }
        }
        else if ( newFrame < 0 )
        {
            if ( _loopMode == ofxGenericAnimatedImageLoopTypePingPong )
            {
                _animationDirection = 1;
            }
            else if ( _loopMode == ofxGenericAnimatedImageLoopTypeClamp )
            {
                clearTimer();
            }
            
            if ( _animationDirection < 0 && _delegate )
            {
                _delegate.lock()->animatedImage_animationEnded( dynamic_pointer_cast< ofxGenericAnimatedImageView >( _this ) );
            }
            
            newFrame = 0;
        }
        
        _currentFrame = newFrame;
        showFrame( _currentFrame );
    }
}

task waitUntilNotFiringTask() {
	firing = true;
	wait1Msec(initialWait);
	clearTimer(firingTimer);

	while (time1(firingTimer) < timeWithoutFiring) {
		if (Error > notFiringCutoff) clearTimer(firingTimer);
	}
	firing = false;
}

task main()
{
	clearTimer(T1); //reset timer
	while(true){
		while(time1[T1] > 200) { //set timer to 100 so it doesn't overflow
			writeDebugStreamLine("%d", SensorValue(sonarSensor)); //display sonar sensor in robotC debug stream
			clearTimer(T1); //reset timer
		} //while time1
	}//while true
} //task

void ControlFunction(MOTOR_PI* motorA, MOTOR_PI* motorB){
	int storeTime;
	storeTime=time1[T1];
	if(storeTime >= SAMPLE_PERIOD/2){
		ReadSpeed(motorA, storeTime);
		ReadSpeed(motorB, storeTime);
		clearTimer(T1);
	}
	if(time1[T2] >= SAMPLE_PERIOD){
		PI_Control(motorA);
  	PI_Control(motorB);
		clearTimer(T2);
	}
}

task autonomous()
{
	TargetSpeed = 950;
	setpower = 104;
	startTask(motorcontrol);
	motor[Fly1] = power;
	motor[Fly2] = power;
	motor[Fly3] = power;
	motor[Fly4] = power;
	wait1Msec(1000);
	clearTimer(T1);
	while(true)
	{
		if(abs(Error) < 60)
		{
			motor[intkae] = 127;
			motor[feed] = 127;
		}
		else
		{
			motor[intkae] = 0;
			motor[feed] = 0;
		}
	}
}

///////////////////////////////////////TURN BASED ON GYRO DEGREES EXAMPLE // TurnDegree(-90, 127);//THIS TURNS LEFT 90 DEGREES AT 127 POWER
void TurnDegree(int degrees, int speed, int Timeout)
{
	int left = 0, right = 0;
	SensorValue[Gyro] = 0;
	if (degrees < 0){left = -1; right = 1;}
	else if (degrees > 0){left = 1; right = -1; }
	float ticks = abs(degrees*6.7);
	clearTimer(T3);

	while(abs(SensorValue[Gyro]) < ticks)
	{
		////////////////////////////////////FAILASFE TIMEOUT
		if(time1[T3] > TimeOut && TimeOut > 0){ FailSafeEngaged = 1; break;}
		////////////////////////////////////////////////////
		motor[Left1] = speed*left;
		motor[Left2] = speed*left;
		motor[Left3] = speed*left;
		motor[Right1] = speed*right;
		motor[Right2] = speed*right;
		motor[Right3] = speed*right;
	}
	motor[Left1] = (speed*left*-1)/9;
	motor[Left2] = (speed*left*-1)/9;
	motor[Left3] = (speed*left*-1)/9;
	motor[Right1] = (speed*right*-1)/9;
	motor[Right2] = (speed*right*-1)/9;
	motor[Right3] = (speed*right*-1)/9;
	wait1Msec(250);
	StopDrive();
}

task waitUntilNotFiringTask() {
	firing = true;
	//if (initialWait == 0) {
	//	while (SensorValue[flywheelSwitch] == 1) { EndTimeSlice(); }
	//}
	//else {
		wait1Msec(initialWait);
	//}
	clearTimer(T2/*firingTimer*/);

	while (time1[T2/*firingTimer*/] < timeWithoutFiring) {
		if (SensorValue[flywheelSwitch] == 0) clearTimer(firingTimer);
		EndTimeSlice();
	}
	firing = false;
}

task flywheelStabilization() { //modulates motor powers to maintain constant flywheel velocity
	clearTimer(T1);
	float prevError;
	float error;
	float integral;
	int numbbup = 0; //debug
	float totalError = 0;
  int numloops = 0;

	while (true)
	{
		prevError = targetVelocity - flywheelVelocity;
		integral = 0;

		while (abs(targetVelocity - flywheelVelocity) < bangBangErrorMargin * targetVelocity && targetVelocity > 0/*true*/) { EndTimeSlice();	}

		//bang bang control
		bangBangCount += 1;
		numbbup += (targetVelocity > flywheelVelocity ? 1 : 0);
		bbpercentup = 100 * numbbup / bangBangCount;
		bangBangPerSec = (float)((float)bangBangCount * 1000) / (float)(time1(T1) + .1);
		while (abs(targetVelocity - flywheelVelocity) > bangBangErrorMargin * flywheelVelocity  * 0.75 && targetVelocity > 0) {
			setLauncherPower((targetVelocity > flywheelVelocity) ? (127) : ( 0));
			EndTimeSlice();
		}

		setLauncherPower(defaultPower);
		while (targetVelocity == 0) { EndTimeSlice(); } //pauses while
	}
}

//Function used to determine which button has been pushed without polling
void testAndRespondToButtonPush(char buttonToTest) {
	if (buttonToTest & P1IFG) {

		if (buttonToTest & P1IES) {

			if (flag == 5) {
				LCDclr();
				location = initPlayer();
				printPlayer(location);
				flag = 0;
			}

			else {
				//This code is used to prevent the player (*) from moving outside of the game bounds.
				int mod = 0;
				mod = movePlayerInResponseToButtonPush(buttonToTest);
				clearPlayer(location);
				location += mod;
				location = movePlayer(location, mod);
				clearTimer();
			}

			//Debounces the button
		} else {
			debounce();
		}

		//Toggles between reading rising and falling edge
		P1IES ^= buttonToTest;
		P1IFG &= ~buttonToTest;
	}

}

/*******************************************************************************
  Function:       CNVSTimer::exit()
  Description:    退出定时检测线程
  Calls:            
  Called By:      
  Input:          无 
  Output:         无 
  Return:         无 
*******************************************************************************/
void CNVSTimer::exit()
{
    if(NULL == m_pVosThread)
    {
        IVS_RUN_LOG_ERR("NVSTimer exit: m_pVosThread is null");
        return;
    }

    //先停止线程
    this->m_bExit = VOS_TRUE;
    
    errno = 0;   
    long ret_val = VOS_ThreadJoin(m_pVosThread);
    if (ret_val != VOS_OK)
    {
        IVS_RUN_LOG_ERR("Wait timer thread exit failed. ret_val(%ld). error(%d):%s", ret_val, errno, strerror(errno));
    }

    //释放内存
    VOS_free(m_pVosThread);
    m_pVosThread = VOS_NULL;

    //再释放资源
    clearTimer();

    //释放锁
    (void)VOS_DestroyMutex(m_pMutexListOfTrigger);
    m_pMutexListOfTrigger = VOS_NULL;

    IVS_DBG_LOG("FILE(%s)LINE(%d): CNVSTimer::exit: exit complete.", _TIMER_FL_);
  
    return;
};

byte reserveAvailableTimer(tMotor motorPort1, tMotor motorPort2, byte fullSpeed, byte slowSpeed, int ms, tSensors encoderPort, int encoderSlowPos, int encoderStopPos, bool brake)
{
	for (byte i = 0; i < 4; i++)
	{
		// Check for existing port first to reset, afterwards look for inactive slot to activate
		if ((motorCtrl[i].motorPort1 == motorPort1 && motorCtrl[i].motorPort2 == motorPort2) || !motorCtrl[i].active) {
			clearTimer(motorCtrl[i].timer);
			motorCtrl[i].ms = (ms > 32767 ? 32767 : ms);
			motorCtrl[i].motorPort1 = motorPort1;
			motorCtrl[i].motorPort2 = motorPort2;
			motorCtrl[i].powerPct = fullSpeed;
			motorCtrl[i].slowPct = slowSpeed;
			motorCtrl[i].encoderInUse = true;
			motorCtrl[i].encoderPort = encoderPort;
			motorCtrl[i].encoderSlowPos = encoderSlowPos;
			motorCtrl[i].encoderStopPos = encoderStopPos;
			motorCtrl[i].lastKnownPos = SensorValue[encoderPort];
			motorCtrl[i].brake = brake;
			motorCtrl[i].active = true;		// Activate last
			//writeDebugStreamLine("Reserve Timer: %d", i);
			return i;
		}
	}
	return -1;
}

task autonomous()
{

	wait1Msec(5000);
	startTask(FlywheelController);
	SetTarget(1600,80);
	startTask(recoverFromShots);

	wait1Msec(4500);
	SensorValue[ANGLE] = 0;
	clearTimer(T3);

	while(time1[T3] <2550)
	{
		motor[rdy] = motor[rd] = -127 - (SensorValue[ANGLE]);  // was 8
		motor[ld] = motor[ldy] = -127 + (SensorValue[ANGLE]);
		wait1Msec(10);
	}
	motor[rd] = motor[rdy] = 0;
	motor[ld] = motor[ldy] = 0;
	startTask(autoConveyor);
	while(true)
	{}
//	AutonomousCodePlaceholderForTesting();  // Remove this function call once you have "real" code.
}

task driveStraightTask()
{
	driveStraightRunning = true;

	int coeff = 15;
	int totalClicks = 0;
	int slavePower = drivePower - 2;
	int error = 0;

	SensorValue[leftEncoder] = 0;
	SensorValue[rightEncoder] = 0;
	SensorValue[gyro] = 0;
	clearTimer(driveTimer);

	while (abs(totalClicks) < clicks  && time1(driveTimer) < timeout)
	{
		setDrivePower(drivePower * direction, slavePower * direction);

		error = SensorValue[gyro];

		slavePower += error / coeff;

		totalClicks += (abs(SensorValue[leftEncoder]) + abs(SensorValue[rightEncoder])) / 2;
		SensorValue[leftEncoder] = 0;
		SensorValue[rightEncoder] = 0;

		wait1Msec(100);
	}
	setDrivePower(0, 0);
	wait1Msec(delayAtEnd);
	driveStraightRunning = false;
}

///////////////////////////////////////INTAKE SHOOT BASED ON RPM AND TOLERANCES
void IntakeShoot(int TimeOut, int Tolerance = 8)
{
	if (BallCounter == 1) Tolerance = 12;
	clearTimer(T3);
	while(BallCounter > 0)
	{
		while(SensorValue[IR4] == 1)//MOVE BALL UP TO SENSOR
		{
			//BREAK STATEMENTS
			if(time1[T3] > TimeOut && TimeOut > 0){ FailSafeEngaged = 1; break;}
			if(BallCounter == 0) break;
			//BREAK STATEMENTS
			motor[Intake]= 127;
			motor[feeder]= 127;
		}

		if(rpmError < Tolerance)
		{
			motor[Intake]= 127;
			motor[feeder]= 127;
		}else
		{
			motor[Intake]= 0;
			motor[feeder]= 0;
		}
		if(time1[T3] > TimeOut && TimeOut > 0){ FailSafeEngaged = 1; break;}//BREAK STATEMENTS
	}
}

task feederWait () {
	wait1Msec(1500); //Wait time to delay for auto feeder
	clearTimer(T4);
	while(true) {
		if(!SensorValue[ballHigh]) {
			motor[feeder] = 127;
			} else if(time1[T4]>=feederWaitTime) {
			motor[feeder] = 127;
			clearTimer(T4);
			wait1Msec(200);
			} else {
			motor[feeder] = 0;
		}
		wait1Msec(25);
	}
}

task usercontrol()
{
	// User control code here, inside the loop
	bool toggle1 = false;
	int toggletime = 200;
	bool togglecheck = false;
	int X2 = 0, Y1 = 0, X1 = 0, threshold = 10;

	while (true)
	{
		if (time1[timer1] >= toggletime)
		{
			togglecheck = true;
			clearTimer(timer1);
		}
		//Create "deadzone" variables. Adjust threshold value to increase/decrease deadzone

		//Create "deadzone" for Y1/Ch3
		if(abs(vexRT[Ch3]) > threshold)
			Y1 = vexRT[Ch3];
		else
			Y1 = 0;
		//Create "deadzone" for X1/Ch4
		if(abs(vexRT[Ch4]) > threshold)
			X1 = vexRT[Ch4];
		else
			X1 = 0;
		//Create "deadzone" for X2/Ch1
		if(abs(vexRT[Ch1]) > threshold)
			X2 = vexRT[Ch1];
		else
			X2 = 0;


		if (togglecheck == true)
		{
			if (vexRT[Btn8D] == 1)
			{
				if (toggle1 == false)
					toggle1 = true;
				else
					if (toggle1 == true)
					toggle1 = false;
			}
		}
		if (toggle1 == true)
		{
			X1 = X1 * -1;
			Y1 = Y1 * -1;
		}
		togglecheck = false;

		//Remote Control Commands
		motor[frontRight] = Y1 - X2 - X1;
		motor[backRight] =  Y1 - X2 + X1;
		motor[frontLeft] = Y1 + X2 + X1;
		motor[backLeft] =  Y1 + X2 - X1;
	}
}

void goBackwardFor_time(int time){ //Goes Backwards for a set ammount of time
	setLeftMotors(-autoSpeed);
	setRightMotors(-autoSpeed);
	clearTimer(T1);
	while(time1[T1] < time* 1000){updateLauncherSpeed();}
	stopLeftMotors();
	stopRightMotors();
}