#include "rm_arm_handler.hpp"

const float maxJointSpeedList[USED_ARM_DOF] = {4.0, 4.0, 4.0, 4.0, 4.0, 4.0};

RmArmHandler::RmArmHandler(ArmDriver *driver, int armType, sem_t *sem, KinematicsManager *trapezoidalTrajectory):
    armType_(armType), driver_(driver), trapezoidalTrajectory_(trapezoidalTrajectory)   
{
    goalManager_ = new GoalManager();
    armStatus = ARM_STATUS_ERROR;
    sem_ = sem;
    memset(nowJoints_, 0, sizeof(nowJoints_));
    memset(nextJoints_, 0, sizeof(nextJoints_));

    armStatus = ARM_STATUS_READY;
}

RmArmHandler::~RmArmHandler()
{
    if (goalManager_ != NULL) {
        delete goalManager_;
        goalManager_ = NULL;
    }
    if (trapezoidalTrajectory_ != NULL) {
        delete trapezoidalTrajectory_;
        trapezoidalTrajectory_ = NULL;
    }
}

void PrintData(ArmStatusCallbackData *data)
{
    /*RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "==================MYCALLBACK==================");
    RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "errCode: %d", data->errCode);
    RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "current:");
    for (size_t i = 0; i < USED_ARM_DOF; i++){
        current += std::to_string(data->jointCurrent[i]) + " ";
        //RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%.6f ", data->jointCurrent[i]);
    }
    RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%s", current.c_str());
    
    /*RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "enable:");
    for (size_t i = 0; i < USED_ARM_DOF; i++){
        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%d ", data->jointEnFlag[i]);
    }
    
    RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "error code:");
    for (size_t i = 0; i < USED_ARM_DOF; i++){
        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%d ", data->jointErrCode[i]);
    }
    
    std::string posStr = "joints position: ";
    for (size_t i = 0; i < USED_ARM_DOF; i++){
        posStr += std::to_string(data->jointAngle[i]) + " ";
    }
    RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%s", posStr.c_str());
    
    RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "speed:");
    for (size_t i = 0; i < USED_ARM_DOF; i++){
        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%.6f ", data->jointSpeed[i]);
    }
    
    posStr = "temperature: ";
    //RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "temperature:");
    for (size_t i = 0; i < USED_ARM_DOF; i++){
        posStr += std::to_string(data->jointTemp[i]) + " ";
        //RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%.6f ", data->jointTemp[i]);
    }
    RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%s", posStr.c_str());
    
    posStr = "voltage: ";
    //RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "voltage:");
    for (size_t i = 0; i < USED_ARM_DOF; i++){
        //RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%.6f ", data->jointVoltage[i]);
        posStr += std::to_string(data->jointVoltage[i]) + " ";
    }
    RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%s", posStr.c_str());*/
}

void RmArmHandler::ArmDealCallBackInfo(ArmStatusCallbackData *data)
{
    ARM_STATUS_E temp = ARM_STATUS_READY;
    for (size_t i = 0; i < USED_ARM_DOF; i++)
    {
        if (fabs(nowJoints_[i] - data->jointAngle[i]) > 0.02) {
            temp = ARM_STATUS_WAITING;
        }
        nowJoints_[i] = data->jointAngle[i];
    }
    if (armStatus == ARM_STATUS_WAITING && temp == ARM_STATUS_READY) {
        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "ARM_STATUS_READY.");
        armStatus = ARM_STATUS_READY;
        for (size_t i = 0; i < USED_ARM_DOF; i++) {
            nextJoints_[i] = nowJoints_[i];
        }
        sem_post(sem_);
    } else if (armStatus == ARM_STATUS_READY && temp == ARM_STATUS_WAITING) {
        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "ARM_STATUS_WAITING.");
        armStatus = ARM_STATUS_WAITING;
    }
    if (temp != ARM_STATUS_READY){
        // if (armType_ == 1)
        //     PrintData(data);
    }
}

