#define PI 3.1415926

int   Sign(double x) { if (x < 0) return -1; else return 1; }

float R2D(float angle){ return angle * 180.0 / PI; }

enum  RotSeq{ zyx, xyz }; // 欧拉角旋转次序


CameraSpacePoint CBodyBasics::QuaternionToEuler(Vector4 q, RotSeq rotSeq) 
{
    CameraSpacePoint euler = { 0 };
    const double Epsilon = 0.0009765625f;
    const double Threshold = 0.5f - Epsilon;

    switch (rotSeq)
    {
        case zyx:  // Z-Y-X Euler angles(RPY angles)
        {
            double TEST = q.w*q.y - q.x*q.z;
            if (TEST < -Threshold || TEST > Threshold) // 奇异姿态,俯仰角为±90°
            {
                int sign = Sign(TEST);
                euler.Z = -2 * sign * (double)atan2(q.x, q.w); // yaw
                euler.Y = sign * (PI / 2.0); // pitch
                euler.X = 0; // roll

            }
            else
            {
                euler.X = atan2(2 * (q.y*q.z + q.w*q.x), q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z); // roll
                euler.Y = asin(-2 * (q.x*q.z - q.w*q.y));                                         // pitch
                euler.Z = atan2(2 * (q.x*q.y + q.w*q.z), q.w*q.w + q.x*q.x - q.y*q.y - q.z*q.z); // yaw
            }
        }
        break;

        case xyz:  // X-Y-Z Euler angles
        {
            double TEST = q.x*q.z + q.w*q.y;
            if (TEST < -Threshold || TEST > Threshold) // 奇异姿态,俯仰角为±90°
            {
                int sign = Sign(TEST);
                euler.X = 2 * sign * (double)atan2(q.x, q.w);
                euler.Y = sign * (PI / 2.0);
                euler.Z = 0;
            }
            else
            {
                euler.X = atan2(-2 * (q.y*q.z - q.w*q.x), q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z);
                euler.Y = asin(2 * (q.x*q.z + q.w*q.y));
                euler.Z = atan2(-2 * (q.x*q.y - q.w*q.z), q.w*q.w + q.x*q.x - q.y*q.y - q.z*q.z);
            }
        }

    }
    return euler;
}


