/**
 * Copyright (c) 2020 Samsung Electronics Co., Ltd All Rights Reserved
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "mv_private.h"
#include "Posture.h"

#include <ios>
#include <fstream>
#include <sstream>
#include <string>
#include <unistd.h>
#include <cfloat>

#define POSE_OFFSET_VALUE 20

namespace mediavision
{
namespace inference
{
Posture::Posture() :
    mBvhParser(),
    mBvh(),
    mPose()
{
    LOGI("ENTER");

    mMotionToPoseMap.clear();
    mPose.assign(HUMAN_POSE_MAX_LANDMARKS, std::make_pair(false, cv::Point(-1,-1)));

    mPoseParts.assign(HUMAN_POSE_MAX_PARTS, std::make_pair(false, std::vector<cv::Vec2f>()));

    LOGI("LEAVE");
}

Posture::~Posture()
{
    LOGI("ENTER");

    std::vector<std::pair<bool, cv::Point>>().swap(mPose);

    LOGI("LEAVE");
}


int Posture::getParts(int parts,
                    std::vector<std::pair<bool, cv::Point>>& pose,
                    std::vector<std::pair<bool, std::vector<cv::Vec2f>>>& posePart)
{
    LOGI("ENTER");
    // head
    if (parts & MV_INFERENCE_HUMAN_BODY_PART_HEAD) {
        LOGI("HEAD");
        if (pose[0].first == false || pose[1].first == false || pose[2].first == false) {
            posePart[0].first = false;
        } else {
            posePart[0].first = true;
            posePart[0].second.push_back(getUnitVectors(pose[0].second, pose[1].second));
            posePart[0].second.push_back(getUnitVectors(pose[1].second, pose[2].second));
        }
    }

    // right arm
    if (parts & MV_INFERENCE_HUMAN_BODY_PART_ARM_RIGHT) {
        LOGI("ARM-R");
        if (pose[3].first == false || pose[4].first == false || pose[5].first == false) {
            posePart[1].first = false;
        } else {
            posePart[1].first = true;
            posePart[1].second.push_back(getUnitVectors(pose[3].second, pose[4].second));
            posePart[1].second.push_back(getUnitVectors(pose[4].second, pose[5].second));
        }
    }

    // left arm
    if (parts & MV_INFERENCE_HUMAN_BODY_PART_ARM_LEFT) {
        LOGI("ARM-L");
        if (pose[6].first == false || pose[7].first == false || pose[8].first == false) {
            posePart[2].first = false;
        } else {
            posePart[2].first = true;
            posePart[2].second.push_back(getUnitVectors(pose[6].second, pose[7].second));
            posePart[2].second.push_back(getUnitVectors(pose[7].second, pose[8].second));
        }
    }

    // right leg
    if (parts & MV_INFERENCE_HUMAN_BODY_PART_LEG_RIGHT) {
        LOGI("LEG-R");
        if (pose[10].first == false || pose[11].first == false || pose[12].first == false) {
            posePart[3].first = false;
        } else {
            posePart[3].first = true;
            posePart[3].second.push_back(getUnitVectors(pose[10].second, pose[11].second));
            posePart[3].second.push_back(getUnitVectors(pose[11].second, pose[12].second));
        }
    }

    // left leg
    if (parts & MV_INFERENCE_HUMAN_BODY_PART_LEG_LEFT) {
        LOGI("LEG-L");
        if (pose[13].first == false || pose[14].first == false || pose[15].first == false) {
            posePart[4].first = false;
        } else {
            posePart[4].first = true;
            posePart[4].second.push_back(getUnitVectors(pose[13].second, pose[14].second));
            posePart[4].second.push_back(getUnitVectors(pose[14].second, pose[15].second));

        }
    }

    // body
    if (parts & MV_INFERENCE_HUMAN_BODY_PART_BODY) {
        LOGI("BODY");
        if (pose[2].first == false || pose[9].first == false ||
            pose[10].first == false || pose[13].first == false) {
            posePart[5].first = false;
        } else {
            posePart[5].first = true;
            posePart[5].second.push_back(getUnitVectors(pose[2].second, pose[9].second));
            posePart[5].second.push_back(getUnitVectors(pose[9].second, pose[10].second));
            posePart[5].second.push_back(getUnitVectors(pose[9].second, pose[13].second));
        }
    }

    LOGI("LEAVE");

    return MEDIA_VISION_ERROR_NONE;
}

int Posture::setPoseFromFile(const std::string motionCaptureFilePath, const std::string motionMappingFilePath)
{
    LOGI("ENTER");

    int ret = MEDIA_VISION_ERROR_NONE;

    // parsing motion capture file
    LOGD("%s", motionCaptureFilePath.c_str());
    LOGD("%s", motionMappingFilePath.c_str());
    ret = mBvhParser.parse(motionCaptureFilePath.c_str(), &mBvh);
    LOGD("frames: %d",mBvh.num_frames());

    if (ret != MEDIA_VISION_ERROR_NONE) {
        LOGE("Fail to parse a file [%s]", motionCaptureFilePath.c_str());
        return MEDIA_VISION_ERROR_INTERNAL;
    }

    mBvh.recalculate_joints_ltm();

    LOGD("reading motion mapping....");
    // read motion mapping file
    std::ifstream fp(motionMappingFilePath.c_str());
    if (!fp.is_open()) {
        LOGE("Fail to open %s", motionMappingFilePath.c_str());
        return MEDIA_VISION_ERROR_INVALID_PATH;
    }

    std::string line;
    mMotionToPoseMap.clear();
    while (!fp.eof()) {
        std::getline(fp, line);

        if (line.empty())
            continue;

        LOGD("%s", line.c_str());
        std::istringstream lineStream(line);
        std::string token;
        std::vector<std::string> parsedString;
        while(getline(lineStream, token, ',')) {
            parsedString.push_back(token);
        }
        LOGD("name: %s, mapping index: %d", parsedString[0].c_str(), std::stoi(parsedString[1]));
        mMotionToPoseMap.insert(make_pair(parsedString[0], std::stoi(parsedString[1])));
    }

    fp.close();
    LOGD("mapping size is %zd", mMotionToPoseMap.size());

    // convert bvh to pose
    //convertBvhToPose();
    //for (std::shared_ptr<Bvh::Joint>)

    float pointX, pointY, minX, minY, maxX, maxY;
    minX = minY = FLT_MAX;
    maxX = maxY = FLT_MIN;
    for (std::shared_ptr<Joint> joint : mBvh.joints()) {
        std::map<std::string, int>::iterator it = mMotionToPoseMap.find(std::string(joint->name()));
        if (it != mMotionToPoseMap.end()) {
            pointX = joint->pos(0)[0];
            pointY = joint->pos(0)[1];
            if (pointX < minX)
                minX = pointX;

            if (pointY < minY)
                minY = pointY;

            if (pointX > maxX)
                maxX = pointX;

            if (pointY > maxY)
                maxY = pointY;

            mPose[it->second].first = true;
            mPose[it->second].second = cv::Point(pointX, pointY);
            LOGD("%d: (%f,%f)", it->second, pointX, pointY);
        }
    }

    // add offset to make x > 0 and y > 0
    int height = (int)maxY - (int)minY + POSE_OFFSET_VALUE;
    for (std::vector<std::pair<bool, cv::Point>>::iterator iter = mPose.begin();
        iter != mPose.end(); iter++) {
        if (iter->first == false)
            continue;

        iter->second.x -= minX;
        iter->second.y -= minY;

        iter->second.x += POSE_OFFSET_VALUE;
        iter->second.y += POSE_OFFSET_VALUE;

        iter->second.y = height - iter->second.y;

        LOGE("(%d, %d)", iter->second.x, iter->second.y);
    }

    ret = getParts((MV_INFERENCE_HUMAN_BODY_PART_HEAD |
                    MV_INFERENCE_HUMAN_BODY_PART_ARM_RIGHT |
                    MV_INFERENCE_HUMAN_BODY_PART_ARM_LEFT |
                    MV_INFERENCE_HUMAN_BODY_PART_BODY |
                    MV_INFERENCE_HUMAN_BODY_PART_LEG_RIGHT |
                    MV_INFERENCE_HUMAN_BODY_PART_LEG_LEFT),
                    mPose, mPoseParts);
    if (ret != MEDIA_VISION_ERROR_NONE) {
        LOGE("Fail to getPartse");
        return ret;
    }

    LOGI("LEAVE");

    return ret;
}

cv::Vec2f Posture::getUnitVectors(cv::Point point1, cv::Point point2)
{
    LOGI("ENTER");

    cv::Vec2i vec(point1.x - point2.x, point1.y - point2.y);
    cv::Vec2f unitVec (vec[0]/cv::norm(vec, cv::NORM_L1), vec[1]/cv::norm(vec, cv::NORM_L1));

    LOGI("LEAVE");

    return unitVec;
}

float Posture::cosineSimilarity(std::vector<cv::Vec2f> vec1, std::vector<cv::Vec2f> vec2, int size)
{
    float numer = 0.0f;
    float denom1 = 0.0f;
    float denom2 = 0.0f;

    float value = 0.0f;

    for (int k = 0; k < size; ++k) {
        numer = denom1 = denom2 = 0.0f;
        for (int dim = 0; dim <2; ++dim) {
            numer += (vec1[k][dim] * vec2[k][dim]);
            denom1 += (vec1[k][dim] * vec1[k][dim]);
            denom2 += (vec2[k][dim] * vec2[k][dim]);
        }
        LOGI("similarity: %f", numer / sqrt( denom1 * denom2));
        value += numer / sqrt( denom1 * denom2);

    }

    return value;
}

float Posture::getSimilarity(int parts,
					std::vector<std::pair<bool, std::vector<cv::Vec2f>>>& posePart,
					std::vector<std::pair<bool, std::vector<cv::Vec2f>>>& actionPart)
{
    float score = 0.0f;
    int bodyCount = 0;
    std::vector<int> index;

    if (parts & MV_INFERENCE_HUMAN_BODY_PART_HEAD) {
        index.push_back(0);
    }

    if (parts & MV_INFERENCE_HUMAN_BODY_PART_ARM_RIGHT) {
        index.push_back(1);
    }

    if (parts & MV_INFERENCE_HUMAN_BODY_PART_ARM_LEFT) {
        index.push_back(2);
    }

    if (parts & MV_INFERENCE_HUMAN_BODY_PART_LEG_RIGHT) {
        index.push_back(3);
    }

    if (parts & MV_INFERENCE_HUMAN_BODY_PART_LEG_LEFT) {
        index.push_back(4);
    }

    if (parts & MV_INFERENCE_HUMAN_BODY_PART_BODY) {
        index.push_back(5);
    }

    for (std::vector<int>::iterator it = index.begin(); it != index.end(); ++it) {
        if (posePart[(*it)].first && actionPart[(*it)].first &&
            (posePart[(*it)].second.size() == actionPart[(*it)].second.size())) {
            score += cosineSimilarity(posePart[(*it)].second, actionPart[(*it)].second, posePart[(*it)].second.size());

            bodyCount += posePart[(*it)].second.size();
            LOGI("body[%d], score[%f], count[%d]", (*it), score, bodyCount);
        }
    }

    score /= (float)bodyCount;
    LOGD("score: %1.3f", score);

    return score;

}

int Posture::compare(int parts, std::vector<std::pair<bool, cv::Point>> action, float* score)
{
    LOGI("ENTER");

    std::vector<std::pair<bool, std::vector<cv::Vec2f>>> actionParts;
    actionParts.assign(6, std::make_pair(false, std::vector<cv::Vec2f>()));
    int ret = getParts(parts, action, actionParts);
    if (ret != MEDIA_VISION_ERROR_NONE) {
        LOGE("Fail to getPartse");
        return ret;
    }

    *score = getSimilarity(parts, mPoseParts, actionParts);

    LOGI("LEAVE");

    return MEDIA_VISION_ERROR_NONE;
}

}
} // namespace