#ifndef PCL_TRACKING_IMPL_TRACKING_H_
#define PCL_TRACKING_IMPL_TRACKING_H_

#include <pcl/common/eigen.h>
#include <pcl/tracking/tracking.h>
#include <pcl/memory.h>
#include <pcl/pcl_macros.h>

namespace pcl {
namespace tracking {
struct _ParticleXYZRPY {
  PCL_ADD_POINT4D;
  union {
    struct {
      float roll;
      float pitch;
      float yaw;
      float weight;
    };
    float data_c[4];
  };
};

// particle definition
struct EIGEN_ALIGN16 ParticleXYZRPY : public _ParticleXYZRPY {
  inline ParticleXYZRPY()
  {
    x = y = z = roll = pitch = yaw = 0.0;
    data[3] = 1.0f;
  }

  inline ParticleXYZRPY(float _x, float _y, float _z)
  {
    x = _x;
    y = _y;
    z = _z;
    roll = pitch = yaw = 0.0;
    data[3] = 1.0f;
  }

  inline ParticleXYZRPY(
      float _x, float _y, float _z, float _roll, float _pitch, float _yaw)
  {
    x = _x;
    y = _y;
    z = _z;
    roll = _roll;
    pitch = _pitch;
    yaw = _yaw;
    data[3] = 1.0f;
  }

  inline static int
  stateDimension()
  {
    return 6;
  }

  void
  sample(const std::vector<double>& mean, const std::vector<double>& cov)
  {
    x += static_cast<float>(sampleNormal(mean[0], cov[0]));
    y += static_cast<float>(sampleNormal(mean[1], cov[1]));
    z += static_cast<float>(sampleNormal(mean[2], cov[2]));

    // The roll, pitch, yaw space is not Euclidean, so if we sample roll,
    // pitch, and yaw independently, we bias our sampling in a complicated
    // way that depends on where in the space we are sampling.
    //
    // A solution is to always sample around mean = 0 and project our
    // samples onto the space of rotations, SO(3).  We rely on the fact
    // that we are sampling with small variance, so we do not bias
    // ourselves too much with this projection.  We then rotate our
    // samples by the user's requested mean.  The benefit of this approach
    // is that our distribution's properties are consistent over the space
    // of rotations.
    Eigen::Matrix3f current_rotation;
    current_rotation = getTransformation(x, y, z, roll, pitch, yaw).rotation();
    Eigen::Quaternionf q_current_rotation(current_rotation);

    Eigen::Matrix3f mean_rotation;
    mean_rotation =
        getTransformation(mean[0], mean[1], mean[2], mean[3], mean[4], mean[5])
            .rotation();
    Eigen::Quaternionf q_mean_rotation(mean_rotation);

    // Scales 1.0 radians of variance in RPY sampling into equivalent units for
    // quaternion sampling.
    const float scale_factor = 0.2862;

    float a = sampleNormal(0, scale_factor * cov[3]);
    float b = sampleNormal(0, scale_factor * cov[4]);
    float c = sampleNormal(0, scale_factor * cov[5]);

    Eigen::Vector4f vec_sample_mean_0(a, b, c, 1);
    Eigen::Quaternionf q_sample_mean_0(vec_sample_mean_0);
    q_sample_mean_0.normalize();

    Eigen::Quaternionf q_sample_user_mean =
        q_sample_mean_0 * q_mean_rotation * q_current_rotation;

    Eigen::Affine3f affine_R(q_sample_user_mean.toRotationMatrix());
    pcl::getEulerAngles(affine_R, roll, pitch, yaw);
  }

  void
  zero()
  {
    x = 0.0;
    y = 0.0;
    z = 0.0;
    roll = 0.0;
    pitch = 0.0;
    yaw = 0.0;
  }

  inline Eigen::Affine3f
  toEigenMatrix() const
  {
    return getTransformation(x, y, z, roll, pitch, yaw);
  }

