using OpenCVForUnity.CoreModule;
using OpenCVForUnity.DnnModule;
using OpenCVForUnity.ImgprocModule;
using OpenCVForUnity.UnityUtils;
using System;
using System.Collections.Generic;
using UnityEngine;
using Yoga;

public class OpenPoseEsimater : Estimator
{
    KeypointsModel _openPoseModel; 
    private Net _net;

    private double threshold = 0.5;
    public override void InitModel()
    {
        _net = null;
        var modelFilePath = Utils.getFilePath(YogaConfig.MODEL_PATHS[ModelType.OpenPose]);
        if (string.IsNullOrEmpty(modelFilePath))
        {
            Debug.LogError("modelFilePath is empty. Please copy from “OpenCVForUnity/StreamingAssets/” to “Assets/StreamingAssets/” folder. ");
            return;
        }

        _net = Dnn.readNet(modelFilePath);

        _openPoseModel = new KeypointsModel(_net);
        _openPoseModel.setInputScale(new Scalar(YogaConfig.InScale));
        _openPoseModel.setInputSize(new Size(YogaConfig.InWidth, YogaConfig.InHeight));
        _openPoseModel.setInputMean(new Scalar(YogaConfig.InMean));
        _openPoseModel.setInputSwapRB(false);
        _openPoseModel.setInputCrop(false);
    }

    public override void DisposeModel()
    {
        if (_net != null)
            _net.Dispose();
    }

    public override bool Esitmate(Mat bgrMat, List<float[]> voloResultBox, out List<Point> points)
    {
        points = null;

        if (_openPoseModel == null)
            throw new ArgumentException("CVEstimator.Init: args[1] is not KeypointsModel")
;
        OpenCVForUnity.CoreModule.Rect roiRect = new OpenCVForUnity.CoreModule.Rect(
                (int)voloResultBox[0][0],
                (int)voloResultBox[0][1],
                Math.Abs((int)(voloResultBox[0][2] - voloResultBox[0][0])),
                Math.Abs((int)(voloResultBox[0][3] - voloResultBox[0][1])));

        if (roiRect.y < 0 || //0 <= _rowRange.start
            (roiRect.y + roiRect.height) < roiRect.y || // _rowRange.start <= _rowRange.end
            bgrMat.rows() < (roiRect.y + roiRect.height)) //_rowRange.end <= m.rows 
            return false; //重新检测

        Mat personRectImg = new Mat(bgrMat, roiRect);//获取人体区域
        points = _openPoseModel.estimate(personRectImg, (float)threshold).toList();
        //将人体区域的坐标转换为原图坐标
        for (int j = 0; j < points.Count; j++)
        {
            if (points[j] == null ||
                (points[j].x == -1 && points[j].y == -1)) //没找到的点，跳过
                continue;

            points[j].x += roiRect.x;
            points[j].y += roiRect.y;
        }

        if (!YogaManager.Instance.ActionCheckPoints(points))
        {
            Debug.Log("ActionCheckPoints failed. Re-Estimation.");
            return false; //重新检测
        }
        return true;
    }

    public override void Check(ref Mat img)
    {
        if (_net == null)
        {
            Imgproc.putText(img, "model file is not loaded.", new Point(5, img.rows() - 30), Imgproc.FONT_HERSHEY_SIMPLEX, 0.7, new Scalar(255, 255, 255), 2, Imgproc.LINE_AA, false);
            Imgproc.putText(img, "Please read console message.", new Point(5, img.rows() - 10), Imgproc.FONT_HERSHEY_SIMPLEX, 0.7, new Scalar(255, 255, 255), 2, Imgproc.LINE_AA, false);
        }
    }
}