#!/usr/bin/env python
from __future__ import print_function
from tf.transformations import quaternion_from_euler

import sys
import rospy
import dlib
import numpy as np
import tf2_geometry_msgs
import tf2_ros
import math
import cv2
import actionlib
import matplotlib.pyplot as plt

from skimage.morphology import skeletonize
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal, MoveBaseActionResult
from std_msgs.msg import String
#import matplotlib.pyplot as plt
from sensor_msgs.msg import Image
from geometry_msgs.msg import PointStamped, Vector3, Pose, Twist, TransformStamped, PoseWithCovarianceStamped
from cv_bridge import CvBridge, CvBridgeError
from visualization_msgs.msg import Marker, MarkerArray
from std_msgs.msg import ColorRGBA
from nav_msgs.msg import OccupancyGrid
from skimage.util import invert
from exercise6.msg import objs_info
from exercise4.msg import faces_info

PI = 3.1415926535897 
x_current = 247

y_current = 260
x_previous = 247
y_previous = 260
map_resolution = 0
map_transform = TransformStamped()
heigth = 0
width = 0
tf_buf = None
tf_listener = None
move_map= []
rgb=[]

arm_movement_pub = rospy.Publisher("/arm_command", String, queue_size=1)
twist_pub = rospy.Publisher('/mobile_base/commands/velocity', Twist, queue_size=10)

client = actionlib.SimpleActionClient('move_base',MoveBaseAction)

sides = {"east": 0, "north": PI/float(2), "west": PI, "south": float(1.5)*PI}

dfa = {0:{0:1}, 
       1:{0:0, 1:2},
       2:{0:2, 1:3},
       3:{0:4, 1:10},
       4:{0:4, 1:5},
       5:{0:6},
       6:{0:5, 1:7},
       7:{0:7, 1:8},
       8:{0:9, 1:15},
       9:{0:9, 1:2},
       10:{0:5, 1:11},
       11:{0:12},
       12:{0:12, 1:13},
       13:{0:14},
       14:{0:14,1:7},
       15:{0:5, 1:16}}

#for publishing dialogue and bot beliefs
belief_display_pub = rospy.Publisher("/belief_text", String, queue_size=3)
dialogue_display_pub = rospy.Publisher("/conversation_text", String, queue_size=3)

class Face:
  def __init__(self, haircolor, hairlength, id, pose, img):
    self.haircolor = haircolor
    self.hairlength = hairlength
    self.id = id
    self.pose = pose
    self.img = img

# type is an int: 0 means bot belief, 1 means its dialogue
def display(text, text_type):

    t = String()
    t.data = text

    if text_type == 0:
        belief_display_pub.publish(t)
    else:
        dialogue_display_pub.publish(t)


def interakcijaZObrazom():

    global stanje

    povezava = input("Povezava: ")

    if(povezava == 0):
        print("Pred tem obrazom smo prvic. Naredmo sliko in vprasamo katera je njena najljubsa barva.")
        while(povezava == 0):
            povezava = input("Povezava: ")
            if(povezava == 0):
                print("Barve nismo prepoznali, ponovimo vprasanje.")
            elif(povezava == 1):
                print("Barvo smo prepoznali, vpracamo se k Gargamelu.")
                stanje = 2
                priblizevanje()
            else:
                print("Napaka1")
                break
    elif(povezava == 1):
        print("Pred tem obrazom smo drugic, pokazemo sliko Gargamela in vprasamo, ce bi se porocila.")
        povezava = input("Povezava: ")
        if(povezava == 0):
            print("Obraz zavrnil poroko, poiscemo drugi obraz")
            stanje = 5
            raziskovanjeProstora()
        elif(povezava == 1):
            print("Obraz se strinjal s poroko.")
            print("Naloga koncana!")
        else:
            print("Napaka2")

def interakcijaZGargamelom():
    
    global stanje
    global informacije

    if(not informacije):
        stanje = 4
        informacije = True
        print("Robot naredi sliko Gargamela.")
        print("Robot vprasa Gargamela, kaksne zenske so mu vsec.")
        povezava = 0
        while(povezava == 0):
            povezava = input("Povezava: ")
            if(povezava == 1):
                print("Robot je prepoznal informacije.")
                stanje = 5
                raziskovanjeProstora()
            else:
                print("Robot ni prepoznal informacija, ponovi vprasanje.")
    else:
        print("Pokazemo sliko Gargamelu in ga vprasamo, ce mu je vsec.")
        povezava = input("Povezava: ")
        if(povezava == 0):
            print("Gargamelu ni vsec zenska, poiscemo novo.")
            stanje = 5
            raziskovanjeProstora()
        elif(povezava == 1):
            stanje = 11
            raziskovanjeProstora()
        else:
            print("Napaka3")


