package org.usfirst.frc.team4272.robotlib;

import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.PIDOutput;

public class LimitSwitchedPIDOutput implements PIDOutput {
	private final PIDOutput out;
	private final DigitalInput forward;
	private final DigitalInput backward;
	private final boolean i_dont_know_which_way_is_which;
	
	public LimitSwitchedPIDOutput(PIDOutput out, DigitalInput forward, DigitalInput backward, boolean i_dont_know_which_way_is_which) {
		this.out = out;
		this.forward = forward;
		this.backward = backward;
		this.i_dont_know_which_way_is_which = i_dont_know_which_way_is_which;
	}
	
	public LimitSwitchedPIDOutput(PIDOutput out, DigitalInput forward, DigitalInput backward) {
		this(out, forward,backward, false);
	}
	
	public void pidWrite(double v) {
		if (i_dont_know_which_way_is_which) {
			if (forward.get() || backward.get()) {
				v = 0;
			}
		} else {
			if (forward.get()) {
				v = Math.min(v, 0);
			}
			if (backward.get()) {
				v = Math.max(v, 0);
			}
		}
		out.pidWrite(v);
	}
}