/**
 * @file data_types.h
 * @brief 定义数据结构和枚举类
 * @copyright Copyright (C) 2023 ROKAE (Beijing) Technology Co., LTD. All Rights Reserved.
 * Information in this file is the intellectual property of Rokae Technology Co., Ltd,
 * And may contains trade secrets that must be stored and viewed confidentially.
 */

#ifndef ROKAEAPI_INCLUDE_ROKAE_DATA_TYPES_H_
#define ROKAEAPI_INCLUDE_ROKAE_DATA_TYPES_H_

#if defined(_MSC_VER) && (_MSC_VER >= 1200)
#pragma once
#endif // defined(_MSC_VER) && (_MSC_VER >= 1200)

#include <array>
#include <vector>
#include <string>
#include <any>
#include "base.h"

namespace rokae {

 const int USE_DEFAULT = -1;
 const int Unknown = -1;

// *********************         Enum class          **********************
// *********************           枚举类             **********************

 /**
  * @enum OperationState
  * @brief 机器人工作状态
  */
  enum class OperationState {
    idle             = 0, ///< 机器人静止
    jog              = 1, ///< jog
    rtControlling    = 2, ///< 实时模式控制中
    drag             = 3, ///< 拖动已开启
    rlProgram        = 4, ///< RL工程运行中
    demo             = 5, ///< Demo演示中
    dynamicIdentify  = 6, ///< 动力学辨识中
    frictionIdentify = 7, ///< 摩擦力辨识中
    loadIdentify     = 8, ///< 负载辨识中
    moving           = 9, ///< 机器人运动中
    unknown          = Unknown ///< 未知
  };

 /**
  * @enum WorkType
  * @brief 机型类别
  */
 enum class WorkType {
   industrial,   ///< 工业机器人
   collaborative ///< 协作机器人
 };

 /**
  * @enum OperateMode
  * @brief 机器人操作模式
  */
 enum class OperateMode {
   manual    = 0,      ///< 手动
   automatic = 1,      ///< 自动
   unknown   = Unknown ///< 未知(发生异常)
 };

 /**
  * @enum PowerState
  * @brief 机器人上下电及急停状态
  */
 enum class PowerState {
   on      = 0, ///< 上电
   off     = 1, ///< 下电
   estop   = 2, ///< 急停被按下
   gstop   = 3, ///< 安全门打开
   unknown = Unknown ///< 未知(发生异常)
 };

 /**
  * @brief 位姿坐标系类型
  */
 enum class CoordinateType {
   flangeInBase, ///< 法兰相对于基坐标系
   endInRef      ///< 末端相对于外部坐标系
 };

 /**
  * @enum MotionControlMode
  * @brief SDK运动控制模式
  */
 enum class MotionControlMode : unsigned {
   Idle,       ///< 空闲
   NrtCommand, ///< 非实时模式执行运动指令
   NrtRLTask,  ///< 非实时模式运行RL工程
   RtCommand,  ///< 实时模式控制
 };

 /**
  * @enum RtControllerMode
  * @brief 控制器实时控制模式
  */
 enum class RtControllerMode : unsigned {
   jointPosition,      ///< 实时轴空间位置控制
   cartesianPosition,  ///< 实时笛卡尔空间位置控制
   jointImpedance,     ///< 实时轴空间阻抗控制
   cartesianImpedance, ///< 实时笛卡尔空间阻抗控制
   torque              ///< 实时力矩控制
 };

