#!/usr/bin/env python

import math
import rospy
import re
from datetime import datetime
from nav_msgs.msg import Odometry  
from gazebo_msgs.srv import GetModelState
from gazebo_msgs.srv import GetWorldProperties 


TurtlebotBurger = 0.05
WriteFlag = 0
now = datetime.now()
current_time = now.strftime("%H:%M:%S")
logtime = str(current_time) + "_log.txt"
f = open(logtime,"w+")

class Block:
    def __init__(self, name, relative_entity_name, radius):
        self._name = name
        self._relative_entity_name = relative_entity_name
        self._radius = radius


class Tutorial:

    _blockListDict = {
        
    }

    def show_gazebo_models(self, botx, boty):
      get_world_properties = rospy.ServiceProxy('/gazebo/get_world_properties', GetWorldProperties)
      object_list = list()
      resp1 = get_world_properties()
      #print(str(resp1.sim_time))
      i = 0
      for model in resp1.model_names:
        if ("ball" in str(model)) or ("cylinder" in str(model)) or ("box" in str(model) or ("sphere" in str(model))):
           matches = re.findall(r"[-+]?\d*\.\d+|\d+", str(model))
           rad = 0
           if matches:
             if "box" in str(model):
              rad = (matches[0])
              rad = float(rad)/2.0
             else:
                rad = (matches[0])
           s = 'block_' + str(i)
           self._blockListDict.update( {s : Block(model , 'link' , rad)})
           i = i+1
           #print(block)
      
  


      try:
            global f 
            global WriteFlag
            model_coordinates = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState)
            for block in self._blockListDict.itervalues():
                blockName = str(block._name)
                if blockName in resp1.model_names:
                 resp_coordinates = model_coordinates(blockName, block._relative_entity_name)
                 l_x = resp_coordinates.pose.position.x
                 l_y = resp_coordinates.pose.position.y
                 print(blockName)
                 if WriteFlag == 0:
                  f.write(str(blockName) + "\n")
                 print("X: " + str(l_x))
                 print("Y: " + str(l_y))
                 if WriteFlag == 0:
                  f.write("[T:" + str(resp1.sim_time) + "] " + "Bot X: " + str(l_x) + "\n")
                  f.write("[T:" + str(resp1.sim_time) + "] " + "Bot Y: " + str(l_y) + "\n")                
                 dist = distance(botx, boty, l_x, l_y)
                 safedist = 0
                 if block._radius != 0:
                   if "box" in str(blockName):
                       boxkdetection(float(block._radius), botx, boty, l_x, l_y, resp1.sim_time)
                   else: 
                      safedist = float(block._radius) + TurtlebotBurger + 0.05 #Radius/Size + Bot front Rad + 0.05 safe dist 
                      print("Safe " + str(safedist))
                      if WriteFlag == 0:
                       f.write("Safedist.: " + str(safedist) + "\n")
                      print("Dist: " + str(dist))
                      if WriteFlag == 0:
                       f.write("[T:" + str(resp1.sim_time) + "] " + "Dist. To Collision Point: " + str(dist) + "\n")
                      if dist < safedist:                        
                         rospy.loginfo("Collision!")
                         if WriteFlag == 0:
                           f.write("[T:" + str(resp1.sim_time) + "] " + "Collision!")
                           f.close
                           WriteFlag = 1
                 else:
                   
                   print("Dist: " + str(dist))
                   if WriteFlag == 0:
                    f.write("[T:" + str(resp1.sim_time) + "] " + "Dist: " + str(dist) + "\n")

      except rospy.ServiceException as e:
             rospy.loginfo("Get Model State service call failed:  {0}".format(e))


def distance(x1, y1, x2, y2):
    #print("bX1: " + str(x1))
    #print("bY1: " + str(y1))
    #print("X2: " + str(x2)) 
    #print("Y2: " + str(y2))
    dist = math.sqrt((x2 - x1)**2 + (y2 - y1)**2)
    #print(dist)
    return dist