int RmArmHandler::AddNextPoint(float points[GOAL_DATA_LENGTH], const int64_t &index, int type)
{
    std::unique_lock lock(rw_mutex_);
    return goalManager_->AddGoal(points, index, type);
}

int RmArmHandler::AddNextPointFromPoseMoving(const float pose[POSE_DIMENSION], const int64_t &index)
{
    float q_out[GOAL_DATA_LENGTH] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
    for (int i = 0; i < USED_ARM_DOF; i++) {
        q_out[i] = nextJoints_[i];
    }
    int result = trapezoidalTrajectory_->armKinematics_->InverseKinematics(pose, q_out);
    if (result == 0) {
        std::unique_lock lock(rw_mutex_);
        return goalManager_->AddGoal(q_out, index, GOAL_TYPE_MOVING);
    }
    return result;
}

int RmArmHandler::GetNextPointFromPoseStepping(const float pose[POSE_DIMENSION], float *joints)
{
    float poseTemp[POSE_DIMENSION] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
    int result = trapezoidalTrajectory_->armKinematics_->ForwardKinematics(nextJoints_, poseTemp);
    if (result != 0) {
        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "ForwardKinematics error.");
        return UNKNOWN_ERR;
    }
    for (int i = 0; i < POSE_DIMENSION; i++) {
        poseTemp[i] = pose[i] + poseTemp[i];
        if (fabs(poseTemp[i]) < 0.0001) {
            poseTemp[i] = 0.0f;
        }
    }
    RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Current pose:%.6f %.6f %.6f %.6f %.6f %.6f %.6f", 
        poseTemp[0], poseTemp[1], poseTemp[2], poseTemp[3], poseTemp[4], poseTemp[5], poseTemp[6]);
    float q_out[GOAL_DATA_LENGTH] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
    for (int i = 0; i < USED_ARM_DOF; i++) {
        q_out[i] = nextJoints_[i];
    }
    result = trapezoidalTrajectory_->armKinematics_->InverseKinematics(poseTemp, q_out);
    if (result != 0) {
        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "InverseKinematics error.");
        return ARM_AIM_CANNOT_REACH;
    }
    RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Next joints:%.6f %.6f %.6f %.6f %.6f %.6f", 
        q_out[0], q_out[1], q_out[2], q_out[3], q_out[4], q_out[5]);
    for (int i = 0; i < USED_ARM_DOF; i++) {
        joints[i] = q_out[i];
    }
    return 0;
}

int RmArmHandler::AddNextPointFromPose(const float pose[POSE_DIMENSION], const int64_t &index, int type)
{
    if (type == GOAL_TYPE_MOVING) {
        return AddNextPointFromPoseMoving(pose, index);
    } else if (type == GOAL_TYPE_STEPPING) {
        std::unique_lock lock(rw_mutex_);
        return goalManager_->AddGoal(pose, index, GOAL_TYPE_POSE_STEPPING);
    }
    return -1;
}

int RmArmHandler::DeletePoint(int64_t index)
{
    int nowIndex;
    {
        std::shared_lock lock(rw_mutex_);
        nowIndex = goalManager_->GetGoalNowIndex();
    }
    if (index == nowIndex) {
        ArmStop(0);
        /*Trajectory* nowTrajectory = trapezoidalTrajectory_->getNowTrajectory();
        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "DeletePoint: stop trajectory. accs:%d, unis:%d, curs:%d",
            nowTrajectory->accelerate_time_step, nowTrajectory->uniform_time_step, nowTrajectory->current_time_step);*/
        return 0;
    }
    std::unique_lock lock(rw_mutex_);
    return goalManager_->RemoveGoal(index);
}