 namespace RtSupportedFields {
  /// 说明：数据名后为数据类型
  /// ArrayXD = std::array<double, DoF> , DoF为轴数
  /// Array6D = std::array<double, 6>, 以此类型
  constexpr const char *jointPos_m = "q_m";   ///< 关节角度 [rad] - ArrayXD
  constexpr const char *jointPos_c = "q_c";   ///< 指令关节角度 [rad] - ArrayXD
  constexpr const char *jointVel_m = "dq_m";  ///< 关节速度 [rad/s]- ArrayXD
  constexpr const char *jointVel_c = "dq_c";  ///< 指令关节速度 [rad/s] - ArrayXD
  constexpr const char *jointAcc_c = "ddq_c"; ///< 指令关节加速度 [rad/s^2] - ArrayXD
  constexpr const char *tcpPose_m  = "pos_m"; ///< 末端位姿, 相对于基坐标系, 行优先齐次变换矩阵 - Array16D
  constexpr const char *tcpPoseAbc_m = "pos_abc_m"; ///< 末端位姿, 相对于基坐标系 [X,Y,Z,Rx,Ry,Rz] - Array6D
  constexpr const char *tcpPose_c  = "pos_c"; ///< 发送的末端位姿指令, 相对于基坐标系, 行优先齐次变换矩阵 - Array16D
  constexpr const char *tcpVel_c   = "pos_vel_c"; ///< 指令机器人末端速度 - Array6D
  constexpr const char *tcpAcc_c   = "pos_acc_c"; ///< 指令机器人末端加速度 - Array6D
  constexpr const char *elbow_m    = "psi_m";     ///< 臂角 [rad] - double
  constexpr const char *elbow_c    = "psi_c";     ///< 指令臂角 [rad] - double
  constexpr const char *elbowVel_c = "psi_vel_c"; ///< 指令臂角速度 [rad/s] - double
  constexpr const char *elbowAcc_c = "psi_acc_c"; ///< 指令臂角加速度 [rad/s] - double
  constexpr const char *tau_m      = "tau_m";     ///< 关节力矩 [Nm] - ArrayXD
  constexpr const char *tau_c      = "tau_c";     ///< 指令关节力矩 [Nm] - ArrayXD
  constexpr const char *tauFiltered_m    = "tau_filtered_m"; ///< 滤波后关节力矩 [Nm] - ArrayXD
  constexpr const char *tauVel_c         = "tau_vel_c";      ///< 指令力矩微分 [Nm/s] - ArrayXD
  constexpr const char *tauExt_inBase    = "tau_ext_base";   ///< 基坐标系中外部力矩 [Nm] - Array6D
  constexpr const char *tauExt_inStiff   = "tau_ext_stiff";  ///< 力控坐标系中外部力矩 [Nm] - Array6D
  constexpr const char *theta_m          = "theta_m";        ///< 电机位置 - ArrayXD
  constexpr const char *thetaVel_m       = "theta_vel_m";        ///< 电机位置微分 - ArrayXD
  constexpr const char *motorTau         = "motor_tau";          ///< 电机转矩 - ArrayXD
  constexpr const char *motorTauFiltered = "motor_tau_filtered"; ///< 滤波后电机转矩 - ArrayXD
 }

 /**
  * @enum StopLevel
  * @brief 机器人停止运动等级
  */
 enum class StopLevel {
   stop0, ///< 快速停止机器人运动后断电
   stop1, ///< 规划停止机器人运动后断电, 停在原始路径上
   stop2  ///< 规划停止机器人运动后不断电, 停在原始路径上
 };

 /**
  * @struct DragParameter
  * @brief 机器人拖动模式参数, 包括拖动类型和空间
  */
 struct DragParameter {
   enum Space {
     jointSpace     = 0, ///< 轴空间
     cartesianSpace = 1  ///< 笛卡尔空间
   };
   enum Type {
     translationOnly = 0, ///< 仅平移
     rotationOnly    = 1, ///< 仅旋转
     freely          = 2  ///< 自由拖拽
   };
 };

 /**
  * @brief Jog选项: 坐标系
  */
 struct JogOpt {
   enum Space {
     world = 0, ///< 世界坐标系
     flange, ///< 法兰坐标系
     baseFrame, ///< 基坐标系
     toolFrame, ///< 工具坐标系
     wobjFrame, ///< 工件坐标系
     jointSpace ///< 轴空间
   };
 };

 /**
  * @brief xPanel配置: 对外供电模式
  */
 struct xPanelOpt {
   enum Vout {
     off,       ///< 不输出
     reserve,   ///< 保留
     supply12v, ///< 输出12V
     supply24v, ///< 输出24V
   };
 };

