<commit_before>package com.team254.frc2015.behavior;

import com.team254.frc2015.Constants;
import com.team254.frc2015.ElevatorSafety;
import com.team254.frc2015.HardwareAdaptor;
import com.team254.frc2015.behavior.routines.*;
import com.team254.frc2015.subsystems.BottomCarriage;
import com.team254.frc2015.subsystems.Drive;
import com.team254.frc2015.subsystems.Intake;
import com.team254.frc2015.subsystems.TopCarriage;
import com.team254.lib.util.StateHolder;
import com.team254.lib.util.Tappable;

import java.util.Optional;

public class BehaviorManager implements Tappable {

    public boolean isZero(double val) {
        return val < 0.0001 && val > -0.0001;
    }
    protected Drive drive = HardwareAdaptor.kDrive;
    protected TopCarriage top_carriage = HardwareAdaptor.kTopCarriage;
    protected BottomCarriage bottom_carriage = HardwareAdaptor.kBottomCarriage;
    protected Intake intake = HardwareAdaptor.kIntake;

    private ElevatorSafety.Setpoints m_elevator_setpoints = new ElevatorSafety.Setpoints();
    private boolean m_top_jogging = false;
    private boolean m_bottom_jogging = false;


    private Routine m_cur_routine = null;
    private RobotSetpoints m_setpoints;
    private ManualRoutine m_manual_routine = new ManualRoutine();


    public static SimplePresetRoutine rammingModePresetRoutine = new SimplePresetRoutine() {
        @Override
        public String getName() {
            return "Ramming";
        }

        @Override
        public void setPresets() {
            m_bottom_height_setpoint = Optional.of(0.0);
            m_top_height_setpoint = Optional.of(6.0);
            m_preset_setpoints.intake_action = RobotSetpoints.IntakeAction.OPEN;
        }
    };


    /*public static SimplePresetRoutine coopPresetRoutine = new SimplePresetRoutine() {
        @Override
        public void setPresets() {
            m_bottom_height_setpoint = Optional.of(40.0);
            m_top_height_setpoint = Optional.of(Constants.kTopCarriageMaxPositionInches);
            m_preset_setpoints.roller_action = RobotSetpoints.RollerAction.EXHAUST;
            m_preset_setpoints.intake_action = RobotSetpoints.IntakeAction.OPEN;
        }
    };*/

    private void setNewRoutine(Routine new_routine) {
        boolean needs_cancel = new_routine != m_cur_routine && m_cur_routine != null;

        boolean needs_reset = new_routine != m_cur_routine && new_routine != null;
        if (needs_cancel) {
            m_cur_routine.cancel();
        }
        m_cur_routine = new_routine;
        if (needs_reset) {
            m_cur_routine.reset();
        }
    }

    public void reset() {
        setNewRoutine(null);
    }

