#ifndef _VDA_5050_COMMON_H_INCLUDE_
#define _VDA_5050_COMMON_H_INCLUDE_
#include <iostream>
#include <vector>

namespace vda_5050
{
  enum class OrderType
  {
    Base,
    Horizon
  };

  struct NodePosition
  {
    double x, y, theta;
    double allowedDeviationXY, allowedDeviationTheta;
    std::string mapId, mapDescription;
  };

  struct ActionParameter
  {
    std::string key;
    std::string value;
  };

  struct Action
  {
    std::string actionType;
    std::string actionId;
    std::string actionDescription;
    std::string blockingType;
    std::vector<ActionParameter> actionParameters;
  };

  struct Node
  {
    std::string nodeId;
    int sequenceId;
    std::string nodeDescription;
    bool released;
    NodePosition nodePosition;
    std::vector<std::shared_ptr<Action>> actions;
  };

  struct Corridor
  {
    double leftWidth;
    double rightWidth;
    std::string corridorRefPoint;
  };

  struct ControlPoint
  {
    double x, y, weight;
  };

  struct Trajectory
  {
    int degree;
    std::vector<double> knotVector;
    std::vector<ControlPoint> controlPoints;
  };

  struct Edge
  {
    std::string edgeId;
    int sequenceId;
    std::string edgeDescription;
    bool released;
    std::string startNodeId;
    std::string endNodeId;
    double maxSpeed;
    double maxHeight;
    double minHeight;
    double orientation;
    std::string orientationType;
    std::string direction;
    bool rotationAllowed;
    double maxRotationSpeed;
    Trajectory trajectory;
    double length;
    Corridor corridor;
    std::vector<Action> actions;
  };

  struct Order
  {
    int headerId;
    std::string timestamp;
    std::string version;
    std::string manufacturer;
    std::string serialNumber;
    std::string orderId;
    int orderUpdateId;
    std::string zoneSetId;
    std::vector<Node> nodes;
    std::vector<Edge> edges;

    OrderType getTypeOrder()
    {
      for (auto &edge : edges)
      {
        if (!edge.released)
          return OrderType::Horizon;
      }
      for (auto &node : nodes)
      {
        if (!node.released)
          return OrderType::Horizon;
      }
      return OrderType::Base;
    }
  };

  struct InstantAction
  {
    int headerId;
    std::string timestamp;
    std::string version;
    std::string manufacturer;
    std::string serialNumber;
    std::vector<std::shared_ptr<Action>> actions;
  };

  struct AGVPosition
  {
    double x;
    double y;
    double theta;
    std::string mapId;
    bool positionInitialized;
    double localizationScore;
    double deviationRange;
  };

  struct Velocity
  {
    double vx;
    double vy;
    double omega;
  };

  struct BatteryState
  {
    double batteryCharge;
    double batteryVoltage;
    int8_t batteryHealth;
    bool charging;
    uint32_t reach;
  };

  struct SafetyState
  {
    std::string eStop;
    bool fieldViolation;
    std::string AUTO_ACK = "autoAck";
    std::string MANUAL = "manual";
    std::string REMOTE = "remote";
    std::string NONE = "none";
  };

  struct ErrorReference
  {
    std::string referenceKey;
    std::string referenceValue;
  };

  struct Error
  {
    struct ErrorLevel
    {
      enum ErrorLevel_EN
      {
        WARNING,
        FATAL
      };

      operator ErrorLevel_EN() const { return level; }

      ErrorLevel &operator=(const ErrorLevel_EN &newType)
      {
        level = newType;
        return *this;
      }

    private:
      ErrorLevel_EN level;

    public:
      // Function to convert ActionStatus to string
      std::string _toString() const
      {
        switch (level)
        {
        case WARNING:
          return "WARNING";
        case FATAL:
          return "FATAL";
        default:
          return "";
        }
      }
    };

    std::string errorType;
    std::vector<ErrorReference> errorReferences;
    std::string errorDescription;
    ErrorLevel errorLevel;
  };

  struct NodeState
  {
    std::string nodeId;
    int32_t sequenceId;
    std::string nodeDescription;
    NodePosition nodePosition;
    bool released;
  };

  struct EdgeState
  {
    std::string edgeId;
    uint32_t sequenceId;
    std::string edgeDescription;
    bool released;
    Trajectory trajectory;
  };

  struct ActionState
  {
    struct ActionStatus
    {
    public:
      enum ActionStatus_EN
      {
        WAITING,
        INITIALIZING,
        RUNNING,
        PAUSED,
        FINISHED,
        FAILED
      };

      operator ActionStatus_EN() const { return type; }

      ActionStatus &operator=(const ActionStatus_EN &newType)
      {
        type = newType;
        return *this;
      }

