#include <cstring>
#include "goal_manager.hpp"

GoalManager::GoalManager()
    : startGoalIndex_(0), endGoalIndex_(0), goalFullFlag_(0), currentGoalRunOffFlag_(0)
{
    memset(nowJointsGoalList_, 0, sizeof(nowJointsGoalList_));
    memset(goalIndexList_, 0, sizeof(goalIndexList_));
}

GoalManager::~GoalManager() {}

int GoalManager::AddGoal(const float points[GOAL_DATA_LENGTH], const int64_t &index, int type)
{
    if (goalFullFlag_ == 1) {
        return -1;
    }
    memcpy(nowJointsGoalList_[endGoalIndex_], points, sizeof(float) * GOAL_DATA_LENGTH);
    goalIndex[index] = endGoalIndex_;
    goalIndexList_[endGoalIndex_] = index;
    goalResultList_[endGoalIndex_] = 0;
    goalTypeList_[endGoalIndex_] = type;
    endGoalIndex_++;
    if (endGoalIndex_ >= MAX_ARM_GOAL_COUNT) {
        endGoalIndex_ = 0;
    }
    if (endGoalIndex_ == startGoalIndex_) {
        goalFullFlag_ = 1;
    }
    return 0;
}

int GoalManager::ConsumeGoal()
{
    if (startGoalIndex_ == endGoalIndex_ && goalFullFlag_ == 0) {
        return -1;
    }
    goalIndex.erase(goalIndexList_[startGoalIndex_]);
    startGoalIndex_++;
    if (startGoalIndex_ >= MAX_ARM_GOAL_COUNT) {
        startGoalIndex_ = 0;
    }
    currentGoalRunOffFlag_ = 0;
    goalFullFlag_ = 0;
    return 0;
}

const float* GoalManager::GetGoal() const
{
    if (startGoalIndex_ == endGoalIndex_ && goalFullFlag_ == 0) {
        return nullptr;
    }
    return nowJointsGoalList_[startGoalIndex_];
}

int GoalManager::RemoveGoal(int64_t index)
{
    auto it = goalIndex.find(index);
    if (it == goalIndex.end()) {
        return -1;
    }
    int delIndex = it->second;
    goalResultList_[delIndex] = ARM_GOAL_CANCELLED;
    return 0;
}

void GoalManager::ClearGoals()
{
    startGoalIndex_ = 0;
    endGoalIndex_ = 0;
    goalFullFlag_ = 0;
    goalIndex.clear();
}

int GoalManager::SetNowGoalResult(int result)
{
    if (startGoalIndex_ == endGoalIndex_ && goalFullFlag_ == 0) {
        return -1;
    }
    goalResultList_[startGoalIndex_] = result;
    if (goalTypeList_[startGoalIndex_] == GOAL_TYPE_POSE_STEPPING && result != 0) {
        goalTypeList_[startGoalIndex_] = GOAL_TYPE_MOVING;
        goalResultList_[startGoalIndex_] = 0;
    }
    return 0;
}

int GoalManager::GetNowGoalResult() const
{
    if (startGoalIndex_ == endGoalIndex_ && goalFullFlag_ == 0) {
        return -1;
    }
    return goalResultList_[startGoalIndex_];
}

int64_t GoalManager::GetGoalNowIndex()
{
    if (startGoalIndex_ == endGoalIndex_ && goalFullFlag_ == 0) {
        return -1;
    }
    return goalIndexList_[startGoalIndex_];
}

int64_t GoalManager::GetCompeletedGoalNowIndex()
{
    if ((startGoalIndex_ == endGoalIndex_ && goalFullFlag_ == 0) || currentGoalRunOffFlag_ == 0) {
        return -1;
    }
    return goalIndexList_[startGoalIndex_];
}

int GoalManager::GetGoalType() const
{
    if (startGoalIndex_ == endGoalIndex_ && goalFullFlag_ == 0) {
        return -1;
    }
    return goalTypeList_[startGoalIndex_];
}

void GoalManager::GoalRunOff()
{
    currentGoalRunOffFlag_ = 1;
}

int GoalManager::GetRunOffFlag() const
{
    return currentGoalRunOffFlag_;
}