 /**
  * @enum ForceControlFrameType
  * @brief 实时模式 - 力控任务坐标系
  */
 enum class ForceControlFrameType {
   world = 0, ///< 世界坐标系
   tool  = 2, ///< 工具坐标系
   path  = 3  ///< 路径坐标系, 力控任务坐标系需要跟踪轨迹变化的过程
 };

// *******************          Data types            ********************
// *******************           数据结构              ********************
#if defined(XCORESDK_SUPPRESS_DLL_WARNING)
#pragma warning(push)
#pragma warning(disable : 4251)
#endif

 /**
  * @class Frame
  * @brief 坐标系
  */
 class XCORE_API Frame {
  public:
   /**
    * @brief default constructor
    */
   Frame() = default;

   /**
    * @brief 初始化trans & rpy
    * @param trans 平移量
    * @param rpy 欧拉角XYZ
    */
   Frame(const std::array<double, 3> &trans, const std::array<double, 3> &rpy);

   /**
    * @brief 初始化trans & rpy
    * @param frame [X, Y, Z, Rx, Ry, Rz]
    */
   Frame(const std::array<double, 6> &frame);

   /**
    * @brief 初始化pos
    * @param matrix 4*4变换矩阵
    */
   Frame(const std::array<double,16> &matrix);

   /**
    * @brief 初始化
    * @param values 长度为6时初始化trans & rot = [X, Y, Z, Rx, Ry, Rz];
    *               长度为16时初始化pos
    * @throw ArgumentException 初始化列表长度错误
    */
   Frame(std::initializer_list<double> values);

   std::array<double, 3> trans {}; ///< 平移量 [X, Y, Z], 单位:米
   std::array<double, 3> rpy {};   ///< 欧拉角 [Rx, Ry, Rz], 单位:弧度
   std::array<double, 16> pos {};  ///< 行优先齐次变换矩阵
 };

 /**
  * @class Finishable
  * @brief 一次运动循环是否结束
  */
 class XCORE_API Finishable {
  public:
   /**
    * @brief 是否已设置运动循环结束
    */
   uint8_t isFinished() const;

   /**
    * @brief 标识运动循环已结束
    */
   void setFinished();

  protected:
   uint8_t finished { 0 }; ///< 用于判断是否结束一个运动循环
 };

 /**
  * @class CartesianPosition
  * @brief 笛卡尔点位
  */
 class XCORE_API CartesianPosition : public Frame, public Finishable {
  public:
   using Frame::Frame;
   /**
    * @brief 偏移
    */
   struct Offset {
     enum Type {
       none,   ///< 无偏移
       offs,   ///< 相对工件坐标系偏移
       relTool ///< 相对工具坐标系偏移
     };

     /**
      * @brief default constructor
      */
     Offset() = default;

     /**
      * @brief constructor
      */
     Offset(Type type, const Frame &frame);

     Type type { none }; ///< 偏移类型
     Frame frame { };    ///< 相对于指定工具/工件坐标系的偏移
   };

   double elbow { 0 };      ///< 臂角, 适用于7轴机器人, 单位：弧度
   bool hasElbow { false }; ///< 是否有臂角
   std::vector<int> confData; ///< 轴配置数据，长度为8: [cf1, cf2, cf3, cf4, cf5, cf6, cf7, cfx]
   std::vector<double> external; ///< 外部关节角度, 单位:弧度
 };

 /**
  * @class JointPosition
  * @brief 关节点位
  */
 class XCORE_API JointPosition : public Finishable {
  public:
   /**
    * @brief default constructor
    */
   JointPosition() = default;
   /**
    * @param joints 长度应与机器人轴数一致. 外部关节可缺省
    */
   JointPosition(std::initializer_list<double> joints);
   JointPosition(std::vector<double> joints);

   /**
    * @brief 初始化joints
    * @param n 长度, 应和机型轴数匹配
    * @param v 初始值
    */
   JointPosition(size_t n, double v = 0);

   std::vector<double> joints; ///< 关节角度值, 单位:弧度
   std::vector<double> external; ///< 外部关节角度值, 单位:弧度
 };

