#ifndef TRAJECTORY_HPP
#define TRAJECTORY_HPP

#include <vector>
#include "arm_define.h"
#include "trapezoidal_trajectory_kinematics.hpp"
#include "rm_driver_kinematics.hpp"

template <typename T, typename K>
class TrajectoryManager
{
public:
    TrajectoryManager(K *armKinematics) : armKinematics_(armKinematics) {
        nowTrajectoryIndex_ = -1;
        trajectoryFullFlag_ = 0;
    }
    TrajectoryManager(K *armKinematics, int armId) : armKinematics_(armKinematics), armId_(armId) {
        nowTrajectoryIndex_ = -1;
        trajectoryFullFlag_ = 0;
    }
    ~TrajectoryManager() {}

    int computeTrajectory(const float *start, const float *end, int type) {
        T* newTrajectory = addTrajectory();
        if (type == GOAL_TYPE_MOVING) {
            return armKinematics_->computeMovingTrajectory(newTrajectory, start, end, armId_);
        } else if (type == GOAL_TYPE_POSE_STEPPING) {
            return armKinematics_->computePoseSteppingTrajectory(newTrajectory, start, end, armId_);
        } else {
            return armKinematics_->computeSteppingTrajectory(newTrajectory, start, end, armId_);
        }
        return UNKNOWN_ERR;
    }

    int updateTrajectory(int64_t startTime, float *aimAngles) {
        if (nowTrajectoryIndex_ < 0) {
            return OK;
        }
        if (trajectoryStartTime_[nowTrajectoryIndex_] == 0) {
            trajectoryStartTime_[nowTrajectoryIndex_] = startTime;
        }
        return armKinematics_->updateTrajectory(&trajectoryHistory_[nowTrajectoryIndex_], aimAngles);
    }

    int getAngleFromTime(float *aimAngles, int64_t timeStamp) {
        if (nowTrajectoryIndex_ < 0) {
            return OK;
        }
        int timeStep = 0;
        int aimIndex = -1;
        int startIndex = nowTrajectoryIndex_ + 1;
        int endIndex = trajectoryFullFlag_ == 1 ? startIndex : 0;
        do {
            startIndex = (startIndex + MAX_TRAJECTORY_HISTORY - 1) % MAX_TRAJECTORY_HISTORY;
            if (trajectoryStartTime_[startIndex] < timeStamp) {
                aimIndex = startIndex;
                timeStep = static_cast<int>((timeStamp - trajectoryStartTime_[startIndex]) / ARM_FOLLOWING_PERIOD);
                break;
            }
        } while (startIndex != endIndex);

        if (aimIndex == -1) {
            return UNKNOWN_ERR;
        }
        armKinematics_->getAngleForCheckingInner(&trajectoryHistory_[aimIndex], timeStep, aimAngles);
        return 0;
    }

    void getAngleForChecking(float *aimAngles) {
        if (nowTrajectoryIndex_ < 0) {
            return;
        }
        T* nowTrajectory = &trajectoryHistory_[nowTrajectoryIndex_];
        armKinematics_->getAngleForCheckingOffset(nowTrajectory, ARM_FOLLOWING_CHECKING_STEP * 2, aimAngles);
    }

    void getAngleForStarting(float *aimAngles) {
        if (nowTrajectoryIndex_ < 0) {
            return;
        }
        T* nowTrajectory = &trajectoryHistory_[nowTrajectoryIndex_];
        armKinematics_->getAngleForCheckingOffset(nowTrajectory, ARM_FOLLOWING_CHECKING_STEP, aimAngles);
    }

    void stopTrajectory() {
        armKinematics_->stopTrajectory(&trajectoryHistory_[nowTrajectoryIndex_]);
    }

    int continuePoseSteppingTrajectory(const float *nowAngles, const float *direct) {
        return armKinematics_->continuePoseSteppingTrajectory(&trajectoryHistory_[nowTrajectoryIndex_], nowAngles, direct);
    }

    T* getNowTrajectory() {
        if (nowTrajectoryIndex_ < 0) {
            return nullptr;
        }
        return &trajectoryHistory_[nowTrajectoryIndex_];
    }

    K *armKinematics_;

private:
    T trajectoryHistory_[MAX_TRAJECTORY_HISTORY];
    int64_t trajectoryStartTime_[MAX_TRAJECTORY_HISTORY];
    int nowTrajectoryIndex_;
    int trajectoryFullFlag_;
    int armId_;

    T* addTrajectory() {
        nowTrajectoryIndex_++;
        if (nowTrajectoryIndex_ >= MAX_TRAJECTORY_HISTORY) {
            nowTrajectoryIndex_ = 0;
            trajectoryFullFlag_ = 1;
        }
        trajectoryStartTime_[nowTrajectoryIndex_] = 0;
        return &trajectoryHistory_[nowTrajectoryIndex_];
    }
};

#endif // TRAJECTORY_HPP