def priblizevanje():

    global stanje
    povezava = 0
    
    while(povezava == 0):
        povezava = input("Povezava: ")
        if(stanje == 2):
            if(povezava == 0):
                print("Robot se se ni priblizal Gargamelu.")
            elif(povezava == 1):
                print("Robot se je priblizal Gargamelu")
                stanje = dfa.get(stanje).get(povezava)
                interakcijaZGargamelom()
        elif(stanje == 7):
            if(povezava == 0):
                print("Robot se se ni priblizal obrazu.")
            elif(povezava == 1):
                print("Robot se je priblizal obrazu")
                stanje = dfa.get(stanje).get(povezava)
                interakcijaZObrazom()
        elif(stanje == 12):
            if(povezava == 0):
                print("Robot se se ni priblizal cilindru.")
            elif(povezava == 1):
                print("Robot se je priblizal cilindru.")
                print("Robot z roko zagrabi prstan in ga vrze v cilinder.")
                stanje = dfa.get(stanje).get(povezava)
                raziskovanjeProstora()
        elif(stanje == 14):
            if(povezava == 0):
                print("Robot se se ni priblizal prstanu.")
            elif(povezava == 1):
                print("Robot se je priblizal prstanu.")
                print("Robot z roko zagrabi prstan. Vracamo se k obrazu.")
                stanje = dfa.get(stanje).get(povezava)
                priblizevanje()
        else:
            print("Napaka4.")
            break



def prepoznavaObraza():

    global stanje
    povezava = -1

    print("Prepoznavanje obraza.")
    if(stanje == 1):
        povezava = input("Povezava: ")
        if(povezava == 0):
            print("Zaznani obraz ni Gargamel, nadaljujemo z iskanjem prostora.")
            stanje = -1
        elif(povezava == 1):
            print("Zaznani obraz je Gargamel, priblizevanje Gargamelu.")
            stanje = dfa.get(stanje).get(povezava)
            priblizevanje()
        else:
            print("Napaka5")
    elif(stanje == 6):
        povezava = input("Povezava: ")
        if(povezava == 0):
            print("Zaznani obraz ne ustreza karektaristikam, nadaljujemo z iskanjem prostora.")
            stanje = -1
        elif(povezava == 1):
            print("Zaznani obraz ustreza karektaristikam, priblizevanje obrazu.")
            stanje = dfa.get(stanje).get(povezava)
            priblizevanje()
        else:
            print("Napaka6")
    else:
        print("Napaka7")

def move_to_pose(client, to_pose):
    global map_transform
    global map_resolution

    print("Sending to: ", to_pose.position.x, " ", to_pose.position.y)

    
    goal = MoveBaseGoal()
    goal.target_pose.header.frame_id = "map"
    goal.target_pose.header.stamp = rospy.Time.now()
    goal.target_pose.pose = to_pose
    client.send_goal(goal)

    wait = client.wait_for_result()

    if not wait:
        rospy.logerr("Action server not available!")
        rospy.signal_shutdown("Action server not available!")
    else:
        result = client.get_result()
        if result:
            return result
        else:
            print("big oof")

def move_bot(client, x,y, xPrev, yPrev):

    global map_transform
    global map_resolution

    pt = PointStamped()
    pt.point.x = x * map_resolution
    pt.point.y = (heigth - y) * map_resolution
    pt.point.z = 0.0

    pose_transformed = tf2_geometry_msgs.do_transform_point(pt, map_transform)


    ptPrev = PointStamped()
    ptPrev.point.x = xPrev * map_resolution
    ptPrev.point.y = (heigth - yPrev) * map_resolution
    ptPrev.point.z = 0.0

    pose_transformedPrev = tf2_geometry_msgs.do_transform_point(ptPrev, map_transform)

    angle = np.arctan2((pose_transformed.point.y-pose_transformedPrev.point.y),(pose_transformed.point.x-pose_transformedPrev.point.x))


    print("Sending to: ", pose_transformed.point.x, " ", pose_transformed.point.y, " angle: ", angle)

   
    q = quaternion_from_euler(0,0,angle)

    
    goal = MoveBaseGoal()
    goal.target_pose.header.frame_id = "map"
    goal.target_pose.header.stamp = rospy.Time.now()
    goal.target_pose.pose.position.x = pose_transformed.point.x
    goal.target_pose.pose.position.y = pose_transformed.point.y
    goal.target_pose.pose.position.z = 0.0
    goal.target_pose.pose.orientation.x = q[0]
    goal.target_pose.pose.orientation.y = q[1]
    goal.target_pose.pose.orientation.z = q[2]
    goal.target_pose.pose.orientation.w = q[3]
    client.send_goal(goal)        

    wait = client.wait_for_result()
    print(wait)

    if not wait:
        rospy.logerr("Action server not available!")
        rospy.signal_shutdown("Action server not available!")
    else:
        result = client.get_result()
        if result:

            print("goal reached!") 
            turnToWall(client,x,y)
               
        else:
            print("big oof")  

