package com.edinarobotics.zed.subsystems;

import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.RobotDrive;

public class Drivetrain {
    private final double DISTANCE_PER_PULSE = 1;
    private RobotDrive robotDrive;
    private double magnitude;
    private double direction;
    private double rotation;
    
    private Encoder frontLeftEncoder;
    private Encoder frontRightEncoder;
    private Encoder backLeftEncoder;
    private Encoder backRightEncoder;
    
    public Drivetrain(int frontLeft, int frontRight, int backLeft, int backRight,
            int frontLeftA, int frontLeftB, int frontRightA, int frontRightB,
            int backLeftA, int backLeftB, int backRightA, int backRightB) {
        this.robotDrive = new RobotDrive(frontLeft, backLeft, frontRight, backRight);
        robotDrive.setInvertedMotor(RobotDrive.MotorType.kFrontRight, true);
        robotDrive.setInvertedMotor(RobotDrive.MotorType.kFrontLeft, false);
        robotDrive.setInvertedMotor(RobotDrive.MotorType.kRearRight, true);
        robotDrive.setInvertedMotor(RobotDrive.MotorType.kRearLeft, false);
        
        frontLeftEncoder = new Encoder(frontLeftA, frontLeftB);
        frontRightEncoder = new Encoder(frontRightA, frontRightB);
        backLeftEncoder = new Encoder(backLeftA, backLeftB);
        backRightEncoder = new Encoder(backRightA, backRightB);
        frontLeftEncoder.setDistancePerPulse(DISTANCE_PER_PULSE);
        frontRightEncoder.setDistancePerPulse(DISTANCE_PER_PULSE);
        backLeftEncoder.setDistancePerPulse(DISTANCE_PER_PULSE);
        backRightEncoder.setDistancePerPulse(DISTANCE_PER_PULSE);
        frontLeftEncoder.start();
        frontRightEncoder.start();
        backLeftEncoder.start();
        backRightEncoder.start();
    }
    
    public void mecanumPolarStrafe(double magnitude, double direction){
        this.magnitude = magnitude;
        this.direction = direction;
        update();
    }
    
    public void mecanumPolarRotation(double rotation){
        this.rotation = rotation;
        update();
    }
    
    public void update(){
        robotDrive.mecanumDrive_Polar(magnitude, direction, rotation);
    }
    
    public double getFrontLeftVelocity() {
        return frontLeftEncoder.getRate();
    }
    
    public double getFrontRightVelocity() {
        return frontRightEncoder.getRate();
    }
    
    public double getBackLeftVelocity() {
        return backLeftEncoder.getRate();
    }
    
    public double getBackRightVelocity() {
        return backRightEncoder.getRate();
    }
    
    public double getAverageVelocity() {
        return 0.5 * ((getFrontLeftVelocity() + getFrontRightVelocity() +
                getBackLeftVelocity() + getBackRightVelocity()) / 4);
    }
}

@@ -340,32 +340,24 @@
 Encoder 
-frontLeftE
+e
 ncoder
+1
 ;%0A    pr
@@ -374,100 +374,16 @@
 der 
-frontRightEncoder;%0A    private Encoder backLeftEncoder;%0A    private Encoder backRightEncoder
+encoder2
 ;%0A  
@@ -487,141 +487,61 @@
 int 
-frontLeftA, int frontLeftB, int frontRightA, int frontRightB,%0A            int backLeftA, int backLeftB, int backRightA, int backRight
+encoder1A, int encoder1B, int encoder2A, int encoder2
 B) %7B
@@ -950,32 +950,24 @@
         
-frontLeftE
+e
 ncoder
+1
  = new E
@@ -977,96 +977,27 @@
 der(
-frontLeftA, frontLeftB);%0A        frontRightEncoder = new Encoder(frontRightA, frontRight
+encoder1A, encoder1
 B);%0A
@@ -1004,31 +1004,24 @@
         
-backLeftE
+e
 ncoder
+2
  = new E
@@ -1031,225 +1031,28 @@
 der(
-backLeftA, backLeftB);%0A        backRightEncoder = new Encoder(backRightA, backRightB);%0A        frontLeftEncoder.setDistancePerPulse(DISTANCE_PER_PULSE);%0A        frontRightEncoder.setDistancePerPulse(DISTANCE_PER_PULSE
+encoder2A, encoder2B
 );%0A 
@@ -1058,31 +1058,24 @@
         
-backLeftE
+e
 ncoder
+1
 .setDist
@@ -1116,32 +1116,24 @@
         
-backRightE
+e
 ncoder
+2
 .setDist
@@ -1178,126 +1178,42 @@
     
-frontLeftEncoder.start();%0A        frontRightEncoder.start();%0A        backLeftEncoder.start();%0A        backRightEncoder
+encoder1.start();%0A        encoder2
 .sta
@@ -1649,25 +1649,30 @@
 uble get
-FrontLeft
+AverageForward
 Velocity
@@ -1688,385 +1688,162 @@
     
-return frontLeftEncoder.getRate();
+/*
 %0A    
-%7D%0A
+ 
     
-%0A    public double getFrontRightVelocity() %7B%0A        return frontRightEncoder.getRate();%0A    %7D%0A    %0A    public double getBackLeftVelocity() %7B%0A        return backLeftEncoder.getRate();
+* The inner 0.5 multiplier is used because mecanum drive only gives
 %0A    
-%7D%0A
     
-%0A    public double getBackRightVelocity() %7B%0A        return backRightEncoder.getRate();
+ * a forward velocity of half the wheel's actual rate.
 %0A    
-%7D%0A
     
-%0A    public double getAverageVelocity() %7B
+ */
 %0A   
@@ -1858,135 +1858,65 @@
 urn 
+((
 0.5
- * ((getFrontLeftVelocity() + getFrontRightVelocity() +%0A                getBackLeftVelocity() + getBackRightVelocity()) / 4)
+*encoder1.getRate()) + (0.5*encoder2.getRate()))/2.0
 ;%0A  

Modify Drivetrain subsystem encoder code to reflect the two encoders.