y2024/control_loops/superstructure/superstructure_goal.fbs

include "frc971/control_loops/profiled_subsystem.fbs";
include "frc971/control_loops/catapult/catapult_goal.fbs";

namespace y2024.control_loops.superstructure;

// Represents goal for the intake pivot and rollers
// INTAKE will extend the pivot and turn on the rollers to intake the note.
// SPIT will extend the pivot and turn on the rollers (in reverse) to spit out the note.
enum IntakeGoal : ubyte {
    NONE = 0,
    INTAKE = 1,
    SPIT = 2,
}

// Represents goal for climber
// FULL_EXTEND is for fully extending the climber
// HALF_EXTEND is for partially extending the climber
// RETRACT is for retracting the climber
enum ClimberGoal : ubyte {
    FULL_EXTEND = 0,
    HALF_EXTEND = 1,
    RETRACT = 2,
}

table ShooterGoal {
    catapult_goal:frc971.control_loops.catapult.CatapultGoal (id: 0);
    auto_aim: bool (id: 1);

    // Position for the turret when we aren't auto aiming
    turret_position: frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal (id: 2);

    // Position for the altitude when we aren't auto aiming
    altitude_position: frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal (id: 3);

     // If true, we started with the ball loaded and should proceed to that state.
    preloaded:bool = false (id: 4);
}

// Represents goal for the note movement through the robot 
// to various scoring positions
// NONE represents no goal for the note
// AMP represents the goal to move the note and the extend to the AMP scoring position
// TRAP represents the goal to move the note and the extend to the TRAP scoring position
// CATAPULT represents the goal to load the note in the catapult.
// It will complete the catapult goal before accepting new goals.
enum NoteGoal : ubyte {
    NONE = 0,
    AMP = 1,
    TRAP = 2,
    CATAPULT = 3,
}


table Goal {
    intake_goal:IntakeGoal = NONE (id: 0);
    catapult_goal:frc971.control_loops.catapult.CatapultGoal (id: 1);
    climber_goal:ClimberGoal (id: 2);
    shooter_goal:ShooterGoal (id: 3);
    note_goal:NoteGoal (id: 4);
    fire: bool (id: 5);
}
root_type Goal;