#pragma once
#include <math.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

//template <typename T>
class CoordinateTransformation {
public:
    struct Point2D {
        float x;
        float y;
    };

    struct transInfo2D {
        Point2D move;
        float angle;
    };

    struct transInfo3D {
        pcl::PointXYZ move;
        float roll;
        float pitch;
        float yaw;

        transInfo3D() = default;
        transInfo3D(pcl::PointXYZ move_, float roll_, float pitch_, float yaw_): move(move_), roll(roll_), pitch(pitch_) {}
    };

    CoordinateTransformation() = default;
    ~CoordinateTransformation() = default;

    void setTranInfo(const transInfo2D &trans, bool radian = false) {

    }

    void setTranInfo(const transInfo3D &trans, bool radian = false) {
        trans3D = true;
        float cosYaw, sinYaw, cosPitch, sinPitch, cosRoll, sinRoll;
        if (!radian) {
            cosYaw = cos(trans.yaw * float(M_PI) / 180.0f);
            sinYaw = sin(trans.yaw * float(M_PI) / 180.0f);
            cosPitch = cos(trans.pitch * float(M_PI) / 180.0f);
            sinPitch = sin(trans.pitch * float(M_PI) / 180.0f);
            cosRoll = cos(trans.roll * float(M_PI) / 180.0f);
            sinRoll = sin(trans.roll * float(M_PI) / 180.0f);
        } else {
            cosYaw = cos(trans.yaw);
            sinYaw = sin(trans.yaw);
            cosPitch = cos(trans.pitch);
            sinPitch = sin(trans.pitch);
            cosRoll = cos(trans.roll);
            sinRoll = sin(trans.roll);
        }
        trans3D_move = trans.move;
        rotationMatrix3D[0][0] = cosYaw * cosPitch;
        rotationMatrix3D[0][1] = cosYaw * sinPitch * sinRoll - sinYaw * cosRoll;
        rotationMatrix3D[0][2] = cosYaw * sinPitch * cosRoll + sinYaw * sinRoll;
        rotationMatrix3D[1][0] = sinYaw * cosPitch;
        rotationMatrix3D[1][1] = sinYaw * sinPitch * sinRoll + cosYaw * cosRoll;
        rotationMatrix3D[1][2] = sinYaw * sinPitch * cosRoll - cosYaw * sinRoll;
        rotationMatrix3D[2][0] = -sinPitch;
        rotationMatrix3D[2][1] = cosPitch * sinRoll;
        rotationMatrix3D[2][2] = cosPitch * cosRoll;
    }

    void transPoint(const Point2D &in, Point2D &out) {

    }

    void transPoint(const pcl::PointXYZ &in, pcl::PointXYZ &out) {
        trans3DPoint(in, out);
    }
private:
    bool trans3DPoint(const pcl::PointXYZ &in, pcl::PointXYZ &out) {
        if (trans3D) {
            out.x = rotationMatrix3D[0][0] * in.x + rotationMatrix3D[0][1] * in.y + rotationMatrix3D[0][2] * in.z + trans3D_move.x;
            out.y = rotationMatrix3D[1][0] * in.x + rotationMatrix3D[1][1] * in.y + rotationMatrix3D[1][2] * in.z + trans3D_move.y;
            out.z = rotationMatrix3D[2][0] * in.x + rotationMatrix3D[2][1] * in.y + rotationMatrix3D[2][2] * in.z + trans3D_move.z;
        }
        return trans3D;
    }

private:
    bool trans2D = false;
    bool trans3D = false;
    Point2D trans2D_move{};
    pcl::PointXYZ trans3D_move{0, 0, 0};
    float rotationMatrix2D[1] = {0};
    float rotationMatrix3D[3][3] = {0};
};