 /**
  * @class Torque
  * @brief 关节扭矩，不包含重力和摩擦力
  */
 class XCORE_API Torque : public Finishable {
  public:
   Torque() = default;
   Torque(std::vector<double> tau);
   Torque(std::initializer_list<double> tau);

   /**
    * @brief 初始化tau
    * @param n 长度, 应和机型轴数匹配
    * @param v 初始值
    */
   Torque(size_t n, double v = 0);

   std::vector<double> tau; ///< 期望关节扭矩，单位: Nm
 };

 /**
   * @class Load
   * @brief 负载信息
   */
 class XCORE_API Load {
  public:
   Load() = default;
   /**
    * @param m 质量
    * @param cog 质心
    * @param inertia 惯量
    */
   Load(double m, const std::array<double, 3> &cog, const std::array<double, 3> &inertia);

   double mass { 0 };  ///< 负载质量, 单位:千克
   std::array<double, 3> cog {};     ///< 质心 [x, y, z], 单位:米
   std::array<double, 3> inertia {}; ///< 惯量 [ix, iy, iz, 单位:千克·平方米
 };

 /**
  * @class Toolset
  * @brief 工具工件组信息, 根据一对工具工件的坐标、负载、机器人手持设置计算得出
  * @note 并不显式区分手持/外部. 该类可这样理解: 如手持工具, 则负载和机器人末端坐标系是工具的, 参考坐标系则是工件的；
  *       反之, 如果手持工件, 则负载和末端坐标系来自工件, 参考坐标系来自工具
  */
 class XCORE_API Toolset {
  public:
   Toolset() = default;
   /**
    * @param load 负载信息
    * @param end 末端坐标系
    * @param ref 参考坐标系
    */
   Toolset(const Load &load, const Frame &end, const Frame &ref);

   Load load {}; ///< 机器人末端手持负载
   Frame end {}; ///< 机器人末端坐标系相对法兰坐标系转换
   Frame ref {}; ///< 机器人参考坐标系相对世界坐标系转换
 };

 /**
  * @class RLProjectInfo
  * @brief RL工程信息
  */
 class XCORE_API RLProjectInfo {
  public:
   explicit RLProjectInfo(std::string name);
   std::string name; ///< 工程名称
   std::vector<std::string> taskList; ///< 任务名称列表
 };

 /**
  * @class WorkToolInfo
  * @brief 工具/工件信息。工件的坐标系已相对其用户坐标系变换
  */
 class XCORE_API WorkToolInfo {
  public:
   WorkToolInfo() = default;
   WorkToolInfo(std::string name, bool isHeld, const Frame &posture, const Load &load);

   std::string name {};  ///< 名称
   std::string alias {}; ///< 别名, 暂未使用
   bool robotHeld {};    ///< 是否机器人手持
   Frame pos {};         ///< 位姿
   Load load {};         ///< 负载
 };

 /**
  * @class NrtCommand
  * @brief 非实时运动指令
  */
 class XCORE_API NrtCommand {
  public:

   int speed { USE_DEFAULT }; ///< 速率
   int zone { USE_DEFAULT };  ///< 转弯区大小

  protected:
   NrtCommand() = default;
   NrtCommand(int speed, int zone);
   virtual ~NrtCommand() = default;
 };

 typedef std::unordered_map<std::string, std::any> EventInfo;
 typedef std::function<void(const EventInfo &)> EventCallback;

 /**
  * @brief 事件类型
  */
 enum class Event {
   moveExecution, ///< 非实时运动指令执行信息
   safety         ///< 安全 (是否碰撞)
 };

 /**
  * @brief 事件信息字段
  */
  namespace EventInfoKey {
   /**
    * 非实时运动指令执行信息
    */
   namespace MoveExecution {
    constexpr const char *ID = "cmdID";     ///< 路径ID, 对应调用moveAppend()时第二个参数; 类型string
    constexpr const char *ReachTarget = "reachTarget"; ///< 轨迹是否到达目标点; 类型bool
    constexpr const char *WaypointIndex = "wayPointIndex"; ///< 当前正在执行的轨迹目标点下标, 从0开始; 类型int
    constexpr const char *Error = "error"; ///< 错误码, 运动指令执行前或执行中的错误; 类型error_code
   }
   namespace Safety {
    constexpr const char *Collided = "collided"; ///< 是否碰撞; 类型bool, true-发生碰撞 | false-未发生或已恢复
   }
  }