def boxkdetection(size, botx, boty, l_x, l_y, simtime):
  global f
  global WriteFlag
  cornerdist = (size)/math.sin(math.radians(45))
  safedist = TurtlebotBurger
  #print("Safe " + str(safedist))
  
  if WriteFlag == 0:
     f.write("Safedist.: " + str(safedist) + "\n")
  
  safedist2 = float(size) + TurtlebotBurger + 0.05
  #print("Safe2 " + str(safedist2))

  xUpR = l_x + cornerdist*math.cos(math.radians(45))
  yUpR = l_y + cornerdist*math.sin(math.radians(45))
  dist1 = distance(botx, boty, xUpR, yUpR)
  #print("Dist1: " + str(dist1))
  if WriteFlag == 0:
     f.write("[T:" + str(simtime) + "] " + "Dist. Box Corner 1: " + str(dist1) + "\n")
  if dist1 < safedist:
     rospy.loginfo("Collision!")
     if WriteFlag == 0:
       f.write("[T:" + str(simtime) + "] " + "Collision!")
       f.close
       WriteFlag = 1

  xUpL = l_x + cornerdist*math.cos(math.radians(135))
  yUpL = l_y + cornerdist*math.sin(math.radians(135))
  dist2 = distance(botx, boty, xUpL, yUpL)
  #print("Dist2: " + str(dist2))
  if WriteFlag == 0:
     f.write("[T:" + str(simtime) + "] " + "Dist. Box Corner 2: " + str(dist2) + "\n")
  if dist2 < safedist:
     rospy.loginfo("Collision!")
     if WriteFlag == 0:
       f.write("[T:" + str(simtime) + "] " + "Collision!")
       f.close
       WriteFlag = 1
  
  xLoL = l_x + cornerdist*math.cos(math.radians(225))
  yLoL = l_y + cornerdist*math.sin(math.radians(225))
  dist3 = distance(botx, boty, xLoL, yLoL)
  #print("Dist3: " + str(dist3))
  if WriteFlag == 0:
     f.write("[T:" + str(simtime) + "] " + "Dist. Box Corner 3: " + str(dist3) + "\n")
  if dist3 < safedist:
     rospy.loginfo("Collision!")
     if WriteFlag == 0:
       f.write("[T:" + str(simtime) + "] " + "Collision!")
       f.close
       WriteFlag = 1

  xLoR = l_x + cornerdist*math.cos(math.radians(315))
  yLoR = l_y + cornerdist*math.sin(math.radians(315))
  dist4 = distance(botx, boty, xLoR, yLoR)
  #print("Dist4: " + str(dist4))
  if WriteFlag == 0:
     f.write("[T:" + str(simtime) + "] " + "Dist. Box Corner 4: " + str(dist4) + "\n")
  if dist4 < safedist:
     rospy.loginfo("Collision!")
     if WriteFlag == 0:
       f.write("[T:" + str(simtime) + "] " + "Collision!")
       f.close
       WriteFlag = 1

  centerdist = distance(botx, boty, l_x, l_y)
  #print("Center: " + str(centerdist))
  if WriteFlag == 0:
     f.write("[T:" + str(simtime) + "] " + "Dist. Box Center: " + str(centerdist) + "\n")
  if centerdist < safedist2:
     rospy.loginfo("Collision!")
     if WriteFlag == 0:
       f.write("[T:" + str(simtime) + "] " + "Collision!")
       f.close
       WriteFlag = 1
    

def callback(msg):
    botx = msg.pose.pose.position.x
    boty = msg.pose.pose.position.y  
    print("BotX: " + str(botx))
    print("BotY: " + str(boty))
    tuto = Tutorial()
    tuto.show_gazebo_models(botx, boty)



def main():
    rospy.init_node('location_monitor')
    rospy.Subscriber("/odom", Odometry, callback)
    rospy.spin()

if __name__ == '__main__':
   main()