/**
 * Start a competition.
 * This code needs to track the order of the field starting to ensure that
 * everything happens in the right order. Repeatedly run the correct
 * method, either Autonomous or OperatorControl when the robot is
 * enabled. After running the correct method, wait for some state to
 * change, either the other mode starts or the robot is disabled. Then go
 * back and wait for the robot to be enabled again.
 */
void SimpleRobot::StartCompetition()
{
    RobotMain();
    if (!m_robotMainOverridden)
    {
        while (1)
        {
            while (IsDisabled())
                Wait(.01);      // wait for robot to be enabled

            if (IsAutonomous())
            {
                Autonomous();   // run the autonomous method
                while (IsAutonomous() && !IsDisabled())
                    Wait(.01);
            }
            else
            {
                OperatorControl();      // run the operator control method
                while (IsOperatorControl() && !IsDisabled())
                    Wait(.01);
            }
        }
    }
}

/**
 * Start a competition.
 * This code needs to track the order of the field starting to ensure that everything happens
 * in the right order. Repeatedly run the correct method, either Autonomous or OperatorControl
 * when the robot is enabled. After running the correct method, wait for some state to change,
 * either the other mode starts or the robot is disabled. Then go back and wait for the robot
 * to be enabled again.
 */
void SimpleRobot::StartCompetition()
{
	RobotMain();
	if (!m_robotMainOverridden)
	{
		while (1)
		{
			if (IsDisabled())
			{
				Disabled();
				while (IsDisabled()) Wait(.01);
			}
			else if (IsAutonomous())
			{
				Autonomous();
				while (IsAutonomous() && IsEnabled()) Wait(.01);
			}
			else
			{
				OperatorControl();
				while (IsOperatorControl() && IsEnabled()) Wait(.01);
			}
		}
	}
}

/**
 * Start a competition.
 * This code needs to track the order of the field starting to ensure that everything happens
 * in the right order. Repeatedly run the correct method, either Autonomous or OperatorControl
 * when the robot is enabled. After running the correct method, wait for some state to change,
 * either the other mode starts or the robot is disabled. Then go back and wait for the robot
 * to be enabled again.
 */
void SimpleRobot::StartCompetition()
{
    RobotMain();
    
    if (!m_robotMainOverridden)
    {
        // first and one-time initialization
        RobotInit();

        while (true)
        {
            if (IsDisabled())
            {
                m_ds->InDisabled(true);
                Disabled();
                m_ds->InDisabled(false);
                while (IsDisabled()) m_ds->WaitForData();
            }
            else if (IsAutonomous())
            {
                m_ds->InAutonomous(true);
                Autonomous();
                m_ds->InAutonomous(false);
                while (IsAutonomous() && IsEnabled()) m_ds->WaitForData();
            }
            else
            {
                m_ds->InOperatorControl(true);
                OperatorControl();
                m_ds->InOperatorControl(false);
                while (IsOperatorControl() && IsEnabled()) m_ds->WaitForData();
            }
        }
    }
}

/**
 * Start a competition.
 * This code needs to track the order of the field starting to ensure that everything happens
 * in the right order. Repeatedly run the correct method, either Autonomous or OperatorControl
 * when the robot is enabled. After running the correct method, wait for some state to change,
 * either the other mode starts or the robot is disabled. Then go back and wait for the robot
 * to be enabled again.
 */
void SimpleRobot::StartCompetition()
{
	RobotMain();
	if ( !m_robotMainOverridden)
	{
		while (Simulator::ShouldContinue())
		{
			Simulator::NextStep(0.0);

			if (IsDisabled())
				continue;

			if (IsAutonomous())
			{
				Autonomous();					// run the autonomous method
				while (IsAutonomous() && !IsDisabled() && Simulator::ShouldContinue()) Wait(.01);
			}
			else
			{
				OperatorControl();				// run the operator control method
				while (IsOperatorControl() && !IsDisabled() && Simulator::ShouldContinue()) Wait(.01);
			}
		}
	}
}

/**
 * Start a competition.
 * This code needs to track the order of the field starting to ensure that everything happens
 * in the right order. Repeatedly run the correct method, either Autonomous or OperatorControl
 * or Test when the robot is enabled. After running the correct method, wait for some state to
 * change, either the other mode starts or the robot is disabled. Then go back and wait for the
 * robot to be enabled again.
 */