void RmArmHandler::ArmRun(int64_t timeStamp)
{
    if (armStatus != ARM_STATUS_MOVING) {
        return;
    }
    int result, type;
    type = goalManager_->GetGoalType();
    result = trapezoidalTrajectory_->updateTrajectory(timeStamp, nextJoints_);

    if (driver_->ArmMove(nextJoints_, armType_) != 0) {
        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Error moving to joint space! ");
        return;
    }

    if (result == TRAJECTORY_COMPLETE) {
        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Trajectory complete.");
        if (type == GOAL_TYPE_POSE_STEPPING) {
            const float* pose = goalManager_->GetGoal();
            result = trapezoidalTrajectory_->continuePoseSteppingTrajectory(nextJoints_, pose);
            if (result != 0) {
                RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "GetNextPointFromPoseStepping failed.");
                ArmStop(ARM_AIM_CANNOT_REACH);
            }
            /*Trajectory* nowTrajectory = trapezoidalTrajectory_->getNowTrajectory();
            RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Pose stepping continue. accs:%d, unis:%d, curs:%d, vels:%f, %f, %f, %f, %f, %f",
                nowTrajectory->accelerate_time_step, nowTrajectory->uniform_time_step, nowTrajectory->current_time_step,
                nowTrajectory->aim_velocity[0], nowTrajectory->aim_velocity[1], nowTrajectory->aim_velocity[2],
                nowTrajectory->aim_velocity[3], nowTrajectory->aim_velocity[4], nowTrajectory->aim_velocity[5]);*/
        } else {
            int runOffFlag = goalManager_->GetRunOffFlag();
            if (runOffFlag == 0) {
                std::unique_lock lock(rw_mutex_);
                goalManager_->GoalRunOff();
                armStatus = ARM_STATUS_WAITING;
            }
        }
    }
}

int RmArmHandler::StartNewTrajectory()
{
    std::shared_lock lock(rw_mutex_);
    const float* goal = goalManager_->GetGoal();
    if (goal == NULL) {
        return ARM_NOW_NO_GOAL;
    }
    if (goalManager_->GetNowGoalResult() != 0) {
        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Failed to start new trajectory because canceled. GoalRunOff1.");
        goalManager_->GoalRunOff();
        return ARM_GOAL_CANCELLED;
    }
    float start[USED_ARM_DOF];
    for (int i = 0; i < USED_ARM_DOF; i++) {
        start[i] = nextJoints_[i];
    }
    int goalType = goalManager_->GetGoalType();
    int result = trapezoidalTrajectory_->computeTrajectory(start, goal, goalType);
    if (result != 0) {
        goalManager_->GoalRunOff();
    }
    /*Trajectory* nowTrajectory = trapezoidalTrajectory_->getNowTrajectory();
    RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Pose stepping continue. accs:%d, unis:%d, vels:%f, %f, %f, %f, %f, %f",
        nowTrajectory->accelerate_time_step, nowTrajectory->uniform_time_step,
        nowTrajectory->aim_velocity[0], nowTrajectory->aim_velocity[1], nowTrajectory->aim_velocity[2],
        nowTrajectory->aim_velocity[3], nowTrajectory->aim_velocity[4], nowTrajectory->aim_velocity[5]);*/
    return result;
}

void RmArmHandler::ArmMove()
{
    armStatus = ARM_STATUS_MOVING;
}

void RmArmHandler::ArmStop(int result)
{
    std::unique_lock lock(rw_mutex_);
    trapezoidalTrajectory_->stopTrajectory();
    goalManager_->SetNowGoalResult(result);
}

void RmArmHandler::ArmStopStart(int result)
{
    std::unique_lock lock(rw_mutex_);
    trapezoidalTrajectory_->stopTrajectory();
    goalManager_->SetNowGoalResult(result);
    goalManager_->GoalRunOff();
}

int RmArmHandler::GoalReached()
{
    std::unique_lock lock(rw_mutex_);
    RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "GoalReached, locked.");
    int result = goalManager_->GetNowGoalResult();
    goalManager_->ConsumeGoal();
    return result;
}

int64_t RmArmHandler::GetCurrentRunOffGoalIndex()
{
    std::shared_lock lock(rw_mutex_);
    return goalManager_->GetCompeletedGoalNowIndex();
}

int64_t RmArmHandler::GetCurrentGoalIndex()
{
    std::shared_lock lock(rw_mutex_);
    return goalManager_->GetGoalNowIndex();
}