/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package frc.robot;

import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import com.ctre.phoenix.motorcontrol.can.*;
import com.ctre.phoenix.motorcontrol.FeedbackDevice;
import edu.wpi.first.wpilibj.Timer;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the TimedRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the build.gradle file in the
 * project.
 */
public class Robot extends TimedRobot {
  private WPI_TalonSRX leftMotor, rightMotor;
  private double leftPower, rightPower;
  private Timer  autoTimer;
  private boolean autoDone;

  /**
   * This function is run when the robot is first started up and should be
   * used for any initialization code.
   */
  @Override
  public void robotInit() {
     // CAN motor controllers connected to CAN bus (addrs 3,4)
     leftMotor = new WPI_TalonSRX(3);
     rightMotor = new WPI_TalonSRX(4);
     // clear any previously configured settings
     leftMotor.configFactoryDefault();
     rightMotor.configFactoryDefault();
     // right motor turns in opposite direction
     rightMotor.setInverted(true);
     // Quadrature encoders are connected directly to the motor controllers
     leftMotor.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder);
     rightMotor.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder);
     // Timer for closed-loop updates
     autoTimer = new Timer();
  }

  /**
   * This function is called every robot packet, no matter the mode. Use
   * this for items like diagnostics that you want ran during disabled,
   * autonomous, teleoperated and test.
   *
   * <p>This runs after the mode specific periodic functions, but before
   * LiveWindow and SmartDashboard integrated updating.
   */
  @Override
  public void robotPeriodic() {
     // Encoders connected to CAN controller return raw counts
     int l_raw = leftMotor.getSelectedSensorPosition();
     int r_raw = rightMotor.getSelectedSensorPosition();
     // display distances on smart dashboard
     SmartDashboard.putNumber("Left Distance", raw2inches(l_raw, 360*2, 7.5));
     SmartDashboard.putNumber("Right Distance", raw2inches(r_raw, 360*2, 7.5));
     // display power settings
     SmartDashboard.putNumber("Left Power", leftPower);
     SmartDashboard.putNumber("Right Power", rightPower);
  }

  double raw2inches(int raw_count, int counts_per_rotation, double wheel_diameter) {
    return (raw_count / counts_per_rotation) * (3.141492 * wheel_diameter);
  }

  /**
   * This code runs when you enable autonomous mode
   */
  @Override
  public void autonomousInit() {
     // Reset encoder counts to 0
     leftMotor.setSelectedSensorPosition(0);
     rightMotor.setSelectedSensorPosition(0);
     // start motors turning forward at 20% power
     leftPower = 0.20;
     leftMotor.set(leftPower);
     rightPower = 0.20;
     rightMotor.set(rightPower);
     // Start the timer that determines how often control loop runs
     autoTimer.start();
     // clear flag indicating autonomous is finished
     autoDone = false;
  }

  /**
   * This function is called periodically during autonomous.
   */
  @Override
  public void autonomousPeriodic() {
    if (!autoDone) {
      // Read distance each wheel has traveled
      int l_raw = leftMotor.getSelectedSensorPosition();
      int r_raw = rightMotor.getSelectedSensorPosition();
      double l_inches = raw2inches(l_raw, 360*2, 7.5); 
      double r_inches = raw2inches(r_raw, 360*2, 7.5);
  
      // adjust motor power every 1/4 second to keep us going straight
      if (autoTimer.hasPeriodPassed(0.25)) {
        // if we're going straight, leftDistance == rightDistance
        // otherwise, compute the error and adjust the motor powers to minimize it
        double error = l_inches - r_inches;
        double kP = 0.01; // proportion of error used to correct motor power
        // adjust motor power to try to keep left/right distances same
        leftPower  -= kP*error; // positive error means left is going too fast
        rightPower += kP*error; // right motor spins opposite direction of left
        leftMotor.set(leftPower);
        rightMotor.set(rightPower);
      }

      if ((l_inches > 36.0) || (r_inches > 36.0)) {
          leftMotor.stopMotor();
          rightMotor.stopMotor();
          autoDone = true;
      }
    }
  }

  /**
   * This function is called periodically during operator control.
   */
  @Override
  public void teleopPeriodic() {
  }

  /**
   * This function is called periodically during test mode.
   */
  @Override
  public void testPeriodic() {
  }
}