# Copyright (c) Facebook, Inc. and its affiliates. All Rights Reserved.
import argparse
import cv2

from openpose.config import cfg
from openpose.engine.predictor import LiveDemo
from openpose.model.detector.build_model import build_model
from openpose.data import make_data_loader
import time
import matplotlib.pyplot as plt
import matplotlib.patches as plt_patches
import numpy as np
heat_cmap = plt.get_cmap('jet')


def main():
    parser = argparse.ArgumentParser(description="PyTorch Object Detection Webcam Demo")
    parser.add_argument(
        "--config-file",
        default="../configs/caffe2/e2e_mask_rcnn_R_50_FPN_1x_caffe2.yaml",
        metavar="FILE",
        help="path to config file",
    )
    parser.add_argument(
        "--confidence-threshold",
        type=float,
        default=0.7,
        help="Minimum score for the prediction to be shown",
    )
    parser.add_argument(
        "--min-image-size",
        type=int,
        default=224,
        help="Smallest size of the image to feed to the model. "
            "Model was trained with 800, which gives best results",
    )
    parser.add_argument(
        "--show-mask-heatmaps",
        dest="show_mask_heatmaps",
        help="Show a heatmap probability for the top masks-per-dim masks",
        action="store_true",
    )
    parser.add_argument(
        "--masks-per-dim",
        type=int,
        default=2,
        help="Number of heatmaps per dimension to show",
    )
    parser.add_argument(
        "opts",
        help="Modify model config options using the command-line",
        default=None,
        nargs=argparse.REMAINDER,
    )

    args = parser.parse_args()

    # load config from file and command-line arguments
    cfg.merge_from_file(args.config_file)
    cfg.merge_from_list(args.opts)
    cfg.freeze()
    # prepare object that handles inference plus adds predictions on top of image
    
    # create the model
    model = build_model(cfg,'OpenPose')

    live_demo = LiveDemo(
        cfg,
        model,
        confidence_threshold=args.confidence_threshold,
        show_mask_heatmaps=args.show_mask_heatmaps,
        masks_per_dim=args.masks_per_dim,
        min_image_size=args.min_image_size,
    )
    cam = cv2.VideoCapture(0)

    kps_name = [
        'background',
        'nose',
        'left_eye',
        'right_eye',
        'left_ear',
        'right_ear',
        'left_shoulder',
        'right_shoulder',
        'left_elbow',
        'right_elbow',
        'left_wrist',
        'right_wrist',
        'left_hip',
        'right_hip',
        'left_knee',
        'right_knee',
        'left_ankle',
        'right_ankle'
    ]
    vis_ch = 0
    max_ch = 17
    vis_lvl = 0
    while True:
        ret_val, img = cam.read()
        prediction,features = live_demo.run_on_opencv_image(img)
        overlay = img.copy()
        overlay = live_demo.overlay_boxes(overlay,prediction)
        overlay = live_demo.overlay_keypoints(overlay,prediction)

        ##overlay heatmap
        heatmaps = []
        for lvl in range(4):
            heat_lvl = features[lvl].cpu().numpy()[0,vis_ch]
            heatmaps.append(heat_lvl)
        cn_size = heatmaps[0].shape[::-1]
        for lvl in range(1,4):
            heatmaps[lvl] = cv2.resize(heatmaps[lvl],cn_size)
        
        #heatmap = np.sum(heatmaps,axis=0)/4.0
        heatmap = heatmaps[vis_lvl]

        overlay_heat = live_demo.overlay_heatmap(img,heatmap)

        font = cv2.FONT_HERSHEY_SIMPLEX
        overlay_heat = cv2.putText(overlay_heat,'channel:{}, lvl:{}'.format(kps_name[vis_ch],vis_lvl),(10,20),font,1,(0,255,0),2)

        cv2.imshow('heatmap',overlay_heat)
        cv2.imshow('img',overlay)
        retkey=cv2.waitKey(1)
        if retkey==ord('q'):
            break
        elif retkey==ord('d'):
            vis_ch += 1
        elif retkey==ord('a'):
            vis_ch -= 1
        elif retkey==ord('j'):
            vis_lvl += 1
        elif retkey==ord('k'):
            vis_lvl -= 1


        if vis_lvl >= 3:
            vis_lvl = 3
        elif vis_lvl <0:
            vis_lvl = 0
        if vis_ch >= max_ch:
            vis_ch = max_ch
        elif vis_ch <0 :
            vis_ch = 0


    cv2.destroyAllWindows()
        # for lvl in range(4):
        #     ax2[lvl].imshow(features[lvl][0,0,:,:])
        # plt.draw()
        # plt.pause(0.01)
        # #input()
    
    # while True:
    #     start_time = time.time()
    #     #ret_val, img = cam.read()
    #     composite = live_demo.run_on_opencv_image(img)
    #     print(composite)
    #     print("Time: {:.2f} s / img".format(time.time() - start_time))
    #     cv2.imshow("COCO detections", img)
    #     if cv2.waitKey(1) == 27:
    #         break  # esc to quit
    # cv2.destroyAllWindows()


if __name__ == "__main__":
    main()
