package org.usfirst.frc4904.standard.custom;


import edu.wpi.first.wpilibj.PIDOutput;

public class ChassisPIDOutputWrapper implements ChassisController {
    private static class PIDSubOutput implements PIDOutput {
        private double value;

        private PIDSubOutput(double defaultValue) {
            value = defaultValue;
        }

        @Override
        public void pidWrite(double output) {
            value = output;
        }
    }
    private final PIDSubOutput x;
    private final PIDSubOutput y;
    private final PIDSubOutput turnSpeed;

    public ChassisPIDOutputWrapper(double defaultX, double defaultY, double defaultTurnSpeed) {
        x = new PIDSubOutput(defaultX);
        y = new PIDSubOutput(defaultY);
        turnSpeed = new PIDSubOutput(defaultTurnSpeed);
    }

    @Override
    public double getX() {
        return x.value;
    }

    @Override
    public double getY() {
        return y.value;
    }

    @Override
    public double getTurnSpeed() {
        return turnSpeed.value;
    }

    public PIDOutput getXSink() {
        return x;
    }

    public PIDOutput getYSink() {
        return y;
    }

    public PIDOutput getTurnSpeedSink() {
        return turnSpeed;
    }
}