void SampleRobot::StartCompetition()
{
	LiveWindow *lw = LiveWindow::GetInstance();

	SmartDashboard::init();
	NetworkTable::GetTable("LiveWindow")->GetSubTable("~STATUS~")->PutBoolean("LW Enabled", false);

	RobotMain();

	if (!m_robotMainOverridden)
	{
		// first and one-time initialization
		lw->SetEnabled(false);
		RobotInit();

		while (true)
		{
			if (IsDisabled())
			{
				m_ds.InDisabled(true);
				Disabled();
				m_ds.InDisabled(false);
				while (IsDisabled()) sleep(1); //m_ds.WaitForData();
			}
			else if (IsAutonomous())
			{
				m_ds.InAutonomous(true);
				Autonomous();
				m_ds.InAutonomous(false);
				while (IsAutonomous() && IsEnabled()) sleep(1); //m_ds.WaitForData();
			}
            else if (IsTest())
            {
              lw->SetEnabled(true);
              m_ds.InTest(true);
              Test();
              m_ds.InTest(false);
              while (IsTest() && IsEnabled()) sleep(1); //m_ds.WaitForData();
              lw->SetEnabled(false);
            }
			else
			{
				m_ds.InOperatorControl(true);
				OperatorControl();
				m_ds.InOperatorControl(false);
				while (IsOperatorControl() && IsEnabled()) sleep(1); //m_ds.WaitForData();
			}
		}
	}
}

	//Grab first two and turn to go right
	void AutonomousType10() {
		SmartDashboard::PutString("STATUS:", "STARTING AUTO 10");
		robotDrive.MecanumDrive_Cartesian(0, -0.2, 0);
		if (WaitF(1.2))
			return;
		robotDrive.MecanumDrive_Cartesian(0, 0, 0);
		chainLift.SetSpeed(0.5);
		while (IsAutonomous() && maxUp.Get() && midPoint.Get()) {
		}
		chainLift.SetSpeed(0);
		robotDrive.MecanumDrive_Cartesian(0, 0.4, 0);
		if (WaitF(1.6))
			return;

		robotDrive.MecanumDrive_Polar(0, 0, 0.3);
		if (WaitF(2.6))
			return;

		robotDrive.MecanumDrive_Cartesian(0, -0.4, 0);
		if (WaitF(1))
			return;

		robotDrive.MecanumDrive_Polar(0, 0, -0.3);
		if (WaitF(2.6))
			return;

		robotDrive.MecanumDrive_Cartesian(0, -0.4, 0);
		if (WaitF(1.6))
			return;
		robotDrive.MecanumDrive_Cartesian(0, 0, 0);
		SmartDashboard::PutString("STATUS:", "AUTO 10 COMPLETE");
	}

	void Drive (float speed, int dist)
	{
		leftDriveEncoder.Reset();
		leftDriveEncoder.Start();
		
		int reading = 0;
		dist = abs(dist);
		
		// The encoder.Reset() method seems not to set Get() values back to zero,
		// so we use a variable to capture the initial value.
		dsLCD->PrintfLine(DriverStationLCD::kUser_Line2, "initial=%d\n", leftDriveEncoder.Get());
		dsLCD->UpdateLCD();

		// Start moving the robot
		robotDrive.Drive(speed, 0.0);
		
		while ((IsAutonomous()) && (reading <= dist))
		{
			reading = abs(leftDriveEncoder.Get());				
			dsLCD->PrintfLine(DriverStationLCD::kUser_Line3, "reading=%d\n", reading);
			dsLCD->UpdateLCD();
		}

		robotDrive.Drive(0.0, 0.0);
		
		leftDriveEncoder.Stop();
	}	

	void AutonomousStateMachine() {
		pneumaticsControl->initialize();
		shooterControl->initializeAuto();
		driveControl.initializeAuto();
		enum AutoState {
			AutoDrive, AutoSetup, AutoShoot
		};
		AutoState autoState;
		autoState = AutoDrive;

		bool feederState = false;
		bool hasFired = false;
		Timer feeder;

		while (IsAutonomous() && IsEnabled()) {
			GetWatchdog().Feed();
			switch (autoState) {//Start of Case Machine
			case AutoDrive://Drives the robot to set Destination 
				bool atDestination = driveControl.autoPIDDrive2();
				if (atDestination) {//If at Destination, Transition to Shooting Setup
					autoState = AutoSetup;
				}
				break;
			case AutoSetup://Starts the ballgrabber motors to place the ball and extends ballgrabber
				if (!pneumaticsControl->ballGrabberIsExtended()) {//extends ballgrabber if not already extended
					pneumaticsControl->ballGrabberExtend();
				}

				if (!feederState) {//Started the feeder timer once
					feederState = true;
					feeder.Start();
					autoState = AutoShoot;
				}

				break;
			case AutoShoot://Shoots the ball
				if (feeder.Get() < 2.0) {//Runs ballgrabber for 2.0 Seconds at most
					shooterControl->feed(true);
				} else if (feeder.Get() >= 2.0) {//Transition to Shooting
					shooterControl->feed(false);
					feeder.Stop();
				}
				
				if (pneumaticsControl->ballGrabberIsExtended() && !hasFired) {
					shooterControl->autoShoot();
					dsLCD->PrintfLine(DriverStationLCD::kUser_Line1,
							"The robot is(should be) shooting");
					if (shooterControl->doneAutoFire()) {//Returns true only after shoot is done firing
						hasFired = true;
					}
				} else if (hasFired) {//runs only after shoot is done
					dsLCD->PrintfLine(DriverStationLCD::kUser_Line1,
							"AutoMode Finished");
				}
				break;
			}
			dsLCD->UpdateLCD();

		}
	}

	void output (void)
	{
		REDRUM;
		if (IsAutonomous())
			driverOut->PrintfLine(DriverStationLCD::kUser_Line1, "blaarag");
		else if (IsOperatorControl())
		{	
			REDRUM;
		}
		outputCounter++;
					
					if (outputCounter % 30 == 0){
						REDRUM;
					driverOut->PrintfLine(DriverStationLCD::kUser_Line2, "Top Shooter RPM: %f",(float)LTop.GetSpeed());
					driverOut->PrintfLine(DriverStationLCD::kUser_Line3, "Bot Shooter RPM: %f",(float)LBot.GetSpeed());
			//		driverOut->PrintfLine(DriverStationLCD::kUser_Line6, "Pilot Z-Axis: %f",pilot.GetZ());
					
					}
					
					if (outputCounter % 60 == 0){
						REDRUM;
					driverOut->PrintfLine(DriverStationLCD::kUser_Line4, "Top CANJag Temp: %f Celcius",LTop.GetTemperature()*(9/5) + 32);
					driverOut->PrintfLine(DriverStationLCD::kUser_Line5, "Bot CANJag Temp: %f Celcius",LBot.GetTemperature()*(9/5) + 32);
				    outputCounter = 1;
					}
		driverOut->UpdateLCD();
	}//nom nom nom

