package frc.robot.subsystems;

import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants;
import frc.robot.commands.IntakeIntermediate;
import frc.robot.util.MechState;

public class MainMech extends SubsystemBase 
{
    public final Arm m_arm = new Arm();
    public final Chain m_chain = new Chain();

    public enum MechCmd {
        INTAKE,
        SPEAKER,
        AMP,
        DEFAULT,
        MANUAL
    }

    private double deadzone(double low, double high, double val)
    {
        if(val > low && val < high) return 0.0;
        return ((val > 0.0) ? val - high : val - low);
    }
    public MechCmd current_state = MechCmd.SPEAKER;

    public Command m_manual_cmd = this.run(() -> {
       
    });

    public MechState states[] = {
        new MechState(0.8, -48),
        new MechState(0, -42),
        new MechState(0.95, 54.5),
        new MechState(0.5, -20),
        new MechState(0.5, 20)
    };

    public MainMech() { }

    public void setDefaultCommands(Command arm_command, Command chain_command) 
    {
        m_arm.setDefaultCommand(arm_command);
        m_chain.setDefaultCommand(chain_command);
    }

    public Command command(MechState state) {
        
        Command c = Commands.parallel(  
            m_arm.run(() -> m_arm.atAngleDegrees(state.m_arm_pos)), 
            m_chain.run(() -> m_chain.atPercentage(state.m_chain_pos)));
        c.addRequirements(this);
        return c;
    }

    public Command toState(MechCmd state) 
    {
        MechState next = states[state.ordinal()];
        if (next.isValid()) {
            Command next_cmd = command(next);
            return next_cmd;
        }
        return Commands.none();
    }

    @Override
    public void periodic() { }
}