#!/usr/bin/env python

import numpy as np
import sys
import re
from math import cos, sin, acos, sqrt, ceil

import yaml
import threading
import copy

import rospy
import tf
import geometry_msgs.msg
from std_msgs.msg import *
from nav_msgs.msg import OccupancyGrid, MapMetaData
from visualization_msgs.msg import Marker, MarkerArray
from geometry_msgs.msg import Polygon, PolygonStamped, PointStamped
import rospkg
import tf
from people_tracking.msg import PersonLocation, PersonLocationArray
from contamination_monitor.msg import ContamPersonLocation, ContamPersonLocationArray

from tf.transformations import euler_from_quaternion
from tf.transformations import quaternion_from_euler
from tf.transformations import quaternion_inverse
from tf import transformations

import os
import timeit
import subprocess


### For this, we don't care so much about ppl, so much as moving objects, thus keep track of object id
###   and contamination level

class WarnSystem():
    NOT_CONTAMINATED = -1
    FULLY_CONTAMINATED = 100
    UNKNOWN_CONTAMINATED = 50
    
    def __init__(self):
        self.contam_level = -1 # neg 1 corresponds to unkonwn in occupancy grid
        self.lock = threading.Lock()

        self.ogrid = OccupancyGrid()
        self.step = 0
        self.offset = (0,0)

        self.area_extension = 0.5

        self.objects_contam_level = {} # stores tracked objects contamination level, k=ob_id, v=contam level

        self.tf_listener = tf.TransformListener()
        
        self.ppl_warnings = dict()

        # Setup default filepath 
        rospack = rospkg.RosPack()
        pkg_path = rospack.get_path('contamination_monitor')
         
        self.occ_grid_sub = rospy.Subscriber("contamination_grid", OccupancyGrid, self.occ_grid_update, queue_size=10)        
        self.contam_person_sub = rospy.Subscriber("contam_people_locations", ContamPersonLocationArray, self.contam_person_update, queue_size=10)
           

    def occ_grid_update(self, ogrid_msg):
        with self.lock:
            self.ogrid = ogrid_msg
            self.step = ogrid_msg.info.resolution
            self.offset = (ogrid_msg.info.origin.position.x, ogrid_msg.info.origin.position.y)

    def contam_person_update(self, cont_person_loc_msg):     
        for contam_person_loc in cont_person_loc_msg.contam_people_location:

            person_loc = contam_person_loc.person_location
            contam_level = contam_person_loc.contamination_level
            # print "person: ", person_loc.name
            # print "contam_level: ", contam_level
            
            person_id = person_loc.name
            x = person_loc.pose.position.x
            y = person_loc.pose.position.y
            z = person_loc.pose.position.z
            orientation = person_loc.pose.orientation
            a = person_loc.ellipse_a
            b = person_loc.ellipse_b
            theta = person_loc.ellipse_theta
            frame_id = person_loc.header.frame_id

            # Check to see if human needs to be warned
            if self.needs_warning(person_id, contam_level, (x,y,z) , a, b, theta):
                self.send_warning(person_id, (x,y,z))
                self.log_warning(person_id, (x,y,z), rospy.Time.now())

                        



    def needs_warning(self, person_id, contam_level, ellipse_center, ellipse_a, ellipse_b, ellipse_theta, dist_thresh = 1.0, time_thresh = 20.0):