def turnToWall(client, x,y):

    global move_map
    global map_transform
    global map_resolution

    r=17

    NE=0
    NW=0
    SE=0
    SW=0

    # north-east
    for i in xrange(-r,0,1):
        for j in xrange(0,r,1):       
            
            y2 = i + y
            x2 = j + x

            if move_map[y2][x2] == 100:
                NE= NE+1
                

    # north-west
    for i in xrange(-r,0,1):
        for j in xrange(-r,0,1):
            
            y2 = i + y
            x2 = j + x

            if move_map[y2][x2] == 100:
                NW = NW + 1
               

    # south-west        
    for i in xrange(0,r,1):
        for j in xrange(-r,0,1):
            
            y2 = i + y
            x2 = j + x

            if move_map[y2][x2] == 100:
                SW = SW + 1
                rgb[y2][x2][1] = 100

    # south-east
    for i in xrange(0,r,1):
        for j in xrange(0,r,1):
            
            y2 = i + y
            x2 = j + x

            if move_map[y2][x2] == 100:
                SE = SE + 1
                

    north= NE + NW
    south = SE + SW
    east = SE + NE
    west = SW + NW

    angle = 0 
    angle2 = 0

    winner = max(north, south, east, west)
    winnerStr = ""
    second = 0
    secondStr = ""

    # side with most walls
    if(north == winner):
        winnerStr = "north"
        second = max(south, east, west)
        angle= angle = sides.get(winnerStr)
    elif(south == winner):
        winnerStr = "south"
        second = max(north, east, west)
        angle = angle = sides.get(winnerStr)
    elif( east == winner):
        second = max(south, north, west)
        winnerStr = "east"
        angle = sides.get(winnerStr)
    else:
        second = max(south, north, east)
        winnerStr = "west"
        angle = sides.get(winnerStr)
    
    #second drafting
    if(north == second):
       secondStr = "north"
       angle2= sides.get(secondStr)
    elif(south == second):
       secondStr = "south"
       angle2 = sides.get(secondStr)
    elif( east == second):
       secondStr = "east"
       angle2= sides.get(secondStr)
    else:
      secondStr = "west"
      angle2 = sides.get(secondStr)
      

    pt = PointStamped()
    pt.point.x = x * map_resolution
    pt.point.y = (heigth - y) * map_resolution
    pt.point.z = 0.0

    pose_transformed = tf2_geometry_msgs.do_transform_point(pt, map_transform)

    q = quaternion_from_euler(0,0,angle)

    goal = MoveBaseGoal()
    goal.target_pose.header.frame_id = "map"
    goal.target_pose.header.stamp = rospy.Time.now()
    goal.target_pose.pose.position.x = pose_transformed.point.x
    goal.target_pose.pose.position.y = pose_transformed.point.y
    goal.target_pose.pose.position.z = 0.0
    goal.target_pose.pose.orientation.x = q[0]
    goal.target_pose.pose.orientation.y = q[1]
    goal.target_pose.pose.orientation.z = q[2]
    goal.target_pose.pose.orientation.w = q[3]

    client.send_goal(goal)
    wait = client.wait_for_result()

    if not wait:
        rospy.logerr("Action server not available!")
        rospy.signal_shutdown("Action server not available!")
    else:
        result = client.get_result()
        if result:
            print("turned to wall! ", winnerStr) 
            q = quaternion_from_euler(0,0,angle2)
            goal.target_pose.pose.orientation.x = q[0]
            goal.target_pose.pose.orientation.y = q[1]
            goal.target_pose.pose.orientation.z = q[2]
            goal.target_pose.pose.orientation.w = q[3]

            client.send_goal(goal)
            wait = client.wait_for_result()
            
            if not wait:
                rospy.logerr("Action server not available!")
                rospy.signal_shutdown("Action server not available!")
            else:
                result = client.get_result()
                if result:
                    print("turned to second wall! ", secondStr) 
                
                else:
                    print("big oof") 
        
        else:
            print("big oof")  
    
    

    

def next_goal(client, map, r, width, heigth):

    max_d=0
    goalx=0
    goaly=0
    global x_current 
    global y_current
    r2=15
    r3=r2

    for i in xrange(-r,r,1):
       for j in xrange(-r,r,1):
          
           y = i + y_current
           x = j + x_current

           if map[y][x] == 1:

                if i > -r2 and i < r2 and j > -r2 and j < r2:
                    map[y][x] = 0
                if euclidian_distance(x,y ,x_current, y_current) > max_d:

                    max_d = euclidian_distance(x,y ,x_current, y_current)
                    goalx= x
                    goaly= y

    if goalx == 0 and goaly == 0:

        goals_count=0
        min_d = 1000

        for i in xrange(heigth):
            for j in xrange(width): 

               if map[i][j] == 1:
                    goals_count= goals_count+1

                    if euclidian_distance(j,i ,x_current, y_current) < min_d:
                        
                        min_d = euclidian_distance(j,i ,x_current, y_current)
                        goalx= j
                        goaly= i
        if goals_count < 5:
          print("space explored! ", goals_count)
          goalx=-1
        else:
            print("break, number of goals: ",goals_count)
            map[goaly][goalx] = 180  

    else:
        map[goaly][goalx] = 180
              
   
            
    
    print(goalx, " ", goaly)
    x_previous  = x_current
    y_previous = y_current

    x_current=goalx
    y_current=goaly
     

    if goalx != -1:
    
     move_bot(client, x_current,y_current,x_previous, y_previous)

    return  map
           

