// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.

package frc.robot.subsystems;

import com.kauailabs.navx.frc.AHRS;
import com.revrobotics.CANSparkMax;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
import edu.wpi.first.wpilibj.DoubleSolenoid;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.PneumaticsModuleType;
import edu.wpi.first.wpilibj.DoubleSolenoid.Value;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants;

public class BasePilotable extends SubsystemBase {
  final CANSparkMax avantdroit = new CANSparkMax(Constants.avantdroit, MotorType.kBrushless);
  final CANSparkMax avantgauche = new CANSparkMax(Constants.avantgauche, MotorType.kBrushless);
  final CANSparkMax arrieredroit = new CANSparkMax(Constants.arrieredroit, MotorType.kBrushless);
  final CANSparkMax arrieregauche = new CANSparkMax(Constants.arrieregauche, MotorType.kBrushless);
  final MotorControllerGroup droit = new MotorControllerGroup(avantdroit, arrieredroit);
  final MotorControllerGroup gauche = new MotorControllerGroup(avantgauche, arrieregauche);
  final DifferentialDrive drive = new DifferentialDrive(gauche, droit);
  
  //piston
  private DoubleSolenoid brakedroit = new DoubleSolenoid(PneumaticsModuleType.CTREPCM,Constants.brakedroit, Constants.brakedroit);
  private DoubleSolenoid brakegauche = new DoubleSolenoid(PneumaticsModuleType.CTREPCM,Constants.brakegauche, Constants.brakegauche);
  //gyro
  ShuffleboardTab teb = Shuffleboard.getTab("teb");
  private AHRS gyroscope = new AHRS();public double getangle() {return gyroscope.getAngle();}
   public double getpitch() {
    return gyroscope.getPitch();
  }

  public void drive(double xSpeed, double zRotation){
    drive.arcadeDrive(xSpeed, zRotation);
  }
  public double distance(){
    return (-avantdroit.getEncoder().getPosition()
    +avantgauche.getEncoder().getPosition()
    -arrieredroit.getEncoder().getPosition()
    +arrieregauche.getEncoder().getPosition()) / 4;
  }
  public void Reset() {
    avantdroit.getEncoder().setPosition(0);
    avantgauche.getEncoder().setPosition(0);
    arrieredroit.getEncoder().setPosition(0);
    arrieregauche.getEncoder().setPosition(0);
  }
public void BrakeOuvre(){
  brakedroit.set(Value.kForward);
  brakegauche.set(Value.kForward);
}
public void BrakeFerme(){
  brakedroit.set(Value.kReverse);
  brakegauche.set(Value.kReverse);
}
public void resetGyro(){
  try {gyroscope.reset();} catch(Exception e){DriverStation.reportError("bye bye",true);
 }
  }
  /** Creates a new BasePilotable. */
  public BasePilotable() {
    droit.setInverted(true);
  }

  @Override
  public void periodic() {
    teb .add("encodeuravantdroit",0.1);
    teb .add("encodeurarrieregauche",0.1);
    teb .add("encodeurarrieredroit",0.1);
    teb .add("encodeuravantgauche",0.1);  
    teb .add("distance",0.1);
    teb .add("brakedroit",0.1);
    teb .add("brakegauche", 0.1);
  }
}