void Michael1::Autonomous(void)
{
	printf("\n\n\tStart Autonomous:\n\n");
	GetWatchdog().SetEnabled(false);
	ariels_light->Set(1);
		
	while (IsAutonomous())
	{
		Wait(0.1); //important
		dt->SmoothTankDrive(left_stick, right_stick);
		//dt->UpdateSlip();
		//dt->UpdateSlip(); //calling slipControl(true) should spawn a task which does this.
		
		//printf("Encoder: %f, ", dt->encoder_left->GetDistance());
		//printf("Gyro: %f, ", dt->gyro->GetAngle());
		//printf("Accel: %f", dt->accel->GetAcceleration());
		//printf("\n\n");s
		
		/*Wait(.1);
		if(cam->FindTargets()){
			ariels_light->Set(1);
		} else {
			ariels_light->Set(0);
		}
		*/
	}

}

	//Grab first yellow, back up to auto zone, drop
	void AutonomousType11() {
		SmartDashboard::PutString("STATUS:", "STARTING AUTO 11");
		chainLift.SetSpeed(0.5);
		while (IsAutonomous() && IsEnabled() && maxUp.Get() && midPoint.Get()) {
		}
		chainLift.SetSpeed(0);
		robotDrive.MecanumDrive_Cartesian(0, 0.4, 0);
		if (WaitF(3))
			return;
		robotDrive.MecanumDrive_Cartesian(0, 0, 0);
		chainLift.SetSpeed(-0.5);
		while (IsAutonomous() && IsEnabled() && maxDown.Get()) {
		}
		chainLift.SetSpeed(0);
		SmartDashboard::PutString("STATUS:", "AUTO 11 COMPLETE");
	}

/* RunScript is blocking (pauses thread until script is complete)
 * Takes a pointer to a Command in a Command array (Script).
 * Iterates over said array until reaches "END" command.
 */