def euclidian_distance(x,y ,x2, y2):
    return math.sqrt(math.pow(abs(x-x2),2) + math.pow(abs(y-y2),2))

def calculate_cylinder_approach_pos(x,y, xBot, yBot):

    dx = xBot - x
    dy = yBot - y
    xN = dx / math.sqrt(float(dx*dx + dy *dy))  # normalize
    yN = dy / math.sqrt(float(dx*dx + dy *dy))

    dist = 0.5

    angle = np.arctan2((y-yBot),(x-xBot))
   
    q = quaternion_from_euler(0,0,angle)

    pose = Pose()
    pose.position.x = x + dist*xN
    pose.position.y = y + dist*yN
    pose.position.z = 0
    pose.orientation.x = q[0]
    pose.orientation.y = q[1]
    pose.orientation.z = q[2]
    pose.orientation.w = q[3]

    return pose

def approachCylinder(color):

    global twist_pub
    global arm_movement_pub
    global cylinders

    if not color in cylinders.keys():
        display("No "+ color + " cylinder detected, skipping...", 0)
        return

    print("Approaching the cylinder")
    display("Approaching cylinder.",0)

    goal = MoveBaseGoal()
    goal.target_pose.header.frame_id = "map"
    goal.target_pose.header.stamp = rospy.Time.now()
    goal.target_pose.pose = cylinders.get(color)

    client.send_goal(goal)

    wait = client.wait_for_result()

    if not wait:
        rospy.logerr("Action server not available!")
        rospy.signal_shutdown("Action server not available!")
    else:
        
        distance = 0.3
        speed = 0.1
        rate = rospy.Rate(10)

        vel_msg = Twist()
        vel_msg.linear.x = speed
        vel_msg.linear.y = 0
        vel_msg.linear.z = 0
        vel_msg.angular.x = 0
        vel_msg.angular.y = 0
        vel_msg.angular.z = 0

        t0 = rospy.Time.now().to_sec()
        current_distance = 0
        #Loop to move the turtle in an specified distance
        while(current_distance < distance):
            #Publish the velocity
            twist_pub.publish(vel_msg)
            #Takes actual time to velocity calculus
            t1=rospy.Time.now().to_sec()            
            #Calculates distancePoseStamped
            current_distance= speed*(t1-t0)
            rate.sleep()

        #After the loop, stops the robot
        vel_msg.linear.x = 0
        #Force the robot to stop
        twist_pub.publish(vel_msg)

        print("Approached cylinder.", 0)
       
        
        display("Extending manipulator",0)
        msg = String()
        msg.data = "extend"

        arm_movement_pub.publish(msg)
        rospy.sleep(2)
        display("Retracting manipulator",0)
        msg.data = "retract"
        arm_movement_pub.publish(msg)
        rospy.sleep(2)
        
        print("ROTATING")
        rotate(180)

        print("MOVING BACK")

        t0 = rospy.Time.now().to_sec()
        current_distance = 0
        vel_msg.linear.x = speed

        #Loop to move the turtle in an specified distance
        while(current_distance < distance):
            #Publish the velocity
            twist_pub.publish(vel_msg)
            #Takes actual time to velocity calculus
            t1=rospy.Time.now().to_sec()            
            #Calculates distancePoseStamped
            current_distance= speed*(t1-t0)
            rate.sleep()

        #After the loop, stops the robot
        vel_msg.linear.x = 0
        #Force the robot to stop
        twist_pub.publish(vel_msg) 
     
def rotate(angle):

    global twist_pub

    vel_msg = Twist()
    angular_speed = 60*2*PI/360
    relative_angle = angle * PI / 180
    vel_msg.linear.x = 0
    vel_msg.linear.y = 0
    vel_msg.linear.z = 0
    vel_msg.angular.x = 0
    vel_msg.angular.y = 0
    vel_msg.angular.z = abs(angular_speed)
    t0 = rospy.Time.now().to_sec()
    current_angle = 0
    while(current_angle < relative_angle):
        twist_pub.publish(vel_msg)
        t1 = rospy.Time.now().to_sec()
        current_angle = angular_speed*(t1-t0)
    vel_msg.angular.z = 0
    twist_pub.publish(vel_msg)
       