  static ParticleXYZRPY
  toState(const Eigen::Affine3f& trans)
  {
    float trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw;
    getTranslationAndEulerAngles(
        trans, trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw);
    return {trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw};
  }

  // a[i]
  inline float
  operator[](unsigned int i)
  {
    switch (i) {
    case 0:
      return x;
    case 1:
      return y;
    case 2:
      return z;
    case 3:
      return roll;
    case 4:
      return pitch;
    case 5:
      return yaw;
    default:
      return 0.0;
    }
  }

  PCL_MAKE_ALIGNED_OPERATOR_NEW
};

inline std::ostream&
operator<<(std::ostream& os, const ParticleXYZRPY& p)
{
  os << "(" << p.x << "," << p.y << "," << p.z << "," << p.roll << "," << p.pitch << ","
     << p.yaw << ")";
  return (os);
}

// a * k
inline ParticleXYZRPY
operator*(const ParticleXYZRPY& p, double val)
{
  pcl::tracking::ParticleXYZRPY newp;
  newp.x = static_cast<float>(p.x * val);
  newp.y = static_cast<float>(p.y * val);
  newp.z = static_cast<float>(p.z * val);
  newp.roll = static_cast<float>(p.roll * val);
  newp.pitch = static_cast<float>(p.pitch * val);
  newp.yaw = static_cast<float>(p.yaw * val);
  return (newp);
}

// a + b
inline ParticleXYZRPY
operator+(const ParticleXYZRPY& a, const ParticleXYZRPY& b)
{
  pcl::tracking::ParticleXYZRPY newp;
  newp.x = a.x + b.x;
  newp.y = a.y + b.y;
  newp.z = a.z + b.z;
  newp.roll = a.roll + b.roll;
  newp.pitch = a.pitch + b.pitch;
  newp.yaw = a.yaw + b.yaw;
  return (newp);
}

// a - b
inline ParticleXYZRPY
operator-(const ParticleXYZRPY& a, const ParticleXYZRPY& b)
{
  pcl::tracking::ParticleXYZRPY newp;
  newp.x = a.x - b.x;
  newp.y = a.y - b.y;
  newp.z = a.z - b.z;
  newp.roll = a.roll - b.roll;
  newp.pitch = a.pitch - b.pitch;
  newp.yaw = a.yaw - b.yaw;
  return (newp);
}

} // namespace tracking
} // namespace pcl

//########################################################################33

namespace pcl {
namespace tracking {
struct _ParticleXYZR {
  PCL_ADD_POINT4D;
  union {
    struct {
      float roll;
      float pitch;
      float yaw;
      float weight;
    };
    float data_c[4];
  };
};

// particle definition
struct EIGEN_ALIGN16 ParticleXYZR : public _ParticleXYZR {
  inline ParticleXYZR()
  {
    x = y = z = roll = pitch = yaw = 0.0;
    data[3] = 1.0f;
  }

  inline ParticleXYZR(float _x, float _y, float _z)
  {
    x = _x;
    y = _y;
    z = _z;
    roll = pitch = yaw = 0.0;
    data[3] = 1.0f;
  }

  inline ParticleXYZR(float _x, float _y, float _z, float, float _pitch, float)
  {
    x = _x;
    y = _y;
    z = _z;
    roll = 0;
    pitch = _pitch;
    yaw = 0;
    data[3] = 1.0f;
  }

  inline static int
  stateDimension()
  {
    return 6;
  }

  void
  sample(const std::vector<double>& mean, const std::vector<double>& cov)
  {
    x += static_cast<float>(sampleNormal(mean[0], cov[0]));
    y += static_cast<float>(sampleNormal(mean[1], cov[1]));
    z += static_cast<float>(sampleNormal(mean[2], cov[2]));
    roll = 0;
    pitch += static_cast<float>(sampleNormal(mean[4], cov[4]));
    yaw = 0;
  }

