import pygame
import numpy as np
import math
import gym
import torch
import random
import featureExtractor
import os
from nn import Neural_net as NN
random.seed(4)
_screen_height = 100
_screen_width = 100
_stripobsx = 0
_stripobsy = 20
_stripgoalx = 70
_stripgoaly = 20
_stripagentx = 80
_stripagenty = 10
_max_agent_speed = 5
weights = [1,1,1,1]


class Obstacle:
    goals = None

    def __init__(self,id,xpos = None , ypos = None , xvel = None , yvel = None, radius = None):
        self.id = id
        if xpos==None:
            self.x = np.random.randint(_stripobsx,_screen_width-_stripobsx)
        else:
            self.x = xpos

        if ypos==None:
            self.y = np.random.randint(_stripobsy,_screen_height-_stripobsy)
        else:
            self.y = ypos
        if radius==None:
            self.rad = 20
        else:
            self.rad = radius
        self.curr_goal = None #this is a tuple (x,y)
        if xvel == None:
            self.vel_x = 0
        else:
            self.vel_x = xvel
        if yvel == None:
            self.vel_y = 0
        else:
            self.vel_y = yvel

        self.goal_change_counter = None
        self.curr_counter = 0




class createBoardIRL():
    #saved model contains the state dictionary and not the actual model
    def __init__(self, saved_model = None, sensor_size = None, display = False ,number_of_actions = 4 , hidden_layers = [256 , 256] ,number_of_height = _screen_height , weights = weights ,  width = _screen_width , agent_radius = 10 , static_obstacles = 1 , dynamic_obstacles = 0 , static_obstacle_radius = 10 , dynamic_obstacle_radius = 0 , obstacle_speed_list = []):

        pygame.init()
        self.clock = pygame.time.Clock()
        self.rewardWeights = weights  #numpy array
        self.display = display
        self.gameExit = False
        self.height = _screen_height
        self.width = _screen_width
        self.agent_radius = agent_radius


        self.sensorArraysize = sensor_size


        self.size_action_space = number_of_actions


        self.max_agent_speed = _max_agent_speed
        self.hidden_layers = hidden_layers
        self.no_static_obstacles = static_obstacles
        self.no_dynamic_obstacles = dynamic_obstacles
        self.total_obs = self.no_static_obstacles+self.no_dynamic_obstacles
        self.rad_static_obstacles = static_obstacle_radius
        self.rad_dynamic_obstacles = dynamic_obstacle_radius
        self.agent_action_flag = False
        self.agent_action_keyboard = [False for i in range(4)]

        self.agent_x = None
        self.agent_y = None
        self.agent_x_vel = 0
        self.agent_y_vel = 0


        self.goal_x = None
        self.goal_y = None
        self.old_dist = None
        self.goal_threshold = 20
        self.total_distance = None
        self.obstacle_speed_list = obstacle_speed_list
        self.static_obstacle_list = []
        self.dynamic_obstacle_list = []
        self.obstacle_list = []
        self.sensor_readings = None #numpy array

        if saved_model==None:
            self.agentBrain = NN(self.sensorArraysize , self.hidden_layers,self.size_action_space)
            self.agentBrain.cuda()
        else:
            self.agentBrain = NN(self.sensorArraysize , self.hidden_layers,self.size_action_space)
            self.agentBrain.load_state_dict(torch.load(saved_model))
            self.agentBrain.eval()
            self.agentBrain.cuda()

    #state : list
    #state[0] - tuple containing agent current position
    #state[1] - tuple containing goal position.
    #state[2] - distance from goal
    #state[3] - done?
    #state[4 - end] - tuple obstacle position


        self.state = None
        self.reward = None
        self.total_reward_accumulated = None

        self.white = (255,255,255)
        self.red = (255,0,0)
        self.green = (0,255,0)
        self.blue = (0,0,255)
        self.black = (0,0,0)
        self.caption = 'social navigation world'
        print "initialization done."

    def calculate_distance(self, tup1, tup2):
        x_diff = tup1[0] - tup2[0]
        y_diff = tup1[1] - tup2[1]
        return math.sqrt(math.pow(x_diff, 2) + math.pow(y_diff, 2))


    def check_overlap(self, tup1, tup2):
        dist = self.calculate_distance(tup1, tup2)
        if dist > (self.rad_static_obstacles + self.agent_radius):
            return False
        else:
            return True


    #this method is new for the IRL class
    #this method takes in the state information generated by the environment at each step
    #and converts it into sensory information format

    #sensory information format is as follows:
    #sensory dictionary :
    #   deviation - float ?? --remove as of now
    #   rel_vel - tuple (2)
    #   lidar_info - array [36]
    #   cur_vel - tuple(2)
    #   cur_pos - tuple(2)
    #   distance_from_goal - float
    #   distance_form_closest_obstacle - float
    #   action taken - tuple(2)


    def state_to_sensorReadings(self):

        sensor_readlist = []
        #obstacle_info = self.state[3:]
        closest_obs_dist , rel_vel = self.calculateDistanceFromClosestObstacle()
        sensor_readlist.append(rel_vel[0])
        sensor_readlist.append(rel_vel[1])
        lidar_info = self.calculateLIDARInfo(100)
        for i in range(lidar_info.size):
            sensor_readlist.append(lidar_info[i])

        sensor_readlist.append(self.agent_x_vel)
        sensor_readlist.append(self.agent_y_vel)
        sensor_readlist.append(self.agent_x)
        sensor_readlist.append(self.agent_y)
        sensor_readlist.append(self.state[2])
        sensor_readlist.append(closest_obs_dist)

        return np.asarray(sensor_readlist)




    def calculateDistanceFromClosestObstacle(self):

        min_dist = 10000
        rel_x = None
        rel_y = None
        for i in range(len(self.obstacle_list)):

            obs = self.obstacle_list[i]

            dist = np.linalg.norm([(obs.x - self.agent_x),(obs.y - self.agent_y)])
            if dist < min_dist:

                min_dist = dist
                rel_x = obs.vel_x - self.agent_x_vel
                rel_y = obs.vel_y - self.agent_y_vel


        return min_dist , (rel_x,rel_y)

    #returns an array of size 36
    def calculateLIDARInfo(self, lidar_range):

        lidarHist = np.asarray([lidar_range for i in range(36)])
        for i in range(len(self.obstacle_list)):
            obs = self.obstacle_list[i]
            dist = np.linalg.norm([(obs.x - self.agent_x),(obs.y - self.agent_y)])
            bin = self.getBin(obs)

            if dist < lidarHist[bin]:
                lidarHist[bin] = dist

        return lidarHist


    def getBin(self, obs):

        v1 = np.asarray([0,1])
        v2 = np.asarray([obs.x - self.agent_x , obs.y - self.agent_y])
        angle_diff = self.angle_between(v1,v2)
        if v1[0]>v2[0]:
            angle_diff = 2*math.pi - angle_diff
        angle_diff = (angle_diff*360)/(2*math.pi)
        angle_diff = angle_diff%360
        return int(angle_diff//10)



    def unit_vector(self,vector):
        #print "vector"
        return vector / np.linalg.norm(vector)

    def angle_between(self,v1, v2):
        #Returns the angle in radians between vectors 'v1' and 'v2'::
        v1_u = self.unit_vector(v1)
        v2_u = self.unit_vector(v2)
        return np.arccos(np.clip(np.dot(v1_u, v2_u), -1.0, 1.0))



    def take_action_from_userMouse(self):
        (a, b, c) = pygame.mouse.get_pressed()

        x = 0.0001
        y = 0.0001
        print "heresa"
    #while (True):
        for event in pygame.event.get():
            if event.type == pygame.MOUSEBUTTONDOWN:
                print "heare"
                self.agent_action_flag = True
                if event.type == pygame.MOUSEBUTTONUP:
                    self.agent_action_flag = False
        if self.agent_action_flag:
            (x, y) = pygame.mouse.get_pos()

            x = x - self.agent_x
            y = y - self.agent_y
            if np.hypot(x, y) > _max_agent_speed:
                normalizer = _max_agent_speed / (np.hypot(x, y))
        # print x,y
            else:
                normalizer = 1
            print (x * normalizer, y * normalizer)
            return (x * normalizer, y * normalizer)

        return (0, 0)

    #a simplified version where there are just 4actions
    def take_action_from_userKeyboard(self):

        while (True):
            for event in pygame.event.get():
                if event.type == pygame.KEYDOWN:
                    key = pygame.key.get_pressed()
                    if key[pygame.K_UP]:
                        self.agent_action_keyboard[0]=True
                    if key[pygame.K_RIGHT]:
                        self.agent_action_keyboard[1]=True
                    if key[pygame.K_LEFT]:
                        self.agent_action_keyboard[3]=True
                    if key[pygame.K_DOWN]:
                        self.agent_action_keyboard[2]=True

                if event.type == pygame.KEYUP:

                    if event.key == pygame.K_UP:
                        self.agent_action_keyboard[0]=False
                    if event.key == pygame.K_RIGHT:
                        self.agent_action_keyboard[1]=False
                    if event.key == pygame.K_LEFT:
                        self.agent_action_keyboard[3]=False
                    if event.key == pygame.K_DOWN:
                        self.agent_action_keyboard[2]=False

            for i in range(len(self.agent_action_keyboard)):
                if self.agent_action_keyboard[i]==True:
                    return i

        return None
    #returns the action that has the highest qvalue
    def gen_action_from_agent(self):


        qvalues = self.agentBrain(self.sensor_readings)

        qvalues = qvalues.cpu().detach().numpy()
        return np.argmax(qvalues)

    #this method takes in the result from gen_action_from_agent()
    #and converts into a tuple (x,y) where (x,y) is the amount the
    #agent will move in the x and y direction respectively
    def agent_action_to_WorldAction(self, action):

        action_angle = 2*math.pi * action/self.size_action_space

        action_vector = np.array([0, self.max_agent_speed])
        rot_matrix  = np.array([[math.cos(action_angle), -math.sin(action_angle)],[math.sin(action_angle), math.cos(action_angle)]])

        action_taken = np.matmul(rot_matrix, action_vector)

        return action_taken

    #a simplified version: only 4 ways to move
    #action (int) can attain values from 0-3, and based on that
    #the agent moves left right front or back
    def agent_action_to_WorldActionSimplified(self,action):
        if action==0: #move front
            return np.asarray([0,-5])
        if action==1: #move right
            return np.asarray([5,0])
        if action==2: #move down
            return np.asarray([0,5])
        if action==3: #move left
            return np.asarray([-5,0])




    def check_overlap_rect(self, tup1, tup2, rad):
        if abs(tup1[0] - tup2[0]) < (rad + self.agent_radius) and abs(tup1[1] - tup2[1]) < (
                rad / 2 + self.rad_static_obstacles):
            return True
        else:
            return False


    def generate_randomval(self, lower, upper):
        i = np.random.ranf()
        return lower + i * (upper - lower)


    def reset(self):
        self.agent_action_flag = False
        if self.display:

            self.gameDisplay = pygame.display.set_mode((self.width, self.height))
            pygame.display.set_caption('social navigation world')
            self.gameDisplay.fill(self.white)



        self.obstacle_list = []
        self.goal_x = self.generate_randomval(_screen_width - _stripgoalx, _screen_width)
        self.goal_y = self.generate_randomval(_screen_height - _stripgoaly, _screen_height)
        agent_x = self.generate_randomval(0, _stripagentx)
        agent_y = self.generate_randomval(0, _stripagenty)
        dist = math.sqrt(math.pow(self.goal_x - agent_x, 2) + math.pow(self.goal_y - agent_y, 2))
        self.old_dist = dist
        while (1):
            if self.calculate_distance((self.goal_x, self.goal_y), (agent_x, agent_y)) < 50:
                agent_x = self.generate_randomval(0, _stripagentx)
                agent_y = self.generate_randomval(0, _stripagenty)
            else:
                break
        self.state = [(agent_x, agent_y), (self.goal_x, self.goal_y), dist , 0] #0 - not done /1 - done
        # print('PPPP',self.total_obstacles)
        self.agent_x = agent_x
        self.agent_y = agent_y
        self.total_reward_accumulated = 0
        # intialize the static obstacles
        for i in range(self.no_static_obstacles):
            while (1):
                # print("hit")
                temp_obs = Obstacle(i)
                if (not self.check_overlap_rect((temp_obs.x, temp_obs.y), (agent_x, agent_y),
                                                self.rad_static_obstacles) and not self.check_overlap_rect(
                    (temp_obs.x, temp_obs.y), (self.goal_x, self.goal_y), self.rad_static_obstacles)):
                    self.static_obstacle_list.append(temp_obs)
                    self.state.append((temp_obs.x, temp_obs.y))
                    self.obstacle_list.append(temp_obs)
                    break
        # print(len(self.obstacle_list))

        # initialize the dynamic obstacles
        for i in range(self.no_dynamic_obstacles):
            temp_obs = Obstacle(self.no_static_obstacles+i)
            temp_obs.speed = self.obstacle_speed_list[i]
            temp_obs.curr_goal = self.obstacle_goal_list[i]
            temp_obs.goal_change_counter = self.goal_change_step
            self.dynamic_obstacle_list.append(temp_obs)
            self.state.append((temp_obs.x, temp_obs.y))
            self.obstacle_list.append(temp_obs)

        self.total_distance = self.calculate_distance(self.state[0], self.state[1])

        #self.sensor_readings = self.state_to_sensorReadings()
        self.sensor_readings = featureExtractor.featureExtractor(self.state,self.obstacle_list,(self.agent_x_vel,self.agent_y_vel),self.agent_radius)
        #self.agentBrain = NN(self.state_to_sensorReadings().size , self.hidden_layers,self.size_action_space)

        return np.array(self.state)


    def renderObstacle(self, obs):
        pygame.draw.circle(self.gameDisplay, self.red, (obs.x, obs.y), obs.rad)




    def render(self):  # renders the screen using the current state of the environment

        self.gameDisplay.fill(self.white)
        for obs in self.obstacle_list:
            self.renderObstacle(obs)
        # draw goal
        pygame.draw.rect(self.gameDisplay, self.black, [self.goal_x, self.goal_y, 10, 10])
        # draw agent
        pygame.draw.circle(self.gameDisplay, self.black, (int(self.state[0][0]), int(self.state[0][1])), self.agent_radius)
        pygame.display.update()
        self.clock.tick(30)


    # updates the position of the objects in the environment according to the dynamics
    # action is a tuple (x,y), which gets added to the current
    # position of the agent

    def step(self, action):
        self.old_dist = self.calculate_distance(self.state[0], self.state[1])
        old_x = self.state[0][0]
        old_y = self.state[0][1]
        newx = self.state[0][0] + action[0]
        newy = self.state[0][1] + action[1]

        if newx < 0:
            newx = 0
        if newx > _screen_width:
            newx = _screen_width
        if newy < 0:
            newy = 0
        if newy > _screen_height:
            newy = _screen_height
        self.state[0] = (newx, newy) #update the position of the agent
        self.state[2] = self.calculate_distance(self.state[0] , self.state[1])#update the distance of the agent

        self.agent_x = self.state[0][0]
        self.agent_y = self.state[0][1]
        reward, done = self.calc_reward()
        if done:
            self.state[3] = 1 #episode done. Hit an obstacle or reached goal
        # print self.state[0]
        self.agent_x_vel = self.agent_x - old_x
        self.agent_y_vel = self.agent_y - old_y
        #print self.state

        #dynamic obstacles start from the index 3+no_of_static_obstacles
        #change the location of the dynamic obstacles for each step

        for i in range(len(self.dynamic_obstacle_list)):
            obs = self.dynamic_obstacle_list[i]
            x = obs.curr_goal[0] - obs.x
            y = obs.curr_goal[1] - obs.y
            magnitude = np.linalg.norm(x,y)
            obs.x = obs.x + (x/magnitude)*obs.speed
            obs.vel_x = (x/magnitude)*obs.speed
            obs.vel_y = (y/magnitude)*obs.speed
            obs.y = obs.y + obs.vel_y

            self.state[4+len(self.no_static_obstacles)+i] = (obs.x , obs.y)


        self.sensor_readings = featureExtractor.featureExtractor(self.state,self.obstacle_list,(self.agent_x_vel,self.agent_y_vel),self.agent_radius)
        if self.display:
            self.render()
        #print self.sensor_readings


        return np.array(self.state), reward, done, {}

    def calc_reward(self):
        done = False
        for obs in self.obstacle_list:

            done = self.check_overlap(self.state[0], (obs.x, obs.y))
            if done:
                return -1, done

        if self.calculate_distance(self.state[0], self.state[1]) < self.goal_threshold:

            done = True
            reward = np.dot(self.rewardWeights , self.sensor_readings)
            self.total_reward_accumulated += reward

            return reward, done

        else:
            cur_dist = self.calculate_distance(self.state[0], self.state[1])

            diff_dist = self.old_dist - cur_dist
            #rint self.rewardWeights.size
            #print self.sensor_readings.size
            #print 'reward_weights',self.rewardWeights.shape
            #print 'sensor_readings',self.sensor_readings.shape
            reward = np.dot(self.rewardWeights , self.sensor_readings)
            self.total_reward_accumulated += reward
            return reward, done

        return -1 , done
    # -1 if it hits an obstacle or fraction of the distance it travels towards
    # the goal with a total of 1 when it reaches the goal

    def quit_game(self):

        pygame.quit()
'''
    def calc_reward(self):
        done = False
        for obs in self.obstacle_list:

            done = self.check_overlap(self.state[0], (obs.x, obs.y))
            if done:
                return -1, done

        if self.calculate_distance(self.state[0], self.state[1]) < self.goal_threshold:

            done = True
            reward = 1 - self.total_reward_accumulated
            self.total_reward_accumulated += reward

            return reward, done

        else:
            cur_dist = self.calculate_distance(self.state[0], self.state[1])

            diff_dist = self.old_dist - cur_dist
            reward = diff_dist / self.total_distance
            self.total_reward_accumulated += reward
            return reward, done
'''


if __name__ == '__main__':
    print "ddd"
    cb = createBoardIRL(weights=np.random.rand(15) , display=True)
    for i in range(100):
        print "Here"
        cb.reset()
        for j in range(300000):
            if cb.display:
                cb.render()
            action = cb.take_action_from_userKeyboard()

            #action = cb.gen_action_from_agent()
            action = cb.agent_action_to_WorldActionSimplified(action)
            print action
            #print cb.sensor_readings
            state ,reward ,done , _ = cb.step(action)
            #print reward
            if done:
                break
        #print reward
        #print cb.total_reward_accumulated

    pygame.quit()


import pygame
import numpy as np
import math
import gym
import torch
import random
import featureExtractor
import os
from nn import Neural_net as NN
random.seed(4)
_screen_height = 100
_screen_width = 100
_stripobsx = 0
_stripobsy = 20
_stripgoalx = 70
_stripgoaly = 20
_stripagentx = 80
_stripagenty = 10
_max_agent_speed = 5
weights = [1,1,1,1]


class Obstacle:
    goals = None

    def __init__(self,id,xpos = None , ypos = None , xvel = None , yvel = None, radius = None):
        self.id = id
        if xpos==None:
            self.x = np.random.randint(_stripobsx,_screen_width-_stripobsx)
        else:
            self.x = xpos

        if ypos==None:
            self.y = np.random.randint(_stripobsy,_screen_height-_stripobsy)
        else:
            self.y = ypos
        if radius==None:
            self.rad = 20
        else:
            self.rad = radius
        self.curr_goal = None #this is a tuple (x,y)
        if xvel == None:
            self.vel_x = 0
        else:
            self.vel_x = xvel
        if yvel == None:
            self.vel_y = 0
        else:
            self.vel_y = yvel

        self.goal_change_counter = None
        self.curr_counter = 0




class createBoardIRL():
    #saved model contains the state dictionary and not the actual model
    def __init__(self, saved_model = None, sensor_size = None, display = False ,number_of_actions = 4 , hidden_layers = [256 , 256] ,number_of_height = _screen_height , weights = weights ,  width = _screen_width , agent_radius = 10 , static_obstacles = 1 , dynamic_obstacles = 0 , static_obstacle_radius = 10 , dynamic_obstacle_radius = 0 , obstacle_speed_list = []):

        pygame.init()
        self.clock = pygame.time.Clock()
        self.rewardWeights = weights  #numpy array
        self.display = display
        self.gameExit = False
        self.height = _screen_height
        self.width = _screen_width
        self.agent_radius = agent_radius


        self.sensorArraysize = sensor_size


        self.size_action_space = number_of_actions


        self.max_agent_speed = _max_agent_speed
        self.hidden_layers = hidden_layers
        self.no_static_obstacles = static_obstacles
        self.no_dynamic_obstacles = dynamic_obstacles
        self.total_obs = self.no_static_obstacles+self.no_dynamic_obstacles
        self.rad_static_obstacles = static_obstacle_radius
        self.rad_dynamic_obstacles = dynamic_obstacle_radius
        self.agent_action_flag = False
        self.agent_action_keyboard = [False for i in range(4)]

        self.agent_x = None
        self.agent_y = None
        self.agent_x_vel = 0
        self.agent_y_vel = 0


        self.goal_x = None
        self.goal_y = None
        self.old_dist = None
        self.goal_threshold = 20
        self.total_distance = None
        self.obstacle_speed_list = obstacle_speed_list
        self.static_obstacle_list = []
        self.dynamic_obstacle_list = []
        self.obstacle_list = []
        self.sensor_readings = None #numpy array

        if saved_model==None:
            self.agentBrain = NN(self.sensorArraysize , self.hidden_layers,self.size_action_space)
            self.agentBrain.cuda()
        else:
            self.agentBrain = NN(self.sensorArraysize , self.hidden_layers,self.size_action_space)
            self.agentBrain.load_state_dict(torch.load(saved_model))
            self.agentBrain.eval()
            self.agentBrain.cuda()

    #state : list
    #state[0] - tuple containing agent current position
    #state[1] - tuple containing goal position.
    #state[2] - distance from goal
    #state[3] - done?
    #state[4 - end] - tuple obstacle position


        self.state = None
        self.reward = None
        self.total_reward_accumulated = None

        self.white = (255,255,255)
        self.red = (255,0,0)
        self.green = (0,255,0)
        self.blue = (0,0,255)
        self.black = (0,0,0)
        self.caption = 'social navigation world'
        print "initialization done."

    def calculate_distance(self, tup1, tup2):
        x_diff = tup1[0] - tup2[0]
        y_diff = tup1[1] - tup2[1]
        return math.sqrt(math.pow(x_diff, 2) + math.pow(y_diff, 2))


    def check_overlap(self, tup1, tup2):
        dist = self.calculate_distance(tup1, tup2)
        if dist > (self.rad_static_obstacles + self.agent_radius):
            return False
        else:
            return True


    #this method is new for the IRL class
    #this method takes in the state information generated by the environment at each step
    #and converts it into sensory information format

    #sensory information format is as follows:
    #sensory dictionary :
    #   deviation - float ?? --remove as of now
    #   rel_vel - tuple (2)
    #   lidar_info - array [36]
    #   cur_vel - tuple(2)
    #   cur_pos - tuple(2)
    #   distance_from_goal - float
    #   distance_form_closest_obstacle - float
    #   action taken - tuple(2)


    def state_to_sensorReadings(self):

        sensor_readlist = []
        #obstacle_info = self.state[3:]
        closest_obs_dist , rel_vel = self.calculateDistanceFromClosestObstacle()
        sensor_readlist.append(rel_vel[0])
        sensor_readlist.append(rel_vel[1])
        lidar_info = self.calculateLIDARInfo(100)
        for i in range(lidar_info.size):
            sensor_readlist.append(lidar_info[i])

        sensor_readlist.append(self.agent_x_vel)
        sensor_readlist.append(self.agent_y_vel)
        sensor_readlist.append(self.agent_x)
        sensor_readlist.append(self.agent_y)
        sensor_readlist.append(self.state[2])
        sensor_readlist.append(closest_obs_dist)

        return np.asarray(sensor_readlist)




    def calculateDistanceFromClosestObstacle(self):

        min_dist = 10000
        rel_x = None
        rel_y = None
        for i in range(len(self.obstacle_list)):

            obs = self.obstacle_list[i]

            dist = np.linalg.norm([(obs.x - self.agent_x),(obs.y - self.agent_y)])
            if dist < min_dist:

                min_dist = dist
                rel_x = obs.vel_x - self.agent_x_vel
                rel_y = obs.vel_y - self.agent_y_vel


        return min_dist , (rel_x,rel_y)

    #returns an array of size 36
    def calculateLIDARInfo(self, lidar_range):

        lidarHist = np.asarray([lidar_range for i in range(36)])
        for i in range(len(self.obstacle_list)):
            obs = self.obstacle_list[i]
            dist = np.linalg.norm([(obs.x - self.agent_x),(obs.y - self.agent_y)])
            bin = self.getBin(obs)

            if dist < lidarHist[bin]:
                lidarHist[bin] = dist

        return lidarHist


    def getBin(self, obs):

        v1 = np.asarray([0,1])
        v2 = np.asarray([obs.x - self.agent_x , obs.y - self.agent_y])
        angle_diff = self.angle_between(v1,v2)
        if v1[0]>v2[0]:
            angle_diff = 2*math.pi - angle_diff
        angle_diff = (angle_diff*360)/(2*math.pi)
        angle_diff = angle_diff%360
        return int(angle_diff//10)



    def unit_vector(self,vector):
        #print "vector"
        return vector / np.linalg.norm(vector)

    def angle_between(self,v1, v2):
        #Returns the angle in radians between vectors 'v1' and 'v2'::
        v1_u = self.unit_vector(v1)
        v2_u = self.unit_vector(v2)
        return np.arccos(np.clip(np.dot(v1_u, v2_u), -1.0, 1.0))



    def take_action_from_userMouse(self):
        (a, b, c) = pygame.mouse.get_pressed()

        x = 0.0001
        y = 0.0001
        print "heresa"
    #while (True):
        for event in pygame.event.get():
            if event.type == pygame.MOUSEBUTTONDOWN:
                print "heare"
                self.agent_action_flag = True
                if event.type == pygame.MOUSEBUTTONUP:
                    self.agent_action_flag = False
        if self.agent_action_flag:
            (x, y) = pygame.mouse.get_pos()

            x = x - self.agent_x
            y = y - self.agent_y
            if np.hypot(x, y) > _max_agent_speed:
                normalizer = _max_agent_speed / (np.hypot(x, y))
        # print x,y
            else:
                normalizer = 1
            print (x * normalizer, y * normalizer)
            return (x * normalizer, y * normalizer)

        return (0, 0)

    #a simplified version where there are just 4actions
    def take_action_from_userKeyboard(self):

        while (True):
            for event in pygame.event.get():
                if event.type == pygame.KEYDOWN:
                    key = pygame.key.get_pressed()
                    if key[pygame.K_UP]:
                        self.agent_action_keyboard[0]=True
                    if key[pygame.K_RIGHT]:
                        self.agent_action_keyboard[1]=True
                    if key[pygame.K_LEFT]:
                        self.agent_action_keyboard[3]=True
                    if key[pygame.K_DOWN]:
                        self.agent_action_keyboard[2]=True

                if event.type == pygame.KEYUP:

                    if event.key == pygame.K_UP:
                        self.agent_action_keyboard[0]=False
                    if event.key == pygame.K_RIGHT:
                        self.agent_action_keyboard[1]=False
                    if event.key == pygame.K_LEFT:
                        self.agent_action_keyboard[3]=False
                    if event.key == pygame.K_DOWN:
                        self.agent_action_keyboard[2]=False

            for i in range(len(self.agent_action_keyboard)):
                if self.agent_action_keyboard[i]==True:
                    return i

        return None
    #returns the action that has the highest qvalue
    def gen_action_from_agent(self):


        qvalues = self.agentBrain(self.sensor_readings)

        qvalues = qvalues.cpu().detach().numpy()
        return np.argmax(qvalues)

    #this method takes in the result from gen_action_from_agent()
    #and converts into a tuple (x,y) where (x,y) is the amount the
    #agent will move in the x and y direction respectively
    def agent_action_to_WorldAction(self, action):

        action_angle = 2*math.pi * action/self.size_action_space

        action_vector = np.array([0, self.max_agent_speed])
        rot_matrix  = np.array([[math.cos(action_angle), -math.sin(action_angle)],[math.sin(action_angle), math.cos(action_angle)]])

        action_taken = np.matmul(rot_matrix, action_vector)

        return action_taken

    #a simplified version: only 4 ways to move
    #action (int) can attain values from 0-3, and based on that
    #the agent moves left right front or back
    def agent_action_to_WorldActionSimplified(self,action):
        if action==0: #move front
            return np.asarray([0,-5])
        if action==1: #move right
            return np.asarray([5,0])
        if action==2: #move down
            return np.asarray([0,5])
        if action==3: #move left
            return np.asarray([-5,0])




    def check_overlap_rect(self, tup1, tup2, rad):
        if abs(tup1[0] - tup2[0]) < (rad + self.agent_radius) and abs(tup1[1] - tup2[1]) < (
                rad / 2 + self.rad_static_obstacles):
            return True
        else:
            return False


    def generate_randomval(self, lower, upper):
        i = np.random.ranf()
        return lower + i * (upper - lower)


    def reset(self):
        self.agent_action_flag = False
        if self.display:

            self.gameDisplay = pygame.display.set_mode((self.width, self.height))
            pygame.display.set_caption('social navigation world')
            self.gameDisplay.fill(self.white)



        self.obstacle_list = []
        self.goal_x = self.generate_randomval(_screen_width - _stripgoalx, _screen_width)
        self.goal_y = self.generate_randomval(_screen_height - _stripgoaly, _screen_height)
        agent_x = self.generate_randomval(0, _stripagentx)
        agent_y = self.generate_randomval(0, _stripagenty)
        dist = math.sqrt(math.pow(self.goal_x - agent_x, 2) + math.pow(self.goal_y - agent_y, 2))
        self.old_dist = dist
        while (1):
            if self.calculate_distance((self.goal_x, self.goal_y), (agent_x, agent_y)) < 50:
                agent_x = self.generate_randomval(0, _stripagentx)
                agent_y = self.generate_randomval(0, _stripagenty)
            else:
                break
        self.state = [(agent_x, agent_y), (self.goal_x, self.goal_y), dist , 0] #0 - not done /1 - done
        # print('PPPP',self.total_obstacles)
        self.agent_x = agent_x
        self.agent_y = agent_y
        self.total_reward_accumulated = 0
        # intialize the static obstacles
        for i in range(self.no_static_obstacles):
            while (1):
                # print("hit")
                temp_obs = Obstacle(i)
                if (not self.check_overlap_rect((temp_obs.x, temp_obs.y), (agent_x, agent_y),
                                                self.rad_static_obstacles) and not self.check_overlap_rect(
                    (temp_obs.x, temp_obs.y), (self.goal_x, self.goal_y), self.rad_static_obstacles)):
                    self.static_obstacle_list.append(temp_obs)
                    self.state.append((temp_obs.x, temp_obs.y))
                    self.obstacle_list.append(temp_obs)
                    break
        # print(len(self.obstacle_list))

        # initialize the dynamic obstacles
        for i in range(self.no_dynamic_obstacles):
            temp_obs = Obstacle(self.no_static_obstacles+i)
            temp_obs.speed = self.obstacle_speed_list[i]
            temp_obs.curr_goal = self.obstacle_goal_list[i]
            temp_obs.goal_change_counter = self.goal_change_step
            self.dynamic_obstacle_list.append(temp_obs)
            self.state.append((temp_obs.x, temp_obs.y))
            self.obstacle_list.append(temp_obs)

        self.total_distance = self.calculate_distance(self.state[0], self.state[1])

        #self.sensor_readings = self.state_to_sensorReadings()
        self.sensor_readings = featureExtractor.featureExtractor(self.state,self.obstacle_list,(self.agent_x_vel,self.agent_y_vel),self.agent_radius)
        #self.agentBrain = NN(self.state_to_sensorReadings().size , self.hidden_layers,self.size_action_space)

        return np.array(self.state)


    def renderObstacle(self, obs):
        pygame.draw.circle(self.gameDisplay, self.red, (obs.x, obs.y), obs.rad)




    def render(self):  # renders the screen using the current state of the environment

        self.gameDisplay.fill(self.white)
        for obs in self.obstacle_list:
            self.renderObstacle(obs)
        # draw goal
        pygame.draw.rect(self.gameDisplay, self.black, [self.goal_x, self.goal_y, 10, 10])
        # draw agent
        pygame.draw.circle(self.gameDisplay, self.black, (int(self.state[0][0]), int(self.state[0][1])), self.agent_radius)
        pygame.display.update()
        self.clock.tick(30)


    # updates the position of the objects in the environment according to the dynamics
    # action is a tuple (x,y), which gets added to the current
    # position of the agent

    def step(self, action):
        self.old_dist = self.calculate_distance(self.state[0], self.state[1])
        old_x = self.state[0][0]
        old_y = self.state[0][1]
        newx = self.state[0][0] + action[0]
        newy = self.state[0][1] + action[1]

        if newx < 0:
            newx = 0
        if newx > _screen_width:
            newx = _screen_width
        if newy < 0:
            newy = 0
        if newy > _screen_height:
            newy = _screen_height
        self.state[0] = (newx, newy) #update the position of the agent
        self.state[2] = self.calculate_distance(self.state[0] , self.state[1])#update the distance of the agent

        self.agent_x = self.state[0][0]
        self.agent_y = self.state[0][1]
        reward, done = self.calc_reward()
        if done:
            self.state[3] = 1 #episode done. Hit an obstacle or reached goal
        # print self.state[0]
        self.agent_x_vel = self.agent_x - old_x
        self.agent_y_vel = self.agent_y - old_y
        #print self.state

        #dynamic obstacles start from the index 3+no_of_static_obstacles
        #change the location of the dynamic obstacles for each step

        for i in range(len(self.dynamic_obstacle_list)):
            obs = self.dynamic_obstacle_list[i]
            x = obs.curr_goal[0] - obs.x
            y = obs.curr_goal[1] - obs.y
            magnitude = np.linalg.norm(x,y)
            obs.x = obs.x + (x/magnitude)*obs.speed
            obs.vel_x = (x/magnitude)*obs.speed
            obs.vel_y = (y/magnitude)*obs.speed
            obs.y = obs.y + obs.vel_y

            self.state[4+len(self.no_static_obstacles)+i] = (obs.x , obs.y)


        self.sensor_readings = featureExtractor.featureExtractor(self.state,self.obstacle_list,(self.agent_x_vel,self.agent_y_vel),self.agent_radius)
        if self.display:
            self.render()
        #print self.sensor_readings


        return np.array(self.state), reward, done, {}

    def calc_reward(self):
        done = False
        for obs in self.obstacle_list:

            done = self.check_overlap(self.state[0], (obs.x, obs.y))
            if done:
                return -1, done

        if self.calculate_distance(self.state[0], self.state[1]) < self.goal_threshold:

            done = True
            reward = np.dot(self.rewardWeights , self.sensor_readings)
            self.total_reward_accumulated += reward

            return reward, done

        else:
            cur_dist = self.calculate_distance(self.state[0], self.state[1])

            diff_dist = self.old_dist - cur_dist
            #rint self.rewardWeights.size
            #print self.sensor_readings.size
            #print 'reward_weights',self.rewardWeights.shape
            #print 'sensor_readings',self.sensor_readings.shape
            reward = np.dot(self.rewardWeights , self.sensor_readings)
            self.total_reward_accumulated += reward
            return reward, done

        return -1 , done
    # -1 if it hits an obstacle or fraction of the distance it travels towards
    # the goal with a total of 1 when it reaches the goal

    def quit_game(self):

        pygame.quit()
'''
    def calc_reward(self):
        done = False
        for obs in self.obstacle_list:

            done = self.check_overlap(self.state[0], (obs.x, obs.y))
            if done:
                return -1, done

        if self.calculate_distance(self.state[0], self.state[1]) < self.goal_threshold:

            done = True
            reward = 1 - self.total_reward_accumulated
            self.total_reward_accumulated += reward

            return reward, done

        else:
            cur_dist = self.calculate_distance(self.state[0], self.state[1])

            diff_dist = self.old_dist - cur_dist
            reward = diff_dist / self.total_distance
            self.total_reward_accumulated += reward
            return reward, done
'''


if __name__ == '__main__':
    print "ddd"
    cb = createBoardIRL(weights=np.random.rand(15) , display=True)
    for i in range(100):
        print "Here"
        cb.reset()
        for j in range(300000):
            if cb.display:
                cb.render()
            action = cb.take_action_from_userKeyboard()

            #action = cb.gen_action_from_agent()
            action = cb.agent_action_to_WorldActionSimplified(action)
            print action
            #print cb.sensor_readings
            state ,reward ,done , _ = cb.step(action)
            #print reward
            if done:
                break
        #print reward
        #print cb.total_reward_accumulated

    pygame.quit()


#this is a tuple (x,y) #saved model contains the state dictionary and not the actual model #numpy array #numpy array #state : list #state[0] - tuple containing agent current position #state[1] - tuple containing goal position. #state[2] - distance from goal #state[3] - done? #state[4 - end] - tuple obstacle position #this method is new for the IRL class #this method takes in the state information generated by the environment at each step #and converts it into sensory information format #sensory information format is as follows: #sensory dictionary : #   deviation - float ?? --remove as of now #   rel_vel - tuple (2) #   lidar_info - array [36] #   cur_vel - tuple(2) #   cur_pos - tuple(2) #   distance_from_goal - float #   distance_form_closest_obstacle - float #   action taken - tuple(2) #obstacle_info = self.state[3:] #returns an array of size 36 #print "vector" #Returns the angle in radians between vectors 'v1' and 'v2':: #while (True): # print x,y #a simplified version where there are just 4actions #returns the action that has the highest qvalue #this method takes in the result from gen_action_from_agent() #and converts into a tuple (x,y) where (x,y) is the amount the #agent will move in the x and y direction respectively #a simplified version: only 4 ways to move #action (int) can attain values from 0-3, and based on that #the agent moves left right front or back #move front #move right #move down #move left #0 - not done /1 - done # print('PPPP',self.total_obstacles) # intialize the static obstacles # print("hit") # print(len(self.obstacle_list)) # initialize the dynamic obstacles #self.sensor_readings = self.state_to_sensorReadings() #self.agentBrain = NN(self.state_to_sensorReadings().size , self.hidden_layers,self.size_action_space) # renders the screen using the current state of the environment # draw goal # draw agent # updates the position of the objects in the environment according to the dynamics # action is a tuple (x,y), which gets added to the current # position of the agent #update the position of the agent #update the distance of the agent #episode done. Hit an obstacle or reached goal # print self.state[0] #print self.state #dynamic obstacles start from the index 3+no_of_static_obstacles #change the location of the dynamic obstacles for each step #print self.sensor_readings #rint self.rewardWeights.size #print self.sensor_readings.size #print 'reward_weights',self.rewardWeights.shape #print 'sensor_readings',self.sensor_readings.shape # -1 if it hits an obstacle or fraction of the distance it travels towards # the goal with a total of 1 when it reaches the goal def calc_reward(self):         done = False         for obs in self.obstacle_list:              done = self.check_overlap(self.state[0], (obs.x, obs.y))             if done:                 return -1, done          if self.calculate_distance(self.state[0], self.state[1]) < self.goal_threshold:              done = True             reward = 1 - self.total_reward_accumulated             self.total_reward_accumulated += reward              return reward, done          else:             cur_dist = self.calculate_distance(self.state[0], self.state[1])              diff_dist = self.old_dist - cur_dist             reward = diff_dist / self.total_distance             self.total_reward_accumulated += reward             return reward, done #action = cb.gen_action_from_agent() #print cb.sensor_readings #print reward #print reward #print cb.total_reward_accumulated