def calculate_rings_approach_pos( x,y, skeleton):

    global map_transform
    global map_resolution
    global heigth 
    global width

    min_d=1000
    xgoal=0
    ygoal=0

    #find nearest possible goal on the skeleton
    for i in xrange(heigth):
        for j in xrange(width):
            if skeleton[i][j] == 1:

                pt = PointStamped()
                pt.point.x = j * map_resolution
                pt.point.y = (heigth - i) * map_resolution
                pt.point.z = 0.0

                pose_transformed = tf2_geometry_msgs.do_transform_point(pt, map_transform)


                x2=pose_transformed.point.x
                y2=pose_transformed.point.y

                if  euclidian_distance(x,y,x2,y2) < min_d:
                    min_d = euclidian_distance(x,y,x2,y2)
                    xgoal = x2
                    ygoal = y2              

    angle = np.arctan2((y-ygoal),(x-xgoal))   
    q = quaternion_from_euler(0,0,angle)


    pose = Pose()
    pose.position.x = xgoal
    pose.position.y = ygoal
    pose.position.z = 0.0
    pose.orientation.x = q[0]
    pose.orientation.y = q[1]
    pose.orientation.z = q[2]
    pose.orientation.w = q[3]

    distance = euclidian_distance(xgoal,ygoal, x, y)

    print("DISTANCE:", distance)

    return [distance, pose]

def calculate_face_approach_pos(x,y, robotx, roboty, skeleton):
    global heigth
    global width
    global map_resolution
    global map_transform
     
    min_d=1000
    xgoal=0
    ygoal=0

    #find nearest possible goal on the skeleton
    for i in xrange(heigth):
        for j in xrange(width):
            if skeleton[i][j] == 1:

                pt = PointStamped()
                pt.point.x = j * map_resolution
                pt.point.y = (heigth - i) * map_resolution
                pt.point.z = 0.0

                pose_transformed = tf2_geometry_msgs.do_transform_point(pt, map_transform)


                x2=pose_transformed.point.x
                y2=pose_transformed.point.y

                if  euclidian_distance(x,y,x2,y2) < min_d and  euclidian_distance(x2,y2, robotx, roboty) < 0.5:
                    min_d = euclidian_distance(x,y,x2,y2)
                    xgoal = x2
                    ygoal = y2

    angle = np.arctan2((y-ygoal),(x-xgoal))

    q = quaternion_from_euler(0,0,angle)

    pose= Pose()
    pose.position.x = xgoal
    pose.position.y = ygoal
    pose.position.z = 0.0
    pose.orientation.x = q[0]
    pose.orientation.y = q[1]
    pose.orientation.z = q[2]
    pose.orientation.w = q[3]

    return pose

def speechRecog(question):
    
    toggle = "on"
    togglePub.publish(toggle)

    confirmed = False
    output = ""
    person = "Gargamel"

    while(confirmed == False):

        try:
            recognized = rospy.wait_for_message("/recognizer/output", String)
        except Exception as e:
            print(e)

        if question == "gargamelWomen":

            if len(recognized.data.split(' ')) == 4:
                print(recognized.data.split(' '))
                darkshort =  recognized.data.split(' ')[2]
                col = darkshort.split("dark")
                print(col)
                col = col[1]
                recognized.data= "women with dark "+ col + " hair"

            gargamel = "Gargamel: " + recognized.data
            str_ = "Robot : I heard: " + recognized.data + " Is that correct?"
            display(gargamel, 1)
            display(str_, 1)
            output = recognized.data
            if recognized.data.split(" ")[0] != "women":
                display("Robot: I didnt expect that answer, Please repeat that..",1)
                continue
        elif question == 'favColor':
            print("Recognized: ", recognized.data)
            if recognized.data.split(" ")[0] != "my":
                display("Robot: I didnt expect that answer, Please repeat that..",1)
                continue
            woman = "Woman: " + recognized.data
            str_ = "Robot: I heard: " + recognized.data + " Is that correct?"
            fav_color = recognized.data.split(' ')[4]
            display(woman, 1)
            display(str_, 1)
            output = fav_color
            person = "Woman"
        elif question == 'gargWomanAck':
            gargamel = "Gargamel: " + recognized.data
            str_ = "Robot : I heard: " + recognized.data + " Is that correct?"
            display(gargamel, 1)
            display(str_, 1)
            output = recognized.data
        elif question == 'proposal':
            spouse = "Potential spouse: " + recognized.data
            str_ = "Robot : I heard: " + recognized.data + " Is that correct?"
            display(spouse, 1)
            display(str_, 1)
            person = "Spouse"
            output = recognized.data
        
        try:
            recognized = rospy.wait_for_message("/recognizer/output", String)
        except Exception as e:
            print(e)

        if recognized.data == "yes":
            display(person+ ": yes", 1)
            confirmed = True
        else:
            display(person+ ": no", 1)
            display("Robot: Please repeat that..",1)
     
    return output
    