  void
  zero()
  {
    x = 0.0;
    y = 0.0;
    z = 0.0;
    roll = 0.0;
    pitch = 0.0;
    yaw = 0.0;
  }

  inline Eigen::Affine3f
  toEigenMatrix() const
  {
    return getTransformation(x, y, z, roll, pitch, yaw);
  }

  static ParticleXYZR
  toState(const Eigen::Affine3f& trans)
  {
    float trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw;
    getTranslationAndEulerAngles(
        trans, trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw);
    return (pcl::tracking::ParticleXYZR(trans_x, trans_y, trans_z, 0, trans_pitch, 0));
  }

  // a[i]
  inline float
  operator[](unsigned int i)
  {
    switch (i) {
    case 0:
      return x;
    case 1:
      return y;
    case 2:
      return z;
    case 3:
      return roll;
    case 4:
      return pitch;
    case 5:
      return yaw;
    default:
      return 0.0;
    }
  }

  PCL_MAKE_ALIGNED_OPERATOR_NEW
};

inline std::ostream&
operator<<(std::ostream& os, const ParticleXYZR& p)
{
  os << "(" << p.x << "," << p.y << "," << p.z << "," << p.roll << "," << p.pitch << ","
     << p.yaw << ")";
  return (os);
}

// a * k
inline ParticleXYZR
operator*(const ParticleXYZR& p, double val)
{
  pcl::tracking::ParticleXYZR newp;
  newp.x = static_cast<float>(p.x * val);
  newp.y = static_cast<float>(p.y * val);
  newp.z = static_cast<float>(p.z * val);
  newp.roll = static_cast<float>(p.roll * val);
  newp.pitch = static_cast<float>(p.pitch * val);
  newp.yaw = static_cast<float>(p.yaw * val);
  return (newp);
}

// a + b
inline ParticleXYZR
operator+(const ParticleXYZR& a, const ParticleXYZR& b)
{
  pcl::tracking::ParticleXYZR newp;
  newp.x = a.x + b.x;
  newp.y = a.y + b.y;
  newp.z = a.z + b.z;
  newp.roll = 0;
  newp.pitch = a.pitch + b.pitch;
  newp.yaw = 0.0;
  return (newp);
}

// a - b
inline ParticleXYZR
operator-(const ParticleXYZR& a, const ParticleXYZR& b)
{
  pcl::tracking::ParticleXYZR newp;
  newp.x = a.x - b.x;
  newp.y = a.y - b.y;
  newp.z = a.z - b.z;
  newp.roll = 0.0;
  newp.pitch = a.pitch - b.pitch;
  newp.yaw = 0.0;
  return (newp);
}

} // namespace tracking
} // namespace pcl

//########################################################################33

namespace pcl {
namespace tracking {
struct _ParticleXYRPY {
  PCL_ADD_POINT4D;
  union {
    struct {
      float roll;
      float pitch;
      float yaw;
      float weight;
    };
    float data_c[4];
  };
};

// particle definition
struct EIGEN_ALIGN16 ParticleXYRPY : public _ParticleXYRPY {
  inline ParticleXYRPY()
  {
    x = y = z = roll = pitch = yaw = 0.0;
    data[3] = 1.0f;
  }

  inline ParticleXYRPY(float _x, float, float _z)
  {
    x = _x;
    y = 0;
    z = _z;
    roll = pitch = yaw = 0.0;
    data[3] = 1.0f;
  }

  inline ParticleXYRPY(float _x, float, float _z, float _roll, float _pitch, float _yaw)
  {
    x = _x;
    y = 0;
    z = _z;
    roll = _roll;
    pitch = _pitch;
    yaw = _yaw;
    data[3] = 1.0f;
  }

  inline static int
  stateDimension()
  {
    return 6;
  }

