#pragma once

#include "api.h"
#include "pros/adi.hpp"
#include "pros/rotation.hpp"

namespace ez{

    class ArmMech{
        private:
        std::vector<int> armStates;
        int numArmStates;
        float armKP;

        public:
        int currArmState;
        int armTarget;
        float armError;
        float armVelocity;

        ArmMech();
        ArmMech(std::initializer_list<int> armStates, int numArmStates);

        void cycle_states();
        void reset_state();
        void arm_control();

        int get_arm_state();
        int get_num_arm_states();
        int get_arm_kp();
    };

    class LiftMech {
        private:
        std::vector<int> liftStates;
        int numLiftStates;
        float liftKP;

        public:
        int currLiftState;
        int liftTarget;
        float liftError;
        float liftVelocity;

        LiftMech();
        LiftMech(std::initializer_list<int> liftStates, int numLiftStates);

        void cycle_states();
        void reset_state();
        void lift_control();

        int get_lift_state();
        int get_num_lift_states();
        int get_lift_kp();
    };

    class WristMech {
        private:
        std::vector<int> wristStates;
        int numWristStates;
        float wristKP;

        public:
        int currWristState;
        int wristTarget;
        float wristError;
        float wristVelocity;

        WristMech();
        WristMech(std::initializer_list<int> wristStates, int numWristStates);

        void cycle_states();
        void reset_state();
        void wrist_control();

        int get_wrist_state();
        int get_num_wrist_states();
        int get_wrist_kp();
    };
};