 /**
  * @class MoveAbsJCommand
  * @brief 运动指令 - 轴运动MoveAbsJ
  */
 class XCORE_API MoveAbsJCommand : public NrtCommand{
  public:
   /**
    * @param target 目标轴角度
    * @param speed 运行速度
    * @param zone 转弯区
    */
   MoveAbsJCommand(JointPosition target, int speed = USE_DEFAULT, int zone = USE_DEFAULT);

   JointPosition target; ///< 目标关节点位
 };

 /**
  * @class MoveJCommand
  * @brief 运动指令 - 轴运动MoveJ
  */
 class XCORE_API MoveJCommand : public NrtCommand {
  public:
   /**
    * @param target 目标笛卡尔点位
    * @param speed 运行速度
    * @param zone 转弯区
    */
   MoveJCommand(CartesianPosition target, int speed = USE_DEFAULT, int zone = USE_DEFAULT);

   CartesianPosition target; ///< 目标笛卡尔点位
   CartesianPosition::Offset offset; /// 偏移选项
 };

 /**
  * @class MoveLCommand
  * @brief 运动指令 - 末端直线轨迹MoveL
  */
 class XCORE_API MoveLCommand : public NrtCommand {
  public:
   /**
    * @param target 目标笛卡尔点位
    * @param speed 速率
    * @param zone 转弯区
    */
   MoveLCommand(CartesianPosition target, int speed = USE_DEFAULT, int zone = USE_DEFAULT);

   CartesianPosition target; ///< 目标笛卡尔点位
   CartesianPosition::Offset offset; /// 偏移选项
 };

 /**
  * @class MoveCCommand
  * @brief 运动指令 - 圆弧轨迹MoveC
  */
 class XCORE_API MoveCCommand : public NrtCommand {
  public:
   /**
    * @param target 目标点
    * @param aux 辅助点
    * @param speed 运行速度
    * @param zone 转弯区
    */
   MoveCCommand(CartesianPosition target, CartesianPosition aux, int speed = USE_DEFAULT, int zone = USE_DEFAULT);

   CartesianPosition target; ///< 目标笛卡尔点位
   CartesianPosition::Offset targetOffset; ///< 偏移选项
   CartesianPosition aux;    ///< 辅助点位
   CartesianPosition::Offset auxOffset; ///< 偏移选项
 };

 /**
  * @brief 运动指令 - 全圆轨迹MoveCF
  */
 class XCORE_API MoveCFCommand : public MoveCCommand{
  public:
   using MoveCCommand::MoveCCommand;
   /**
    * @param target 目标点
    * @param aux 辅助点
    * @param speed 运行速度
    * @param zone 转弯区
    * @param angle 执行角度
    */
   MoveCFCommand(const CartesianPosition &target, const CartesianPosition &aux, double angle, int speed = USE_DEFAULT, int zone = USE_DEFAULT);

   double angle { 360 }; ///< 全圆执行角度, 单位: 度
 };

 /**
  * @class LogInfo
  * @brief 控制器日志信息
  */
 class XCORE_API LogInfo {
  public:
   /**
    * @brief 日志等级
    */
   enum Level {
     info,    ///< 通知
     warning, ///< 警告
     error    ///< 错误
   };

   LogInfo(int id, std::string ts, std::string ct, std::string r);
   const int id;                ///< 日志ID号
   const std::string timestamp; ///< 日期及时间
   const std::string content;   ///< 日志内容
   const std::string repair;    ///< 修复办法
 };

#if defined(XCORESDK_SUPPRESS_DLL_WARNING)
#pragma warning(pop)
#endif // if defined(XCORESDK_SUPPRESS_DLL_WARNING)

}  // namespace rokae

#endif //ROKAEAPI_INCLUDE_ROKAE_DATA_TYPES_H_