  void
  sample(const std::vector<double>& mean, const std::vector<double>& cov)
  {
    x += static_cast<float>(sampleNormal(mean[0], cov[0]));
    y = 0;
    z += static_cast<float>(sampleNormal(mean[2], cov[2]));
    roll += static_cast<float>(sampleNormal(mean[3], cov[3]));
    pitch += static_cast<float>(sampleNormal(mean[4], cov[4]));
    yaw += static_cast<float>(sampleNormal(mean[5], cov[5]));
  }

  void
  zero()
  {
    x = 0.0;
    y = 0.0;
    z = 0.0;
    roll = 0.0;
    pitch = 0.0;
    yaw = 0.0;
  }

  inline Eigen::Affine3f
  toEigenMatrix() const
  {
    return getTransformation(x, y, z, roll, pitch, yaw);
  }

  static ParticleXYRPY
  toState(const Eigen::Affine3f& trans)
  {
    float trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw;
    getTranslationAndEulerAngles(
        trans, trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw);
    return (pcl::tracking::ParticleXYRPY(
        trans_x, 0, trans_z, trans_roll, trans_pitch, trans_yaw));
  }

  // a[i]
  inline float
  operator[](unsigned int i)
  {
    switch (i) {
    case 0:
      return x;
    case 1:
      return y;
    case 2:
      return z;
    case 3:
      return roll;
    case 4:
      return pitch;
    case 5:
      return yaw;
    default:
      return 0.0;
    }
  }

  PCL_MAKE_ALIGNED_OPERATOR_NEW
};

inline std::ostream&
operator<<(std::ostream& os, const ParticleXYRPY& p)
{
  os << "(" << p.x << "," << p.y << "," << p.z << "," << p.roll << "," << p.pitch << ","
     << p.yaw << ")";
  return (os);
}

// a * k
inline ParticleXYRPY
operator*(const ParticleXYRPY& p, double val)
{
  pcl::tracking::ParticleXYRPY newp;
  newp.x = static_cast<float>(p.x * val);
  newp.y = static_cast<float>(p.y * val);
  newp.z = static_cast<float>(p.z * val);
  newp.roll = static_cast<float>(p.roll * val);
  newp.pitch = static_cast<float>(p.pitch * val);
  newp.yaw = static_cast<float>(p.yaw * val);
  return (newp);
}

// a + b
inline ParticleXYRPY
operator+(const ParticleXYRPY& a, const ParticleXYRPY& b)
{
  pcl::tracking::ParticleXYRPY newp;
  newp.x = a.x + b.x;
  newp.y = 0;
  newp.z = a.z + b.z;
  newp.roll = a.roll + b.roll;
  newp.pitch = a.pitch + b.pitch;
  newp.yaw = a.yaw + b.yaw;
  return (newp);
}

// a - b
inline ParticleXYRPY
operator-(const ParticleXYRPY& a, const ParticleXYRPY& b)
{
  pcl::tracking::ParticleXYRPY newp;
  newp.x = a.x - b.x;
  newp.z = a.z - b.z;
  newp.y = 0;
  newp.roll = a.roll - b.roll;
  newp.pitch = a.pitch - b.pitch;
  newp.yaw = a.yaw - b.yaw;
  return (newp);
}

} // namespace tracking
} // namespace pcl

//########################################################################33

namespace pcl {
namespace tracking {
struct _ParticleXYRP {
  PCL_ADD_POINT4D;
  union {
    struct {
      float roll;
      float pitch;
      float yaw;
      float weight;
    };
    float data_c[4];
  };
};

// particle definition
struct EIGEN_ALIGN16 ParticleXYRP : public _ParticleXYRP {
  inline ParticleXYRP()
  {
    x = y = z = roll = pitch = yaw = 0.0;
    data[3] = 1.0f;
  }

  inline ParticleXYRP(float _x, float, float _z)
  {
    x = _x;
    y = 0;
    z = _z;
    roll = pitch = yaw = 0.0;
    data[3] = 1.0f;
  }