def najdiGargamela():

    global gargamel
    global client
    global bridge
    global gargamel_img
    
    display("Going to Gargamel",0)

    #pojdi do gargamela
    goal = MoveBaseGoal()
    goal.target_pose.header.frame_id = "map"
    goal.target_pose.header.stamp = rospy.Time.now()
    goal.target_pose.pose = gargamel
    client.send_goal(goal)

    wait = client.wait_for_result()

    if not wait:
        rospy.logerr("Action server not available!")
        rospy.signal_shutdown("Action server not available!")
    else:
        result = client.get_result()
        if result:
            print("Gargamel reached!")
            display("Gargamel reached!", 0)

            try:
               gargamel_img_msg = rospy.wait_for_message("/camera/rgb/image_raw", Image)
            except Exception as e:
               print(e)
               return 0

            try:
                gargamel_img = bridge.imgmsg_to_cv2(gargamel_img_msg , "bgr8")
            except CvBridgeError as e:
                print(e)

            display("ROBOT: What kind of women do you preffer?", 1)
            womanType = speechRecog("gargamelWomen")
            findGargamelsWoman(womanType)
            


        else:
            print("big oof")  

def approach_ring(color):
    global rings
    global client
    global ringsDistances
    global twist_pub

    display("Moving to ring", 0)

    if not color in rings.keys():
        display("No "+ color + " ring detected, skipping...", 0)
        return

    ring_approach_pose = rings[color]
    ring_distance= ringsDistances[color]

    goal = MoveBaseGoal()
    goal.target_pose.header.frame_id = "map"
    goal.target_pose.header.stamp = rospy.Time.now()
    goal.target_pose.pose = ring_approach_pose


    client.send_goal(goal)
    wait = client.wait_for_result()

    if not wait:
        rospy.logerr("Action server not available!")
        rospy.signal_shutdown("Action server not available!")
    else:
        result = client.get_result()
        if result:
            #FINE APPROACH
            print("ring reached!") 
            display("closing in on ring", 0)
            distance = ring_distance-0.1
            speed=0.1

            rate = rospy.Rate(10)

            vel_msg = Twist()
            vel_msg.linear.x = speed
            vel_msg.linear.y = 0
            vel_msg.linear.z = 0
            vel_msg.angular.x = 0
            vel_msg.angular.y = 0
            vel_msg.angular.z = 0

            t0 = rospy.Time.now().to_sec()
            current_distance = 0
            #Loop to move the turtle in an specified distance
            while(current_distance < distance):
                #Publish the velocity
                twist_pub.publish(vel_msg)
                #Takes actual time to velocity calculus
                t1=rospy.Time.now().to_sec()            
                #Calculates distancePoseStamped
                current_distance= speed*(t1-t0)
                rate.sleep()

            #After the loop, stops the robot
            vel_msg.linear.x = 0
            #Force the robot to stop
            twist_pub.publish(vel_msg)

            display("Extending manipulator", 0)
            msg = String()
            msg.data = "extend_circle"

            arm_movement_pub.publish(msg)
            rospy.sleep(2)

            display("Retracting manipulator", 0)
            msg.data = "retract"

            arm_movement_pub.publish(msg)
            rospy.sleep(2)
           
            print("ROTATING")
            rotate(180)

            print("MOVING BACK")

            t0 = rospy.Time.now().to_sec()
            current_distance = 0
            vel_msg.linear.x = speed

            #Loop to move the turtle in an specified distance
            while(current_distance < distance):
                #Publish the velocity
                twist_pub.publish(vel_msg)
                #Takes actual time to velocity calculus
                t1=rospy.Time.now().to_sec()            
                #Calculates distancePoseStamped
                current_distance= speed*(t1-t0)
                rate.sleep()

            #After the loop, stops the robot
            vel_msg.linear.x = 0
            #Force the robot to stop
            twist_pub.publish(vel_msg)
            
          
        else:
            print("big oof")  

def findGargamelsWoman(womanType):
    global faces 
    global currentCandidates

    print("TYPE :", womanType)
    womanType = womanType.split(" ")
    hcolor = womanType[2]
    hlength = womanType[3]
    currentCandidates = []

    #ugly but whatever
    if hcolor == "dark":
        hcolor= "1"
    else:
        hcolor="0"
    if hlength== "long":
        hlength="1"
    else:
        hlength="0"

    print("hcolor, hlength", hcolor, hlength)

    for i in xrange(len(faces)):
        #print("TEST:",faces[i].id, faces[i].haircolor, faces[i].hairlength,  str(faces[i].haircolor) == str(hcolor), faces[i].haircolor == "1"  )
        if(faces[i].haircolor == hcolor and faces[i].hairlength == hlength):
            currentCandidates.append(faces[i])
            print("Test: ", len(currentCandidates) )
            print("id: ", faces[i].id )

    if len(currentCandidates) == 0:
        display("No such woman detected D:", 1)
    else:
        display("Approaching a match",0)
        approachMatch()
        
