#ifndef GOAL_MANAGER_HPP
#define GOAL_MANAGER_HPP

#include <cstdlib>
#include <unordered_map>
#include "arm_define.h"

class GoalManager
{
public:
    GoalManager();
    ~GoalManager();

    int AddGoal(const float points[GOAL_DATA_LENGTH], const int64_t &index, int type);
    int ConsumeGoal();
    int RemoveGoal(int64_t index);
    int SetNowGoalResult(int result);
    int GetNowGoalResult() const;
    int GetGoalType() const;
    int64_t GetGoalNowIndex();
    int64_t GetCompeletedGoalNowIndex();
    void ClearGoals();
    const float* GetGoal() const;
    void GoalRunOff();
    int GetRunOffFlag() const;
private:
    float nowJointsGoalList_[MAX_ARM_GOAL_COUNT][GOAL_DATA_LENGTH];
    int goalResultList_[MAX_ARM_GOAL_COUNT];
    int goalTypeList_[MAX_ARM_GOAL_COUNT];
    int64_t goalIndexList_[MAX_ARM_GOAL_COUNT];
    int startGoalIndex_;
    int endGoalIndex_;
    int goalFullFlag_;
    int currentGoalRunOffFlag_;

    std::unordered_map<int64_t, int> goalIndex{MAX_ARM_GOAL_COUNT};
};

#endif