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() {
    }

    protected void interrupted() {
    }
    
}

@@ -1431,24 +1431,123 @@
 oid end() %7B%0A
+        collector.setCollectorLiftDirection(Collector.CollectorLiftDirection.COLLECTOR_LIFT_STOP);%0A
     %7D%0A%0A    p
@@ -1572,24 +1572,39 @@
 rrupted() %7B%0A
+        end();%0A
     %7D%0A    %0A%7D

Add collector lift stop instruction to SetCollectorToLimitCommand.