def approachMatch():
    global currentCandidates
    global married
    global bridge
    global client
    global gargamel
    global currentCandidateImg
    # TO DO: pojdi do zenske jo vprasaj za barvo, nazaj do gargamela ga vprasaj ce mu je vsec
    # JA: nadaljuj s cilindri in obroci
    # NE: odstrani zensko iz currentMatches in nadaljuj z naslednjo, ce naslednje ni nbe naredi nic
    # bo sel robot nazaj do gargamela in ga spet sprasval
    candidate = currentCandidates[0]

    move_to_pose(client,candidate.pose)

    display("ROBOT: What is your favourite color?", 1)
    favColor = speechRecog("favColor")

    try:
        currentCandidateImgMsg = rospy.wait_for_message("/camera/rgb/image_raw", Image)
    except Exception as e:
        print(e)
    
    try:
        currentCandidateImg = bridge.imgmsg_to_cv2(currentCandidateImgMsg , "bgr8")
    except CvBridgeError as e:
        print(e)

    display("Taking a picture",0)
    
    move_to_pose(client, gargamel)

    display("Robot: Do you like this woman?", 1)
    
    print("BEFORE IMAGE")
    cv2.imshow("Potential spouse",currentCandidateImg)
    cv2.waitKey(0)
    cv2.destroyWindow("Potential spouse")
    print("AFTER IMAGE")
    
    gargAnswer = speechRecog("gargWomanAck")

    if gargAnswer == 'yes':
        # call other stuff
        display("Found a match", 0)
        approachCylinder(favColor)
        approach_ring(favColor)
        propose()

    else:
        currentCandidates.pop(0)

        if len(currentCandidates)  > 0 :
            display("Finding other suitable matches..", 0)
            approachMatch()
        
def propose():

    global currentCandidates
    global married
    global bridge
    global client
    global gargamel 
    global gargamel_img

    display("Moving to propose", 0)
    move_to_pose(client, currentCandidates[0].pose)
    display("Proposing", 0)
    display("Robot: Will you marry Gargamel?", 1)

    print("BEFORE IMAGE")
    cv2.imshow("Potential wife", gargamel_img)
    cv2.waitKey(0)
    cv2.destroyWindow("Potential wife")
    print("AFTER IMAGE")

    answer = speechRecog("proposal")

    if answer == "yes":

      married = True
      display("Robot: hooray", 1)
      return 

    else:

        currentCandidates.pop(0)

        if len(currentCandidates)  > 0 :
            display("Finding other suitable matches..", 0)
            approachMatch()
        
        else:
           display("Returning to gargamel...", 0) 
    