void Michael1::RunScript(Command* scpt){
	bool finished = false;
	
	while (IsAutonomous())
		{
			switch(scpt->cmd){
			case TURN:
				dt->Turn(scpt->param1,14.5 - time->Get());
				break;
			case JSTK:
				dt->SetMotors(scpt->param1, scpt->param2);
				Wait(14.5 - time->Get());
				break;
			case WAIT:
				dt->SetMotors(0,0);
				Wait(scpt->param1);
				break;
			case FWD:
				dt->GoDistance(scpt->param1,14.5 - time->Get());
				break;
			default:
				dt->SetMotors(0,0);
				finished = true;
			}
			if (finished){
				break;
			}
			scpt++;
		}
}

	void Autonomous()
	{
		while(IsAutonomous())
		{
			//do stuff
		}
	}

void DriveController :: Run()
{
	if ( IsAutonomous() )
		Autonomous();
	
	else if ( IsOperatorControl() )
		OperatorControl();
}

	void Autonomous() {
		if (DriverStation::GetInstance()->IsFMSAttached()) log->InMatch();
		
		log->Info("AUTONOMOUS START");
		
		while (IsAutonomous()) {
			Wait(0.10);
		}
	}

	void Autonomous(void)
	{
		GetWatchdog().SetEnabled(false);
		
		if (recorder.StartPlayback())
		{
			while (IsAutonomous() && recorder.Playback());
		}
	}

	void Autonomous ()
	{
		DriveTrain.StartDriveTrain();
		Shooter.StartShooterAuto();
		AutonomousState = 1;
		while(IsAutonomous())
		{
			if (AutonomousSwitch.Get() == 0)
			{
				if (AutonomousState == 1)
				{
					DriveTrain.RunDriveTrain(1, 1, 0, 0);
					if ((DriveTrain.LeftEncTotal >= 2400) || (DriveTrain.RightEncTotal >= 2400))
					{
						AutonomousState = 2;
					}
				}
				else if ((AutonomousState == 2) && (HotGoal == true))
				{
					Shooter.RunShooter(0, 1, 0);
				}
			}
			else if (AutonomousSwitch.Get() == 1)
			{
				if (AutonomousState == 1)
				{
					DriveTrain.RunDriveTrain(1, 1, 0, 0);
					if ((DriveTrain.LeftEncTotal >= 2400) || (DriveTrain.RightEncTotal >= 2400))
					{
						AutonomousState = 2;
					}
				}
				else if ((AutonomousState == 2) && (WhichHotGoal != 0))
				{
					DriveTrain.RunDriveTrain(WhichHotGoal, -WhichHotGoal, 0, 0);
					if (((WhichHotGoal == 1) && ((DriveTrain.LeftEncTotal >= 2800) || (DriveTrain.RightEncTotal <= 2000))) || ((WhichHotGoal == -1) && ((DriveTrain.LeftEncTotal <= 2000) || (DriveTrain.RightEncTotal >= 2800))))
					{
						AutonomousState = 3;
					}
				}
				else if (AutonomousState == 3)
				{
					Shooter.RunShooter(0, 1, 0);
					AutonomousState = 4;
				}
				else if (AutonomousState == 4)
				{
					DriveTrain.RunDriveTrain(-WhichHotGoal, WhichHotGoal, 0, 0);
					if (((WhichHotGoal == 1) && ((DriveTrain.LeftEncTotal <= 2400) || (DriveTrain.RightEncTotal >= 2400))) || ((WhichHotGoal == -1) && ((DriveTrain.LeftEncTotal >= 2400) || (DriveTrain.RightEncTotal <= 2400))))
					{
						AutonomousState = 5;
					}
				}
			}
		}
	}

void Robot::Autonomous(void)
{
  printf("Auto Mode!");

  while(IsAutonomous() && IsEnabled())
  {
    Wait(.1);
    printf("autonomous\r\n");
  }
}

	void Autonomous(){
		Option *num = (Option *) chooser->GetSelected();
		myRobot->ResetDisplacement();
		Modes->SetMode(num->Get());
		Modes->Run();
		while(IsAutonomous() && IsEnabled()){
			Wait(0.05);
			Scheduler::GetInstance()->Run();
		}
	}

void Robot::AutonFeed() {
    while (IsEnabled() && IsAutonomous()) {
        DS_PrintOut();

        // make ball conveyors run in reverse for all of Autonomous
        lift.Set(Relay::kReverse);

        Wait(0.1);
    }
}