#include "Kinect2Func.h"

using   namespace   std;
using   namespace   cv;

Kinect2Func::Kinect2Func() 
{
    mySensor = nullptr;
    myColorSource = nullptr;
    myColorReader = nullptr;
    myDescription = nullptr;
    myColorFrame = nullptr;
    myBodySource = nullptr;
    myBodyReader = nullptr;
    myBodyFrame = nullptr;
    myMapper = nullptr;
    myBodyIndexSource = nullptr;
    myBodyIndexReader = nullptr;
    myBodyIndexFrame = nullptr;
    myBodyIndexDescription = nullptr;
    depthsource = nullptr;
    depthreader = nullptr;
    depthframe = nullptr;
    depthdescription = nullptr;
    m_bOpen = false;
}

int Kinect2Func::cameraCapture(int CapFreq, int CapTimes) {
    GetDefaultKinectSensor(&mySensor);
    mySensor->Open();

    mySensor->get_ColorFrameSource(&myColorSource);

    myColorSource->OpenReader(&myColorReader);

    int colorHeight = 0, colorWidth = 0;
    myColorSource->get_FrameDescription(&myDescription);
    myDescription->get_Height(&colorHeight);
    myDescription->get_Width(&colorWidth);

    Mat original(colorHeight, colorWidth, CV_8UC4);

    //**********************以上为ColorFrame的读取前准备**************************

    mySensor->get_BodyFrameSource(&myBodySource);

    myBodySource->OpenReader(&myBodyReader);

    int myBodyCount = 0;
    myBodySource->get_BodyCount(&myBodyCount);

    mySensor->get_CoordinateMapper(&myMapper);

    //**********************以上为BodyFrame以及Mapper的准备***********************

    mySensor->get_BodyIndexFrameSource(&myBodyIndexSource);
    int	bodyheight = 0, bodywidth = 0;
    myBodyIndexSource->get_FrameDescription(&myBodyIndexDescription);
    myBodyIndexDescription->get_Height(&bodyheight);
    myBodyIndexDescription->get_Width(&bodywidth);
    myBodyIndexSource->OpenReader(&myBodyIndexReader);
    Mat	img(bodyheight, bodywidth, CV_8UC3);
    Vec3b	color[7] = { Vec3b(0,0,255),Vec3b(0,255,255),Vec3b(255,255,255),Vec3b(0,255,0),Vec3b(255,0,0),Vec3b(255,0,255),Vec3b(0,0,0) };

    //**********************以上为BodyIndexFrame以及提取人体的准备***********************

    mySensor->get_DepthFrameSource(&depthsource);//获取depthFrameSource
    depthsource->get_FrameDescription(&depthdescription);
    int depthheight = 0, depthwidth = 0;
    depthdescription->get_Height(&depthheight);
    depthdescription->get_Width(&depthwidth);
    cout << depthheight << endl << depthwidth << endl;
    depthsource->OpenReader(&depthreader);//获取深度图像Reader

    //**********************以上为depthFrame的准备***********************

    ofstream fs;
    fs.open("skeletonpoints.txt");
    //ofstream fs0;
    //fs0.open("firstskeletonpoints.txt");
    //ofstream fs1;
    //fs1.open("depthtocolor.txt");//depth to color
    //ofstream fs2;
    //fs2.open("depthtocamera.txt");//depth to 3d coordinate
    Mat depthImage(depthheight, depthwidth, CV_16UC1);//创建Mat，用来放深度图像数据
    int result;
    bool firstframe = true;
    int framecount = 0;
    m_bOpen = true;
    while (1)
    {
        framecount++;
        if (!m_bOpen)
        {
            return 0;
        }
        while (myColorReader->AcquireLatestFrame(&myColorFrame) != S_OK);
        myColorFrame->CopyConvertedFrameDataToArray(colorHeight * colorWidth * 4, original.data, ColorImageFormat_Bgra);
        Mat copy = original.clone();        //读取彩色图像并输出到矩阵

        while (myBodyReader->AcquireLatestFrame(&myBodyFrame) != S_OK); //读取身体图像
        IBody   **  myBodyArr = new IBody *[myBodyCount];       //为存身体数据的数组做准备
        for (int i = 0; i < myBodyCount; i++)
            myBodyArr[i] = nullptr;

        while (depthreader->AcquireLatestFrame(&depthframe) != S_OK); //获取最新的深度帧

        //while (myBodyIndexReader->AcquireLatestFrame(&myBodyIndexFrame) != S_OK); //获取身体轮廓

        if (myBodyFrame->GetAndRefreshBodyData(myBodyCount, myBodyArr) == S_OK)     //把身体数据输入数组
            for (int i = 0; i < myBodyCount; i++)
            {
                BOOLEAN     result = false;
                if (myBodyArr[i]->get_IsTracked(&result) == S_OK && result) //先判断是否侦测到
                {
                    Joint   myJointArr[JointType_Count];
                    if (myBodyArr[i]->GetJoints(JointType_Count, myJointArr) == S_OK)   //如果侦测到就把关节数据输入到数组并画图
                    {
                        //for (int j = 0; j < JointType_Count; ++j)
                        //{
                        //    fs << myJointArr[j].Position.X << " " << myJointArr[j].Position.Y << " " << myJointArr[j].Position.Z << endl;
                        //}
                        if (framecount%CapFreq == 0)
                        {
                            fs << myJointArr[0].Position.X * 100 << " " << myJointArr[0].Position.Y * 100 << " " << myJointArr[0].Position.Z * 100 << endl;
                            fs << myJointArr[1].Position.X * 100 << " " << myJointArr[1].Position.Y * 100 << " " << myJointArr[1].Position.Z * 100 << endl;
                            fs << myJointArr[20].Position.X * 100 << " " << myJointArr[20].Position.Y * 100 << " " << myJointArr[20].Position.Z * 100 << endl;
                            fs << myJointArr[3].Position.X * 100 << " " << myJointArr[3].Position.Y * 100 << " " << myJointArr[3].Position.Z * 100 << endl;
                            fs << myJointArr[4].Position.X * 100 << " " << myJointArr[4].Position.Y * 100 << " " << myJointArr[4].Position.Z * 100 << endl;
                            fs << myJointArr[5].Position.X * 100 << " " << myJointArr[5].Position.Y * 100 << " " << myJointArr[5].Position.Z * 100 << endl;
                            fs << myJointArr[6].Position.X * 100 << " " << myJointArr[6].Position.Y * 100 << " " << myJointArr[6].Position.Z * 100 << endl;
                            fs << myJointArr[7].Position.X * 100 << " " << myJointArr[7].Position.Y * 100 << " " << myJointArr[7].Position.Z * 100 << endl;
                            fs << myJointArr[8].Position.X * 100 << " " << myJointArr[8].Position.Y * 100 << " " << myJointArr[8].Position.Z * 100 << endl;
                            fs << myJointArr[9].Position.X * 100 << " " << myJointArr[9].Position.Y * 100 << " " << myJointArr[9].Position.Z * 100 << endl;
                            fs << myJointArr[10].Position.X * 100 << " " << myJointArr[10].Position.Y * 100 << " " << myJointArr[10].Position.Z * 100 << endl;
                            fs << myJointArr[11].Position.X * 100 << " " << myJointArr[11].Position.Y * 100 << " " << myJointArr[11].Position.Z * 100 << endl;
                            fs << myJointArr[12].Position.X * 100 << " " << myJointArr[12].Position.Y * 100 << " " << myJointArr[12].Position.Z * 100 << endl;
                            fs << myJointArr[13].Position.X * 100 << " " << myJointArr[13].Position.Y * 100 << " " << myJointArr[13].Position.Z * 100 << endl;
                            fs << myJointArr[14].Position.X * 100 << " " << myJointArr[14].Position.Y * 100 << " " << myJointArr[14].Position.Z * 100 << endl;
                            fs << myJointArr[15].Position.X * 100 << " " << myJointArr[15].Position.Y * 100 << " " << myJointArr[15].Position.Z * 100 << endl;
                            fs << myJointArr[16].Position.X * 100 << " " << myJointArr[16].Position.Y * 100 << " " << myJointArr[16].Position.Z * 100 << endl;
                            fs << myJointArr[17].Position.X * 100 << " " << myJointArr[17].Position.Y * 100 << " " << myJointArr[17].Position.Z * 100 << endl;
                            fs << myJointArr[18].Position.X * 100 << " " << myJointArr[18].Position.Y * 100 << " " << myJointArr[18].Position.Z * 100 << endl;
                            fs << myJointArr[19].Position.X * 100 << " " << myJointArr[19].Position.Y * 100 << " " << myJointArr[19].Position.Z * 100 << endl;
                        }
                       
                        //if (framecount == 100)
                        //{
                        //    //for (int j = 0; j < JointType_Count; ++j)
                        //    //{
                        //    //    fs0 << myJointArr[j].Position.X << " " << myJointArr[j].Position.Y << " " << myJointArr[j].Position.Z << endl;
                        //    //}
                        //    fs0 << myJointArr[0].Position.X * 100 << " " << myJointArr[0].Position.Y * 100 << " " << myJointArr[0].Position.Z * 100 << endl;
                        //    fs0 << myJointArr[1].Position.X * 100 << " " << myJointArr[1].Position.Y * 100 << " " << myJointArr[1].Position.Z * 100 << endl;
                        //    fs0 << myJointArr[20].Position.X * 100 << " " << myJointArr[20].Position.Y * 100 << " " << myJointArr[20].Position.Z * 100 << endl;
                        //    fs0 << myJointArr[3].Position.X * 100 << " " << myJointArr[3].Position.Y * 100 << " " << myJointArr[3].Position.Z * 100 << endl;
                        //    fs0 << myJointArr[4].Position.X * 100 << " " << myJointArr[4].Position.Y * 100 << " " << myJointArr[4].Position.Z * 100 << endl;
                        //    fs0 << myJointArr[5].Position.X * 100 << " " << myJointArr[5].Position.Y * 100 << " " << myJointArr[5].Position.Z * 100 << endl;
                        //    fs0 << myJointArr[6].Position.X * 100 << " " << myJointArr[6].Position.Y * 100 << " " << myJointArr[6].Position.Z * 100 << endl;
                        //    fs0 << myJointArr[7].Position.X * 100 << " " << myJointArr[7].Position.Y * 100 << " " << myJointArr[7].Position.Z * 100 << endl;
                        //    fs0 << myJointArr[8].Position.X * 100 << " " << myJointArr[8].Position.Y * 100 << " " << myJointArr[8].Position.Z * 100 << endl;
                        //    fs0 << myJointArr[9].Position.X * 100 << " " << myJointArr[9].Position.Y * 100 << " " << myJointArr[9].Position.Z * 100 << endl;
                        //    fs0 << myJointArr[10].Position.X * 100 << " " << myJointArr[10].Position.Y * 100 << " " << myJointArr[10].Position.Z * 100 << endl;
                        //    fs0 << myJointArr[11].Position.X * 100 << " " << myJointArr[11].Position.Y * 100 << " " << myJointArr[11].Position.Z * 100 << endl;
                        //    fs0 << myJointArr[12].Position.X * 100 << " " << myJointArr[12].Position.Y * 100 << " " << myJointArr[12].Position.Z * 100 << endl;
                        //    fs0 << myJointArr[13].Position.X * 100 << " " << myJointArr[13].Position.Y * 100 << " " << myJointArr[13].Position.Z * 100 << endl;
                        //    fs0 << myJointArr[14].Position.X * 100 << " " << myJointArr[14].Position.Y * 100 << " " << myJointArr[14].Position.Z * 100 << endl;
                        //    fs0 << myJointArr[15].Position.X * 100 << " " << myJointArr[15].Position.Y * 100 << " " << myJointArr[15].Position.Z * 100 << endl;
                        //    fs0 << myJointArr[16].Position.X * 100 << " " << myJointArr[16].Position.Y * 100 << " " << myJointArr[16].Position.Z * 100 << endl;
                        //    fs0 << myJointArr[17].Position.X * 100 << " " << myJointArr[17].Position.Y * 100 << " " << myJointArr[17].Position.Z * 100 << endl;
                        //    fs0 << myJointArr[18].Position.X * 100 << " " << myJointArr[18].Position.Y * 100 << " " << myJointArr[18].Position.Z * 100 << endl;
                        //    fs0 << myJointArr[19].Position.X * 100 << " " << myJointArr[19].Position.Y * 100 << " " << myJointArr[19].Position.Z * 100 << endl;
                        //}
                        draw(copy, myJointArr[JointType_Head], myJointArr[JointType_Neck], myMapper);
                        draw(copy, myJointArr[JointType_Neck], myJointArr[JointType_SpineShoulder], myMapper);

                        draw(copy, myJointArr[JointType_SpineShoulder], myJointArr[JointType_ShoulderLeft], myMapper);
                        draw(copy, myJointArr[JointType_SpineShoulder], myJointArr[JointType_SpineMid], myMapper);
                        draw(copy, myJointArr[JointType_SpineShoulder], myJointArr[JointType_ShoulderRight], myMapper);

                        draw(copy, myJointArr[JointType_ShoulderLeft], myJointArr[JointType_ElbowLeft], myMapper);
                        draw(copy, myJointArr[JointType_SpineMid], myJointArr[JointType_SpineBase], myMapper);
                        draw(copy, myJointArr[JointType_ShoulderRight], myJointArr[JointType_ElbowRight], myMapper);

                        draw(copy, myJointArr[JointType_ElbowLeft], myJointArr[JointType_WristLeft], myMapper);
                        draw(copy, myJointArr[JointType_SpineBase], myJointArr[JointType_HipLeft], myMapper);
                        draw(copy, myJointArr[JointType_SpineBase], myJointArr[JointType_HipRight], myMapper);
                        draw(copy, myJointArr[JointType_ElbowRight], myJointArr[JointType_WristRight], myMapper);

                        draw(copy, myJointArr[JointType_WristLeft], myJointArr[JointType_ThumbLeft], myMapper);
                        draw(copy, myJointArr[JointType_WristLeft], myJointArr[JointType_HandLeft], myMapper);
                        draw(copy, myJointArr[JointType_HipLeft], myJointArr[JointType_KneeLeft], myMapper);
                        draw(copy, myJointArr[JointType_HipRight], myJointArr[JointType_KneeRight], myMapper);
                        draw(copy, myJointArr[JointType_WristRight], myJointArr[JointType_ThumbRight], myMapper);
                        draw(copy, myJointArr[JointType_WristRight], myJointArr[JointType_HandRight], myMapper);

                        draw(copy, myJointArr[JointType_HandLeft], myJointArr[JointType_HandTipLeft], myMapper);
                        draw(copy, myJointArr[JointType_KneeLeft], myJointArr[JointType_FootLeft], myMapper);
                        draw(copy, myJointArr[JointType_KneeRight], myJointArr[JointType_FootRight], myMapper);
                        draw(copy, myJointArr[JointType_HandRight], myJointArr[JointType_HandTipRight], myMapper);
                    }
                }
            }
        resize(copy, copy, cv::Size(copy.cols / 2, copy.rows / 2));
        imshow("skeleton", copy);//带skeleton节点的colorimg
        //if (framecount == 100)
        //{
        //    imwrite("colorImg.png", original);//绘制图像
        //}
        delete[]myBodyArr;
        myBodyFrame->Release();
        myColorFrame->Release();

        if (myBodyIndexReader->AcquireLatestFrame(&myBodyIndexFrame) == S_OK)
        {
            UINT	size = 0;
            BYTE	* buffer = nullptr;
            myBodyIndexFrame->AccessUnderlyingBuffer(&size, &buffer);
            for (int i = 0; i < bodyheight; i++)
                for (int j = 0; j < bodywidth; j++)
                {
                    int	index = buffer[i * bodywidth + j];		//0-5代表人体，其它值代表背景，用此将人体和背景渲染成不同颜色
                    if (index <= 5)
                        img.at<Vec3b>(i, j) = color[index];
                    else
                        img.at<Vec3b>(i, j) = color[6];
                }
            //imshow("bodyindex", img);
            //if (framecount == 100)
            //{
            //    imwrite("maskImg.png", img);//绘制图像
            //}
            myBodyIndexFrame->Release();
        }

        depthframe->CopyFrameDataToArray(depthheight * depthwidth, depthArray);//图像数据流转换
        for (int i = 0; i < depthheight; i++)//遍历图像
        {
            for (int j = 0; j < depthwidth; j++)
            {
                depthImage.at<UINT16>(i, j) = static_cast<UINT16>(depthArray[i * depthwidth + j]);
            }
        }
        depthframe->Release();//每一次获取帧后都要释放
        //if (framecount == 100)
        //{
        //    imwrite("depthImg.png", depthImage);//绘制图像
        //}
        //imwrite("Depth.png", depthImage);//绘制图像
        //imshow("Depth", depthImage);//绘制图像

        //if (firstframe&&framecount==100)
        //{
        //    result = myMapper->MapDepthFrameToColorSpace(512 * 424, reinterpret_cast<UINT16*>(depthImage.data), 512 * 424, m_pColorFromDepth);
        //    //Mat i_depthToRgb(424, 512, CV_8UC4);
        //    if (SUCCEEDED(result))
        //    {
        //        for (int i = 0; i < 424 * 512; i++)
        //        {
        //            ColorSpacePoint p = m_pColorFromDepth[i];
        //            if (p.X != -std::numeric_limits<float>::infinity() && p.Y != -std::numeric_limits<float>::infinity())
        //            {
        //                int colorX = static_cast<int>(p.X + 0.5f);
        //                int colorY = static_cast<int>(p.Y + 0.5f);

        //                //if ((colorX >= 0 && colorX < 1920) && (colorY >= 0 && colorY < 1080))
        //                //{
        //                    //i_depthToRgb.data[i * 4] = colorImage.data[(colorY * 1920 + colorX) * 4];
        //                    //i_depthToRgb.data[i * 4 + 1] = colorImage.data[(colorY * 1920 + colorX) * 4 + 1];
        //                    //i_depthToRgb.data[i * 4 + 2] = colorImage.data[(colorY * 1920 + colorX) * 4 + 2];
        //                    //i_depthToRgb.data[i * 4 + 3] = colorImage.data[(colorY * 1920 + colorX) * 4 + 3];
        //                fs1 << i % 512 << " " << i / 512 << " " << colorX << " " << colorY << std::endl;
        //                //    }
        //            }
        //            else
        //            {
        //                fs1 << i % 512 << " " << i / 512 << " " << -1 << " " << -1 << std::endl;
        //            }
        //        }
        //    }
        //    //imwrite("depthToRgb.png", i_depthToRgb);
        //    //imshow("depthToRgb", i_depthToRgb);
        //    result = myMapper->MapDepthFrameToCameraSpace(512 * 424, reinterpret_cast<UINT16*>(depthImage.data), 512 * 424, m_pDepthCoordinates);
        //    for (int i = 0; i < 424 * 512; i++)
        //    {
        //        if (m_pDepthCoordinates[i].X != -std::numeric_limits<float>::infinity() && m_pDepthCoordinates[i].Y != -std::numeric_limits<float>::infinity() && m_pDepthCoordinates[i].Z != -std::numeric_limits<float>::infinity())
        //        {
        //            fs2 << i % 512 << " " << i / 512 << " " << m_pDepthCoordinates[i].X*100 << " " << m_pDepthCoordinates[i].Y * 100 << " " << m_pDepthCoordinates[i].Z * 100 << endl;
        //        }
        //        else
        //        {
        //            fs2 << i % 512 << " " << i / 512 << " " << -1 << " " << -1 << " " << -1 << endl;
        //        }
        //    }
        //    firstframe = false;
        //}
        if (framecount == CapTimes)
            break;
        if (waitKey(5) == VK_ESCAPE)
            break;
    }
    fs.close();
    //fs0.close();
    //fs1.close();
    //fs2.close();
    myMapper->Release();

    myColorSource->Release();
    myColorReader->Release();
    myDescription->Release();
    myBodySource->Release();
    myBodyReader->Release();
    myBodyIndexSource->Release();
    myBodyIndexReader->Release();
    myBodyIndexDescription->Release();
    depthsource->Release();
    depthreader->Release();
    depthdescription->Release();
    mySensor->Close();
    mySensor->Release();
    cv::destroyAllWindows();
    m_bOpen = false;
    return  0;
}

