package com.edinarobotics.zed.commands;

import com.edinarobotics.zed.Components;
import com.edinarobotics.zed.subsystems.Collector;
import edu.wpi.first.wpilibj.command.Command;

public class SetCollectorToLimitCommand extends Command {
    private Collector collector;
    private Collector.CollectorLiftDirection collectorLiftDirection;
    
    public SetCollectorToLimitCommand(Collector.CollectorLiftDirection collectorLiftDirection) {
        super("SetCollectorToLimit");
        this.collector = Components.getInstance().collector;
        this.collectorLiftDirection = collectorLiftDirection;
        setTimeout(2.0);
        requires(collector);
    }
    
    protected void initialize() {
    }

    protected void execute() {
        collector.setCollectorLiftDirection(collectorLiftDirection);
    }

    protected boolean isFinished() {
        boolean limitSwitchValue = false;
        if(collectorLiftDirection.equals(Collector.CollectorLiftDirection.COLLECTOR_LIFT_UP)) {
            limitSwitchValue = collector.getUpperLimitSwitch();
        }
        if(collectorLiftDirection.equals(Collector.CollectorLiftDirection.COLLECTOR_LIFT_DOWN)) {
            limitSwitchValue = collector.getLowerLimitSwitch();
        }
        return limitSwitchValue == collector.getUpperLimitSwitch() ||
                limitSwitchValue == collector.getLowerLimitSwitch() ||
                isTimedOut();
    }

    protected void end() {
        collector.setCollectorLiftDirection(Collector.CollectorLiftDirection.COLLECTOR_LIFT_STOP);
    }

    protected void interrupted() {
        end();
    }
    
}

@@ -691,24 +691,93 @@
 tialize() %7B%0A
+        collector.setCollectorLiftDirection(collectorLiftDirection);%0A
     %7D%0A%0A    p
@@ -1335,144 +1335,109 @@
 lue 
-== collector.getUpperLimitSwitch() %7C%7C%0A                limitSwitchValue == collector.getLowerLimitSwitch() %7C%7C%0A                isTimedOut(
+%7C%7C isTimedOut() %7C%7C collectorLiftDirection.equals(Collector.CollectorLiftDirection.COLLECTOR_LIFT_STOP
 );%0A 

Fix SetCollectorToLimitCommand to actually finish.