      bool operator==(const ActionStatus_EN &otherType) const
      {
        return type == otherType;
      }

    private:
      ActionStatus_EN type;

    public:
      // Function to convert ActionStatus to string
      std::string _toString() const
      {
        switch (type)
        {
        case WAITING:
          return "WAITING";
        case INITIALIZING:
          return "INITIALIZING";
        case RUNNING:
          return "RUNNING";
        case PAUSED:
          return "PAUSED";
        case FINISHED:
          return "FINISHED";
        case FAILED:
          return "FAILED";
        default:
          return "";
        }
      }
    };

    std::string actionId;
    std::string actionType;
    std::string actionDescription;
    ActionStatus actionStatus;
    std::string resultDescription;
  };

  struct InfoReference
  {
    std::string referenceKey;
    std::string referenceValue;
  };

  struct Info
  {
    std::string infoType;
    std::vector<InfoReference> infoReferences;
    std::string infoDescription;
    std::string infoLevel;

    std::string DEBUG = "DEBUG";
    std::string INFO = "INFO";
  };

  struct BoundingBoxReference
  {
    double x;
    double y;
    double z;
    double theta;
  };

  struct LoadDimensions
  {
    double length;
    double width;
    double height;
  };

  struct Load
  {
    std::string loadId;
    std::string loadType;
    std::string loadPosition;
    BoundingBoxReference boundingBoxReference;
    LoadDimensions loadDimensions;
    uint32_t weight;
  };

  struct State
  {
    struct OperatingMode
    {
      enum OperatingMode_EN
      {
        AUTOMATIC,
        SEMIAUTOMATIC,
        MANUAL,
        SERVICE,
        TEACHING
      };

    private:
      OperatingMode_EN type;

    public:
      // Constructor mặc định
      OperatingMode() : type(SERVICE) {}

      // Toán tử chuyển đổi sang enum
      operator OperatingMode_EN() const { return type; }

      OperatingMode &operator=(const OperatingMode_EN &newType)
      {
        type = newType;
        return *this;
      }

      bool operator==(const OperatingMode_EN &otherType) const
      {
        return type == otherType;
      }

      // Chuyển đổi sang chuỗi
      std::string _toString() const
      {
        switch (type)
        {
        case AUTOMATIC:
          return "AUTOMATIC";
        case SEMIAUTOMATIC:
          return "SEMIAUTOMATIC";
        case MANUAL:
          return "MANUAL";
        case SERVICE:
          return "SERVICE";
        case TEACHING:
          return "TEACHING";
        default:
          return "SERVICE";
        }
      }
    };

    State()
    {
      headerId = 1;
      version = "1.0.0";
      manufacturer = "Phenikaa-X";
      serialNumber = "";
      velocity.vx = 0;
      velocity.vy = 0;
      velocity.omega = 0;
      batteryState.batteryCharge = 100;
      batteryState.batteryVoltage = 48;
      batteryState.batteryHealth = 100;
      batteryState.charging = false;
      batteryState.reach = 0;
      std::vector<Error> errors_;
      errors = errors_;
    }

    uint32_t headerId;
    std::string timestamp;
    std::string version;
    std::string manufacturer;
    std::string serialNumber;
    std::string orderId;
    uint32_t orderUpdateId;
    std::string zoneSetId;
    std::string lastNodeId;
    uint32_t lastNodeSequenceId;
    bool driving;
    bool paused;
    bool newBaseRequest;
    double distanceSinceLastNode;
    OperatingMode operatingMode;
    std::vector<NodeState> nodeStates;
    std::vector<EdgeState> edgeStates;
    std::vector<std::shared_ptr<vda_5050::ActionState>> actionStates;
    AGVPosition agvPosition;
    Velocity velocity;
    std::vector<Load> loads;
    BatteryState batteryState;
    std::vector<Error> errors;
    std::vector<Info> information;
    SafetyState safetyState;

    OrderType getTypeOrder()
    {
      for (auto &edge : edgeStates)
      {
        if (!edge.released)
          return OrderType::Horizon;
      }
      for (auto &node : nodeStates)
      {
        if (!node.released)
          return OrderType::Horizon;
      }
      return OrderType::Base;
    }
  };

  struct Visualization
  {
    Visualization()
    {
      headerId = 1;
      version = "1.0.0";
      manufacturer = "Phenikaa-X";
      serialNumber = "AMR2WS";
      velocity.vx = 0;
      velocity.vy = 0;
      velocity.omega = 0;
    }

    uint32_t headerId;
    std::string timestamp;
    std::string version;
    std::string manufacturer;
    std::string serialNumber;
    AGVPosition agvPosition;
    Velocity velocity;
  };
}

#endif // _VDA_5050_COMMON_H_INCLUDE_