int Kinect2Func::cameraShutDown()
{
    m_bOpen = false;
    if (myMapper)
        myMapper->Release();
    if (myColorSource)
        myColorSource->Release();
    if(myColorReader)
        myColorReader->Release();
    if (myDescription)
        myDescription->Release();
    if(myBodySource)
        myBodySource->Release();
    if(myBodyReader)
        myBodyReader->Release();
    if(myBodyIndexSource)
        myBodyIndexSource->Release();
    if(myBodyIndexReader)
        myBodyIndexReader->Release();
    if(myBodyIndexDescription)
        myBodyIndexDescription->Release();
    if(depthsource)
        depthsource->Release();
    if(depthreader)
        depthreader->Release();
    if(depthdescription)
        depthdescription->Release();
    if (mySensor)
    {
        mySensor->Close();
        mySensor->Release();
    }
    cv::destroyAllWindows();
    return 0;
}

void Kinect2Func::draw(Mat & img, Joint & r_1, Joint & r_2, ICoordinateMapper * myMapper)
{
    //用两个关节点来做线段的两端，并且进行状态过滤
    if (r_1.TrackingState == TrackingState_Tracked && r_2.TrackingState == TrackingState_Tracked)
    {
        ColorSpacePoint t_point;    //要把关节点用的摄像机坐标下的点转换成彩色空间的点
        Point   p_1, p_2;
        myMapper->MapCameraPointToColorSpace(r_1.Position, &t_point);
        p_1.x = t_point.X;
        p_1.y = t_point.Y;
        myMapper->MapCameraPointToColorSpace(r_2.Position, &t_point);
        p_2.x = t_point.X;
        p_2.y = t_point.Y;

        line(img, p_1, p_2, Vec3b(0, 255, 0), 5);
        circle(img, p_1, 10, Vec3b(255, 0, 0), -1);
        circle(img, p_2, 10, Vec3b(255, 0, 0), -1);
    }
}