def raziskovanjeProstora():
    
    print("Raziskovanje prostora.")
    display("Searching the space", 0)

    global width
    global heigth
    global map_transform
    global map_resolution
    global move_map
    global rgb
    global rinfo
    global rings
    global cylinders
    global faces
    global gargamel
    global gargamelPic
    global ringsDistances
    global lastFace
    global client
    global bridge
    global togglePub
    global married 

    togglePub = rospy.Publisher('toggle', String, queue_size=10)

    gargamel = {}
    faces =  []
    cylinders = {'init': 0}
    rings = {'init': 0}
    ringsDistances = {'init': 0}

    married= False
    lastRing = 0
    lastFace = 0
    lastCylinder = 0
    
    bridge = CvBridge()

    try:
        map = rospy.wait_for_message("map", OccupancyGrid)
    except Exception as e:
        print(e)

    width=map.info.width
    heigth=map.info.height

    
    print(map.info)    
    cv_map = np.zeros((heigth,width))
    mask = np.zeros((heigth,width))
    walls = np.zeros((heigth,width))
    move_map = np.zeros((heigth,width))
    rgb = np.zeros((heigth,width,3))

    print(cv_map.shape)
    
    map_resolution = map.info.resolution

    map_transform = TransformStamped()
    map_transform.transform.translation.x = map.info.origin.position.x
    map_transform.transform.translation.y = map.info.origin.position.y
    map_transform.transform.translation.z = map.info.origin.position.z
    map_transform.transform.rotation = map.info.origin.orientation
  
  
    x=0
    for i in xrange(heigth-1,-1,-1):
        for j in xrange(width):
            
            
            if map.data[x] == -1 : 
             cv_map[i][j] = 0
             mask[i][j] = 1
           
            if map.data[x] == 0 :
             cv_map[i][j] = 0
            

            if map.data[x] == 100 : 
             cv_map[i][j] = 1
             walls[i][j] = 1
            
            x=x+1



    kernel = np.ones((10,10),np.uint8)
    dilatedOriginal = cv2.dilate(cv_map, kernel ,iterations = 1)

    dilated = invert(dilatedOriginal)
 
    skeleton = skeletonize(dilated)

    for i in xrange(heigth):
        for j in xrange(width):
            
          if mask[i][j]  == 1:
            skeleton[i][j] = 0
    
    for i in range(heigth):
        for j in range(width):
            if skeleton[i][j] == 1:
                rgb[i][j][1] = 150
                move_map[i][j] = 1

            if mask[i][j] == 1:
               rgb[i][j][2] = 150
               move_map[i][j] = 170

            if walls[i][j] == 1:
               rgb[i][j][0] = 150
               move_map[i][j] = 100

    client.wait_for_server()
  
    move_bot(client, x_current,y_current, x_previous, y_previous)
    
    rings_info = objs_info()
    rings_info.poses, rings_info.colors = [], []

    cylinders_info = objs_info()
    cylinders_info.poses, cylinders_info.colors = [], []  
   
    '''
    #cylinders approach position
    try:
        cinfo = rospy.wait_for_message("cylinders_info", objs_info)
    except Exception as e:
        print(e)

    for i in range(lastCylinder, len(cinfo.poses)):
        lastCylinder += 1
        cylinders[cinfo.colors[i]] = calculate_cylinder_approach_pos(cinfo.poses[i].position.x,cinfo.poses[i].position.y, cinfo.robot_point_stamped[i].point.x, cinfo.robot_point_stamped[i].point.y) 
    
    
    
    try:
        facesInfo = rospy.wait_for_message("faces_info", faces_info)
    except Exception as e:
        print(e)
    #print("TEST ", facesInfo)
    for i in range(lastFace, len(facesInfo.poses)):
        lastFace += 1
        facePos = calculate_face_approach_pos(facesInfo.poses[i].position.x,facesInfo.poses[i].position.y, facesInfo.robotPoses[i].position.x, facesInfo.robotPoses[i].position.y, skeleton)
        
        if facesInfo.ids[i] == 0: 
            gargamel = facePos
            #gargamelPic = bridge.imgmsg_to_cv2(faces_info.images[i], "bgr8")
        else : 
            face = Face(facesInfo.hairColor[i], facesInfo.hairLength[i], facesInfo.ids[i], facePos,facesInfo.images[i])
    
            faces.append(face)
    
    try:   
        rinfo = rospy.wait_for_message("rings_info", objs_info)
    except Exception as e:
        print(e)

    for i in range(lastRing, len(rinfo.poses)):
        lastRing += 1 
        arr = calculate_rings_approach_pos(rinfo.poses[i].position.x,rinfo.poses[i].position.y, skeleton)
        rings[rinfo.colors[i]] = arr[1]
        ringsDistances[rinfo.colors[i]] = arr[0]
    '''
    
    while x_current != -1:
        move_map= next_goal(client, move_map,20,width,heigth)

        #cylinders approach position
        try:
            cinfo = rospy.wait_for_message("cylinders_info", objs_info)
        except Exception as e:
            print(e)

        for i in range(lastCylinder, len(cinfo.poses)):
            lastCylinder += 1
            cylinders[cinfo.colors[i]] = calculate_cylinder_approach_pos(cinfo.poses[i].position.x,cinfo.poses[i].position.y, cinfo.robot_point_stamped[i].point.x, cinfo.robot_point_stamped[i].point.y) 
        
        
        
        try:
            facesInfo = rospy.wait_for_message("faces_info", faces_info)
        except Exception as e:
            print(e)
        #print("TEST ", facesInfo)
        for i in range(lastFace, len(facesInfo.poses)):
            lastFace += 1
            facePos = calculate_face_approach_pos(facesInfo.poses[i].position.x,facesInfo.poses[i].position.y, facesInfo.robotPoses[i].position.x, facesInfo.robotPoses[i].position.y, skeleton)
            
            if facesInfo.ids[i] == 0: 
                gargamel = facePos
                #gargamelPic = bridge.imgmsg_to_cv2(faces_info.images[i], "bgr8")
            else : 
                face = Face(facesInfo.hairColor[i], facesInfo.hairLength[i], facesInfo.ids[i], facePos,facesInfo.images[i])
        
                faces.append(face)
        
        try:   
            rinfo = rospy.wait_for_message("rings_info", objs_info)
        except Exception as e:
            print(e)

        for i in range(lastRing, len(rinfo.poses)):
            lastRing += 1 
            arr = calculate_rings_approach_pos(rinfo.poses[i].position.x,rinfo.poses[i].position.y, skeleton)
            rings[rinfo.colors[i]] = arr[1]
            ringsDistances[rinfo.colors[i]] = arr[0]
               

    '''
    plt.close()
    fig, axes = plt.subplots(1, 5, figsize=(8, 4), sharex=True, sharey=True)
    ax = axes.ravel()

    ax[0].imshow(dilatedOriginal, cmap=plt.cm.gray)
    ax[0].set_title('dilated')
    ax[0].axis('off')

    ax[1].imshow(skeleton, cmap=plt.cm.gray)
    ax[1].set_title('skeletonized')
    ax[1].axis('off')

    ax[2].imshow(cv_map, cmap=plt.cm.gray)
    ax[2].set_title('original')
    ax[2].axis('off')

    ax[3].imshow(rgb, cmap=plt.cm.gray)
    ax[3].set_title('rgb representation')
    ax[3].axis('off')

    ax[4].imshow(move_map, cmap=plt.cm.gray)
    ax[4].set_title('moved')
    ax[4].axis('off')

    fig.tight_layout()
    plt.show()
    '''
    display("Space searched!", 0)


def main():
    rospy.init_node("DFA")
    global stanje
    stanje = 0

    global informacije
    informacije = False 

    raziskovanjeProstora()

    while(not married):
        najdiGargamela()
    
    rospy.spin()

if __name__ == '__main__':
    main()
