/**
 * @file control_parameters.h
 * @brief basic control parameters
 * @author DeepRobotics
 * @version 1.0
 * @date 2025-11-07
 * 
 * @copyright Copyright (c) 2025  DeepRobotics
 * 
 */
#pragma once

#include "common_types.h"
#include "custom_types.h"

using namespace types;

class ControlParameters
{
private:
    void GenerateM20Parameters();
    void GenerateFireAntParameters();
public:
    ControlParameters(RobotName robot_name){
        if(robot_name==RobotName::M20) GenerateM20Parameters();
        else if(robot_name==RobotName::FireAnt) GenerateFireAntParameters();
        else{
            std::cerr << "Not Deafult Robot" << std::endl;
            exit(0);
        }
    }
    ~ControlParameters(){}

    /**
     * @brief robot link length
     */
    float body_len_x_, body_len_y_;
    float hip_len_, thigh_len_, shank_len_;

    /**
     * @brief stand height configure
     */
    float pre_height_, stand_height_; 

    /**
     * @brief one leg joint PD gain
     */
    Vec3f swing_leg_kp_, swing_leg_kd_;

    /**
     * @brief joint position limitation
     */
    Vec3f fl_joint_lower_, fl_joint_upper_;

    /**
     * @brief joint velocity limitation
     */
    Vec3f joint_vel_limit_;

    /**
     * @brief joint torque limitation
     */
    Vec3f torque_limit_;
    
    /**
     * @brief stand up duration
     */
    float stand_duration_ = 1.5;

    /**
     * @brief lie down duration
     */
    float liedown_duration_ = 2.0;

    /**
     * @brief wheel vel limit for wheel-legged robot
     */
    float wheel_vel_limit_ = 100;
    
    /**
     * @brief wheel link length to shank
     */
    float wheel_link_len_ = 0.040575;

    /**
     * @brief policy path
     */
    std::string common_policy_path_;
    Vec3f common_policy_p_gain_, common_policy_d_gain_;
};