package us.mn.k12.fairmont.robotics.subsystems;

import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.Talon;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.command.Subsystem;
import us.mn.k12.fairmont.robotics.RobotMap;
import us.mn.k12.fairmont.robotics.commands.DriveWithJoysticks;

/**
 *
 * @author Teacher
 */
public class DriveTrain extends Subsystem {
    // Put methods for controlling this subsystem
    // here. Call these from Commands.

    RobotDrive drive;
    Encoder enc;
    Timer timer;


    public DriveTrain() {
        RobotDrive drive = new RobotDrive(RobotMap.frontLeftMotor, RobotMap.rearLeftMotor, RobotMap.frontRightMotor, RobotMap.rearRightMotor);

        Encoder enc = new Encoder(RobotMap.frontRightEncA, RobotMap.frontRightEncB);
        timer = new Timer();
        enc.start();
    }
    public void driveMechanum_Cartesian(double rightJoyY, double rightJoyX, double rotation) {
        drive.mecanumDrive_Cartesian((rightJoyY * -1), rotation, rightJoyX * -1, 0); // Mechanam wheel rollers should form an x when looking directaly down from the top
    }

    public void drivetest(double thing1, double thing2) {
        drive.tankDrive(thing1, thing2);
    }

    public boolean autoDrive(int dist, int speed) {
        System.out.println("going fishes");
        while (enc.get() <= dist - 1) {
//          tal.set(speed*(.1*(dist-enc.get())));
            if (enc.get() <= dist - 1) {
                driveMechanum_Cartesian(speed,0,speed);
            }
        }
        return true;
    }


    // credit to team 1160's github for pieces of this
    public void autoTimeDrive(double time, boolean run) {
        timer.start();
        timer.reset();
        while (run) {
            driveMechanum_Cartesian(.5,0,.5);
            System.out.println(timer.get());
            if (timer.get() >= time) {
                driveMechanum_Cartesian(0,0,0);
                run = false;
            }
        }
    }


    public void initDefaultCommand() {
        // Set the default command for a subsystem here.
        setDefaultCommand(new DriveWithJoysticks());
    }


}