#pragma once

#include <vector>
#include "api.h"
//#include "main.h"
#include "pros/adi.hpp"
#include "pros/rotation.hpp"

namespace ez{
    /**
     * Enumeration to hold the current part of a match were in.
     */
    enum class ControlPeriodR {
        driver,
        auton
    };
    /**
     * Enumeration to hold the different controllers avalable. 
     */
    enum class ControllerR {
        master,
        slave
    };
    /**
     * Enumeration to hold the different buttons to use.
     */
    enum class ButtonsR {
        arrow_buttons,
        Y_B_buttons,
        left_shoulder_buttons,
        right_shoulder_buttons
    };

    /**
     * Arm mechanism class.
     * 
     * Sets up all constructors, variables, and functions.
     */
    class ArmMech{
        private:
        // Arrays to hold different values for the arm.
        std::vector<int> levelOneArmStates;
        std::vector<int> levelTwoArmStates;
        std::vector<int> levelThreeArmStates;
        std::vector<int> levelFourArmStates;
        int numArmStates = 2;
        float armKP;

        public:
        std::vector<int> selectedStates; // Array for the arm to actaully use.
        int targetPos; // Value to be changed based on the height of the lift
        int currArmState;
        int armTarget;
        float armError;
        float armVelocity;
        
        ArmMech(); // Defualt constructor

        void arm_driver_control(enum ControllerR, enum ButtonsR);
        void arm_auton_control(int pickState);
        void cycle_states();
        void reset_state();
        void arm_control(); // Computes the velocity for the arm to move at.

        void set_arm_kp(float _armKP);
        void set_state_values(std::vector<int> _levelOneArmStates, std::vector<int> _levelTwoArmStates, std::vector<int> _levelThreeArmStates, std::vector<int> _levelFourArmStates);

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

    /**
     * List mechanism class.
     * 
     * Sets up all constructors, variables, and functions.
     */
    class LiftMech {
        private:
        std::vector<int> liftStates; // Array the lift uses to move.
        std::vector<int> firstRange;
        std::vector<int> secondRange; 
        std::vector<int> thirdRange; 
        std::vector<int> fourthRange;
        int numLiftStates;
        float liftKP;

        public:
        int liftHeight; // Current height of the lift in centdegrees
        int currLiftState;
        int liftTarget;
        float liftError;
        float liftVelocity;
    
        LiftMech(); // Default constructor
        LiftMech(std::initializer_list<int> _liftStates, int _numLiftStates); // Constructor to initialze array and amount of values in array.

        void lift_driver_control(enum ControllerR, enum ButtonsR);
        void lift_auton_control(int pickState);
        void cycle_states();
        void reset_state();
        void lift_control(); // Computes the velocity for the lift to move at.  
        int lift_range_finder(); // Finds the ranges for the arm to use 

        void set_lift_kp(float _liftKP);
        void set_ranges(std::vector<int> _firstRange, std::vector<int> _secondRange, std::vector<int> _thirdRange, std::vector<int> _fourthRange);

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

    /**
     * Wrist mechanism class.
     * 
     * Sets up all constructors, variables, and functions.
     */
    class WristMech {
        private:
        std::vector<int> wristStates; // Array the wrist uses to move
        int numWristStates;
        float wristKP;

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

        WristMech(); // Default constuctor
        WristMech(std::initializer_list<int> _wristStates, int _numWristStates); // Constructor to initialze array and amount of values in array.

        void wrist_driver_control(enum ControllerR, enum ButtonsR);

        void rotate_wrist(int wristThreshold); // Rotates the wrist based where the arm is.
        void reset_state();
        void wrist_control();

        int get_wrist_state();
        int get_num_wrist_states();
        float get_wrist_kp();

        void set_wrist_kp(float _wristKP);
    };
};