    public BehaviorManager() {
        m_setpoints = new RobotSetpoints();
        m_setpoints.reset();
    }
    public void update(Commands commands) {
        m_setpoints.reset();


        if (m_cur_routine != null && m_cur_routine.isFinished()) {
            setNewRoutine(null);
        }

        if (commands.cancel_current_routine) {
            setNewRoutine(null);
        } else if (commands.can_grabber_request == Commands.CanGrabberRequests.DO_STAGE && !(m_cur_routine instanceof CanGrabRoutine)) {
            setNewRoutine(new CanGrabRoutine());
        } else if (commands.human_player_mode && !(m_cur_routine instanceof HumanLoadRoutine)) {
            setNewRoutine(new HumanLoadRoutine());
        } else if (!commands.human_player_mode && m_cur_routine instanceof HumanLoadRoutine) {
            setNewRoutine(null);
        } else if (commands.preset_request == Commands.PresetRequest.RAMMING) {
            setNewRoutine(rammingModePresetRoutine);
        } else if (commands.preset_request == Commands.PresetRequest.COOP && !(m_cur_routine instanceof CoopRoutine)) {
            setNewRoutine(new CoopRoutine());
        }

        if (m_cur_routine != null) {
            m_setpoints = m_cur_routine.update(commands, m_setpoints);
        }

        // Get manual m_setpoints
        m_setpoints = m_manual_routine.update(commands, m_setpoints);
        
        boolean can_close_intake = true;
        boolean can_control_top_carriage_pivot = true;
        boolean can_control_top_carriage_grabber = true;
        boolean can_control_bottom_carriage = true;


        // Set elevator m_setpoints and jog
        if (bottom_carriage.isInitialized()) {
            if (m_setpoints.bottom_open_loop_jog.isPresent()) {
                double jog = m_setpoints.bottom_open_loop_jog.get();
                bottom_carriage.setOpenLoop(jog, isZero(jog));
                m_bottom_jogging = true;
            } else if (m_setpoints.m_elevator_setpoints.bottom_setpoint.isPresent()) {
                bottom_carriage.setPositionSetpoint(m_setpoints.m_elevator_setpoints.bottom_setpoint.get(), true);
                m_bottom_jogging = false;
            } else if (m_bottom_jogging && !m_setpoints.bottom_open_loop_jog.isPresent()) {
                bottom_carriage.setOpenLoop(0, true);
                m_bottom_jogging = false;
            }
        }
        if (top_carriage.isInitialized()) {
            if (m_setpoints.top_open_loop_jog.isPresent()) {
                double jog = m_setpoints.top_open_loop_jog.get();
                top_carriage.setOpenLoop(jog, isZero(jog));
                m_top_jogging = true;
            } else if (m_setpoints.m_elevator_setpoints.top_setpoint.isPresent()) {
                top_carriage.setPositionSetpoint(m_setpoints.m_elevator_setpoints.top_setpoint.get(), true);
                m_top_jogging = false;
            } else if (m_top_jogging && !m_setpoints.top_open_loop_jog.isPresent()) {
                top_carriage.setOpenLoop(0, true);
                m_top_jogging = false;
            }
            
        }

        m_elevator_setpoints = ElevatorSafety.generateSafeSetpoints(m_setpoints.m_elevator_setpoints);
        if (m_elevator_setpoints.top_setpoint.isPresent() && !m_top_jogging && !m_bottom_jogging) {
            top_carriage.setPositionSetpoint(m_elevator_setpoints.top_setpoint.get(), true);
        }
        if (m_elevator_setpoints.bottom_setpoint.isPresent() && !m_bottom_jogging && !m_bottom_jogging) {
            bottom_carriage.setPositionSetpoint(m_elevator_setpoints.bottom_setpoint.get(), true);
        }

        // can_close_intake = ElevatorSafety.canCloseIntake();

        // Top carriage actions.
        if (can_control_top_carriage_grabber
                && m_setpoints.claw_action == RobotSetpoints.TopCarriageClawAction.OPEN) {
            top_carriage.setGrabberOpen(true);<commit_msg>Give up controllers when any jog button is hit<commit_after>package com.team254.frc2015.behavior;

import com.team254.frc2015.Constants;
import com.team254.frc2015.ElevatorSafety;
import com.team254.frc2015.HardwareAdaptor;
import com.team254.frc2015.behavior.routines.*;
import com.team254.frc2015.subsystems.BottomCarriage;
import com.team254.frc2015.subsystems.Drive;
import com.team254.frc2015.subsystems.Intake;
import com.team254.frc2015.subsystems.TopCarriage;
import com.team254.lib.util.StateHolder;
import com.team254.lib.util.Tappable;

import java.util.Optional;

public class BehaviorManager implements Tappable {

    public boolean isZero(double val) {
        return val < 0.0001 && val > -0.0001;
    }
    protected Drive drive = HardwareAdaptor.kDrive;
    protected TopCarriage top_carriage = HardwareAdaptor.kTopCarriage;
    protected BottomCarriage bottom_carriage = HardwareAdaptor.kBottomCarriage;
    protected Intake intake = HardwareAdaptor.kIntake;

    private ElevatorSafety.Setpoints m_elevator_setpoints = new ElevatorSafety.Setpoints();
    private boolean m_top_jogging = false;
    private boolean m_bottom_jogging = false;


    private Routine m_cur_routine = null;
    private RobotSetpoints m_setpoints;
    private ManualRoutine m_manual_routine = new ManualRoutine();


    public static SimplePresetRoutine rammingModePresetRoutine = new SimplePresetRoutine() {
        @Override
        public String getName() {
            return "Ramming";
        }

        @Override
        public void setPresets() {
            m_bottom_height_setpoint = Optional.of(0.0);
            m_top_height_setpoint = Optional.of(6.0);
            m_preset_setpoints.intake_action = RobotSetpoints.IntakeAction.OPEN;
        }
    };


    /*public static SimplePresetRoutine coopPresetRoutine = new SimplePresetRoutine() {
        @Override
        public void setPresets() {
            m_bottom_height_setpoint = Optional.of(40.0);
            m_top_height_setpoint = Optional.of(Constants.kTopCarriageMaxPositionInches);
            m_preset_setpoints.roller_action = RobotSetpoints.RollerAction.EXHAUST;
            m_preset_setpoints.intake_action = RobotSetpoints.IntakeAction.OPEN;
        }
    };*/

    private void setNewRoutine(Routine new_routine) {
        boolean needs_cancel = new_routine != m_cur_routine && m_cur_routine != null;

        boolean needs_reset = new_routine != m_cur_routine && new_routine != null;
        if (needs_cancel) {
            m_cur_routine.cancel();
        }
        m_cur_routine = new_routine;
        if (needs_reset) {
            m_cur_routine.reset();
        }
    }

    public void reset() {
        setNewRoutine(null);
    }

    public BehaviorManager() {
        m_setpoints = new RobotSetpoints();
        m_setpoints.reset();
    }
    public void update(Commands commands) {
        m_setpoints.reset();


        if (m_cur_routine != null && m_cur_routine.isFinished()) {
            setNewRoutine(null);
        }

        if (commands.cancel_current_routine) {
            setNewRoutine(null);
        } else if (commands.can_grabber_request == Commands.CanGrabberRequests.DO_STAGE && !(m_cur_routine instanceof CanGrabRoutine)) {
            setNewRoutine(new CanGrabRoutine());
        } else if (commands.human_player_mode && !(m_cur_routine instanceof HumanLoadRoutine)) {
            setNewRoutine(new HumanLoadRoutine());
        } else if (!commands.human_player_mode && m_cur_routine instanceof HumanLoadRoutine) {
            setNewRoutine(null);
        } else if (commands.preset_request == Commands.PresetRequest.RAMMING) {
            setNewRoutine(rammingModePresetRoutine);
        } else if (commands.preset_request == Commands.PresetRequest.COOP && !(m_cur_routine instanceof CoopRoutine)) {
            setNewRoutine(new CoopRoutine());
        }

        if (m_cur_routine != null) {
            m_setpoints = m_cur_routine.update(commands, m_setpoints);
        }

        // Get manual m_setpoints
        m_setpoints = m_manual_routine.update(commands, m_setpoints);
        
        boolean can_close_intake = true;
        boolean can_control_top_carriage_pivot = true;
        boolean can_control_top_carriage_grabber = true;
        boolean can_control_bottom_carriage = true;


        double bottom_jog_speed = 0.0;
        double top_jog_speed = 0.0;
        // Set elevator m_setpoints and jog
        if (bottom_carriage.isInitialized()) {
            if (m_setpoints.bottom_open_loop_jog.isPresent()) {
                bottom_jog_speed = m_setpoints.bottom_open_loop_jog.get();
                m_bottom_jogging = true;
            } else if (m_setpoints.m_elevator_setpoints.bottom_setpoint.isPresent()) {
                bottom_carriage.setPositionSetpoint(m_setpoints.m_elevator_setpoints.bottom_setpoint.get(), true);
                m_bottom_jogging = false;
            } else if (m_bottom_jogging && !m_setpoints.bottom_open_loop_jog.isPresent()) {
                bottom_carriage.setOpenLoop(0, true);
                m_bottom_jogging = false;
            }
        }
        if (top_carriage.isInitialized()) {
            if (m_setpoints.top_open_loop_jog.isPresent()) {
                top_jog_speed = m_setpoints.top_open_loop_jog.get();
                m_top_jogging = true;
            } else if (m_setpoints.m_elevator_setpoints.top_setpoint.isPresent()) {
                top_carriage.setPositionSetpoint(m_setpoints.m_elevator_setpoints.top_setpoint.get(), true);
                m_top_jogging = false;
            } else if (m_top_jogging && !m_setpoints.top_open_loop_jog.isPresent()) {
                top_carriage.setOpenLoop(0, true);
                m_top_jogging = false;
            }
            
        }

        if (m_bottom_jogging || m_top_jogging) {
            bottom_carriage.setOpenLoop(bottom_jog_speed, isZero(bottom_jog_speed));
            top_carriage.setOpenLoop(top_jog_speed, isZero(top_jog_speed));
        }

        m_elevator_setpoints = ElevatorSafety.generateSafeSetpoints(m_setpoints.m_elevator_setpoints);
        if (m_elevator_setpoints.top_setpoint.isPresent() && !m_top_jogging && !m_bottom_jogging) {
            top_carriage.setPositionSetpoint(m_elevator_setpoints.top_setpoint.get(), true);
        }
        if (m_elevator_setpoints.bottom_setpoint.isPresent() && !m_bottom_jogging && !m_bottom_jogging) {
            bottom_carriage.setPositionSetpoint(m_elevator_setpoints.bottom_setpoint.get(), true);
        }

        // can_close_intake = ElevatorSafety.canCloseIntake();

        // Top carriage actions.
        if (can_control_top_carriage_grabber
                && m_setpoints.claw_action == RobotSetpoints.TopCarriageClawAction.OPEN) {
            top_carriage.setGrabberOpen(true);
{'fraction_non_alphanumeric': 0.08780999475537574, 'fraction_numerical': 0.013486176668914362, 'mean_word_length': 2.8224513172966783, 'pattern_counts': {'":': 0, '<': 5, '<?xml version=': 0, '>': 5, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '5708534', 'n_tokens_mistral': 4440, 'n_tokens_neox': 4208, 'n_words': 632}