  inline ParticleXYRP(float _x, float, float _z, float, float _pitch, float _yaw)
  {
    x = _x;
    y = 0;
    z = _z;
    roll = 0;
    pitch = _pitch;
    yaw = _yaw;
    data[3] = 1.0f;
  }

  inline static int
  stateDimension()
  {
    return 6;
  }

  void
  sample(const std::vector<double>& mean, const std::vector<double>& cov)
  {
    x += static_cast<float>(sampleNormal(mean[0], cov[0]));
    y = 0;
    z += static_cast<float>(sampleNormal(mean[2], cov[2]));
    roll = 0;
    pitch += static_cast<float>(sampleNormal(mean[4], cov[4]));
    yaw += static_cast<float>(sampleNormal(mean[5], cov[5]));
  }

  void
  zero()
  {
    x = 0.0;
    y = 0.0;
    z = 0.0;
    roll = 0.0;
    pitch = 0.0;
    yaw = 0.0;
  }

  inline Eigen::Affine3f
  toEigenMatrix() const
  {
    return getTransformation(x, y, z, roll, pitch, yaw);
  }

  static ParticleXYRP
  toState(const Eigen::Affine3f& trans)
  {
    float trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw;
    getTranslationAndEulerAngles(
        trans, trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw);
    return (
        pcl::tracking::ParticleXYRP(trans_x, 0, trans_z, 0, trans_pitch, trans_yaw));
  }

  // a[i]
  inline float
  operator[](unsigned int i)
  {
    switch (i) {
    case 0:
      return x;
    case 1:
      return y;
    case 2:
      return z;
    case 3:
      return roll;
    case 4:
      return pitch;
    case 5:
      return yaw;
    default:
      return 0.0;
    }
  }

  PCL_MAKE_ALIGNED_OPERATOR_NEW
};

inline std::ostream&
operator<<(std::ostream& os, const ParticleXYRP& p)
{
  os << "(" << p.x << "," << p.y << "," << p.z << "," << p.roll << "," << p.pitch << ","
     << p.yaw << ")";
  return (os);
}

// a * k
inline ParticleXYRP
operator*(const ParticleXYRP& p, double val)
{
  pcl::tracking::ParticleXYRP newp;
  newp.x = static_cast<float>(p.x * val);
  newp.y = static_cast<float>(p.y * val);
  newp.z = static_cast<float>(p.z * val);
  newp.roll = static_cast<float>(p.roll * val);
  newp.pitch = static_cast<float>(p.pitch * val);
  newp.yaw = static_cast<float>(p.yaw * val);
  return (newp);
}

// a + b
inline ParticleXYRP
operator+(const ParticleXYRP& a, const ParticleXYRP& b)
{
  pcl::tracking::ParticleXYRP newp;
  newp.x = a.x + b.x;
  newp.y = 0;
  newp.z = a.z + b.z;
  newp.roll = 0;
  newp.pitch = a.pitch + b.pitch;
  newp.yaw = a.yaw + b.yaw;
  return (newp);
}

// a - b
inline ParticleXYRP
operator-(const ParticleXYRP& a, const ParticleXYRP& b)
{
  pcl::tracking::ParticleXYRP newp;
  newp.x = a.x - b.x;
  newp.z = a.z - b.z;
  newp.y = 0;
  newp.roll = 0.0;
  newp.pitch = a.pitch - b.pitch;
  newp.yaw = a.yaw - b.yaw;
  return (newp);
}

} // namespace tracking
} // namespace pcl

//########################################################################33

namespace pcl {
namespace tracking {
struct _ParticleXYR {
  PCL_ADD_POINT4D;
  union {
    struct {
      float roll;
      float pitch;
      float yaw;
      float weight;
    };
    float data_c[4];
  };
};

// particle definition
struct EIGEN_ALIGN16 ParticleXYR : public _ParticleXYR {
  inline ParticleXYR()
  {
    x = y = z = roll = pitch = yaw = 0.0;
    data[3] = 1.0f;
  }