/// Handle new body data
void CBodyBasics::ProcessBody(IBody* pBody)
{
    HRESULT hr;
    BOOLEAN bTracked = false;
    hr = pBody->get_IsTracked(&bTracked);  // Retrieves a boolean value that indicates if the body is tracked

    if (SUCCEEDED(hr) && bTracked)  // 判断是否追踪到骨骼
    {
        Joint joints[JointType_Count];
        JointOrientation jointOrientations[JointType_Count];

        HandState leftHandState = HandState_Unknown;
        HandState rightHandState = HandState_Unknown;

        DepthSpacePoint *depthSpacePosition = new DepthSpacePoint[_countof(joints)]; // 存储深度坐标系中的关节点位置
        
        pBody->get_HandLeftState(&leftHandState);  // 获取左右手状态
        pBody->get_HandRightState(&rightHandState);
        
        hr = pBody->GetJointOrientations(_countof(joints), jointOrientations); // 获取joint orientation
        if (SUCCEEDED(hr))
        {
            CameraSpacePoint euler = QuaternionToEuler(jointOrientations[JointType_Neck].Orientation, xyz); // 四元数转换为X-Y-Z欧拉角

            simxSetJointPosition(clientID, Handle_Yaw,   euler.Y, simx_opmode_oneshot);     // 控制头部左右摆动
            simxSetJointPosition(clientID, Handle_Pitch, PI-euler.X, simx_opmode_oneshot);  // 控制头部俯仰
            
            extApi_sleepMs(5);
        }

        hr = pBody->GetJoints(_countof(joints), joints); // 获得25个关节点
        if (SUCCEEDED(hr))
        {
            // Filtered Joint
            filter.Update(joints);
            const DirectX::XMVECTOR* vec = filter.GetFilteredJoints();    // Retrive Filtered Joints

            for (int type = 0; type < JointType_Count; type++)
            {
                if (joints[type].TrackingState != TrackingState::TrackingState_NotTracked)
                {
                    float x = 0.0f, y = 0.0f, z = 0.0f;
                    // Retrieve the x/y/z component of an XMVECTOR Data and storing that component‘s value in an instance of float referred to by a pointer
                    DirectX::XMVectorGetXPtr(&x, vec[type]);
                    DirectX::XMVectorGetYPtr(&y, vec[type]);
                    DirectX::XMVectorGetZPtr(&z, vec[type]);
                    CameraSpacePoint cameraSpacePoint = { x, y, z };
                    m_pCoordinateMapper->MapCameraPointToDepthSpace(cameraSpacePoint, &depthSpacePosition[type]);
                }
            }
            DrawBody(joints, depthSpacePosition);
            DrawHandState(depthSpacePosition[JointType_HandLeft], leftHandState);
            DrawHandState(depthSpacePosition[JointType_HandRight], rightHandState);
        }
        delete[] depthSpacePosition;
    }
    cv::imshow("skeletonImg", skeletonImg);
    cv::waitKey(5); // 延时5ms
    
    
/// Constructor
CBodyBasics::CBodyBasics() :
m_pKinectSensor(NULL),
m_pCoordinateMapper(NULL),
m_pBodyFrameReader(NULL)
{
    clientID = simxStart((simxChar*)"127.0.0.1", 19999, true, true, 2000, 5); // 连接VREP服务端
    if (clientID != -1)
    {
        std::cout << "Connected to remote API server" << std::endl;

        // Send some data to V-REP in a non-blocking fashion:
        simxAddStatusbarMessage(clientID, "Connected to V-REP!", simx_opmode_oneshot);

        Handle_Yaw = 0;
        Handle_Pitch = 0;
        simxGetObjectHandle(clientID, "HeadYaw",   &Handle_Yaw, simx_opmode_oneshot_wait);   // 获取VREP中Yaw关节的句柄
        simxGetObjectHandle(clientID, "HeadPitch", &Handle_Pitch, simx_opmode_oneshot_wait); // 获取VREP中Pitch关节句柄
    }

}


/// Destructor
CBodyBasics::~CBodyBasics()
{
    SafeRelease(m_pBodyFrameReader);
    SafeRelease(m_pCoordinateMapper);

    if (m_pKinectSensor)
    {
        m_pKinectSensor->Close();
    }
    SafeRelease(m_pKinectSensor);

    // Close the connection to V-REP:   
    simxFinish(clientID);
}

#include <iostream>
#include <stddef.h>
#include <assert.h>
 
using std::cout;
using std::endl;


// Simple_moving_average
class SMA
{
public:
    SMA(unsigned int period) :period(period), window(new double[period]), head(NULL), tail(NULL), total(0)
    {
        assert(period >= 1);
    }
    ~SMA()
    {
        delete[] window;
    }

    // Adds a value to the average, pushing one out if nescessary
    void add(double val)
    {
        // Special case: Initialization
        if (head == NULL)
        {
            head = window;
            *head = val;
            tail = head;
            inc(tail);
            total = val;
            return;
        }

        // Were we already full?
        if (head == tail)
        {
            // Fix total-cache
            total -= *head;
            // Make room
            inc(head);
        }

        // Write the value in the next spot.
        *tail = val;
        inc(tail);

        // Update our total-cache
        total += val;
    }

    // Returns the average of the last P elements added to this SMA.
    // If no elements have been added yet, returns 0.0
    double avg() const
    {
        ptrdiff_t size = this->size();
        if (size == 0)
            return 0; // No entries => 0 average

        return total / (double)size; // Cast to double for floating point arithmetic
    }

private:
    unsigned int period;
    double * window; // Holds the values to calculate the average of.

    // Logically, head is before tail
    double * head; // Points at the oldest element we‘ve stored.
    double * tail; // Points at the newest element we‘ve stored.

    double total; // Cache the total so we don‘t sum everything each time.

    // Bumps the given pointer up by one.
    // Wraps to the start of the array if needed.
    void inc(double * & p)
    {
        if (++p >= window + period)
        {
            p = window;
        }
    }

    // Returns how many numbers we have stored.
    ptrdiff_t size() const
    {
        if (head == NULL)
            return 0;
        if (head == tail)
            return period;
        return (period + tail - head) % period;
    }
};


int main(int argc, char * * argv) 
{
    SMA foo(3);
    SMA bar(5);
 
    int data[] = { 1, 2, 3, 4, 5, 5, 4, 3, 2, 1 };
    for (int * itr = data; itr < data + 10; itr++) 
    {
        foo.add(*itr);
        cout << "Added " << *itr << " avg: " << foo.avg() << endl;
    }
    
    cout << endl;
    
    for (int * itr = data; itr < data + 10; itr++) 
    {
        bar.add(*itr);
        cout << "Added " << *itr << " avg: " << bar.avg() << endl;
    }
 
    return 0;
}