package org.usfirst.frc.team4590.robot.commands;

import org.usfirst.frc.team4590.robot.subsystems.EncoderMotor;

import edu.wpi.first.wpilibj.PIDController;
import edu.wpi.first.wpilibj.PIDController.PercentageTolerance;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

public class Rotate90Deg extends Command {

    EncoderMotor system;
    PIDController pid;

    public Rotate90Deg() {
        // TODO Auto-generated constructor stub
        system = EncoderMotor.getInstance();
        pid = new PIDController(0.0001, 0.00001, 0, 0, system, system);
        requires(system);
    }

    // Called just before this Command runs the first time
    @Override
    protected void initialize() {
        system.resetEncoder();
        pid.setAbsoluteTolerance(200);
        pid.setSetpoint(2200);
        pid.enable();
        SmartDashboard.putData("pid", pid);
    }

    // Called repeatedly when this Command is scheduled to run
    @Override
    protected void execute() {

    }

    private int timesHit = 0;
    // Make this return true when this Command no longer needs to run execute()
    @Override
    protected boolean isFinished() {
        if (pid.onTarget()) timesHit++;
        else timesHit = 0;
        if (timesHit >= 25) return true;
        return false;
    }

    // Called once after isFinished returns true
    @Override
    protected void end() {
        pid.disable();
        system.moveMotors(0);
    }

        // Called when another command which requires one or more of the same
        // subsystems is scheduled to run
    @Override
    protected void interrupted() {
        end();
    }

}