  inline ParticleXYR(float _x, float, float _z)
  {
    x = _x;
    y = 0;
    z = _z;
    roll = pitch = yaw = 0.0;
    data[3] = 1.0f;
  }

  inline ParticleXYR(float _x, float, float _z, float, float _pitch, float)
  {
    x = _x;
    y = 0;
    z = _z;
    roll = 0;
    pitch = _pitch;
    yaw = 0;
    data[3] = 1.0f;
  }

  inline static int
  stateDimension()
  {
    return 6;
  }

  void
  sample(const std::vector<double>& mean, const std::vector<double>& cov)
  {
    x += static_cast<float>(sampleNormal(mean[0], cov[0]));
    y = 0;
    z += static_cast<float>(sampleNormal(mean[2], cov[2]));
    roll = 0;
    pitch += static_cast<float>(sampleNormal(mean[4], cov[4]));
    yaw = 0;
  }

  void
  zero()
  {
    x = 0.0;
    y = 0.0;
    z = 0.0;
    roll = 0.0;
    pitch = 0.0;
    yaw = 0.0;
  }

  inline Eigen::Affine3f
  toEigenMatrix() const
  {
    return getTransformation(x, y, z, roll, pitch, yaw);
  }

  static ParticleXYR
  toState(const Eigen::Affine3f& trans)
  {
    float trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw;
    getTranslationAndEulerAngles(
        trans, trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw);
    return (pcl::tracking::ParticleXYR(trans_x, 0, trans_z, 0, trans_pitch, 0));
  }

  // a[i]
  inline float
  operator[](unsigned int i)
  {
    switch (i) {
    case 0:
      return x;
    case 1:
      return y;
    case 2:
      return z;
    case 3:
      return roll;
    case 4:
      return pitch;
    case 5:
      return yaw;
    default:
      return 0.0;
    }
  }

  PCL_MAKE_ALIGNED_OPERATOR_NEW
};

inline std::ostream&
operator<<(std::ostream& os, const ParticleXYR& p)
{
  os << "(" << p.x << "," << p.y << "," << p.z << "," << p.roll << "," << p.pitch << ","
     << p.yaw << ")";
  return (os);
}

// a * k
inline ParticleXYR
operator*(const ParticleXYR& p, double val)
{
  pcl::tracking::ParticleXYR newp;
  newp.x = static_cast<float>(p.x * val);
  newp.y = static_cast<float>(p.y * val);
  newp.z = static_cast<float>(p.z * val);
  newp.roll = static_cast<float>(p.roll * val);
  newp.pitch = static_cast<float>(p.pitch * val);
  newp.yaw = static_cast<float>(p.yaw * val);
  return (newp);
}

// a + b
inline ParticleXYR
operator+(const ParticleXYR& a, const ParticleXYR& b)
{
  pcl::tracking::ParticleXYR newp;
  newp.x = a.x + b.x;
  newp.y = 0;
  newp.z = a.z + b.z;
  newp.roll = 0;
  newp.pitch = a.pitch + b.pitch;
  newp.yaw = 0.0;
  return (newp);
}

// a - b
inline ParticleXYR
operator-(const ParticleXYR& a, const ParticleXYR& b)
{
  pcl::tracking::ParticleXYR newp;
  newp.x = a.x - b.x;
  newp.z = a.z - b.z;
  newp.y = 0;
  newp.roll = 0.0;
  newp.pitch = a.pitch - b.pitch;
  newp.yaw = 0.0;
  return (newp);
}

} // namespace tracking
} // namespace pcl

#define PCL_STATE_POINT_TYPES                                                          \
  (pcl::tracking::ParticleXYR)(pcl::tracking::ParticleXYZRPY)(                         \
      pcl::tracking::ParticleXYZR)(pcl::tracking::ParticleXYRPY)(                      \
      pcl::tracking::ParticleXYRP)

#endif //