#!/usr/bin/env python

# IMPORTS

import rospy
from geometry_msgs.msg import Twist, Point
from sensor_msgs.msg import LaserScan
from nav_msgs.msg import Odometry
from tf.transformations import euler_from_quaternion
from math import atan2, sqrt, pi, atan, cos, tan, sin, acos


# VARIABLES

# initialse an empty list of 720 slots (this is for the laser)
distances = [None]*720

# set up message types
vel_msg = Twist()
goal = Point()

# set up goal location
setCoordinates = False
temTraj = False
# variables of current location
x = 0
y = 0
theta = 0


objectSpotted = False

# write msg.range into our list of distances for use in the rest of the code

# SUBSCRIBER READERS


def LaserReadings(msg):  # deals with laserScan msg
    global distances

    for i in range(0, 719):
        distances[i] = msg.ranges[i]


def newOdom(msg):  # deals with position msg

    global x
    global y
    global theta

    x = msg.pose.pose.position.x
    y = msg.pose.pose.position.y

    rot_q = msg.pose.pose.orientation
    (roll, pitch, theta) = euler_from_quaternion(
        [rot_q.x, rot_q.y, rot_q.z, rot_q.w])


# STARTUP NODE
rospy.init_node('avoider')
velocity_publisher = rospy.Publisher('/cmd_vel', Twist, queue_size=1)

# THE SUBSCRIBERS
rospy.Subscriber('kobuki/laser/scan', LaserScan, LaserReadings)
sub = rospy.Subscriber('odom', Odometry, newOdom)


# THE METHODS
def userCooridnates():  # input location you wish to go
    global X_goal
    global Y_goal
    X_goal = float(input('Enter the X coordinate:  '))
    Y_goal = float(input('Enter the Y coordinate:  '))
    print('Printing type of input value')
    print ('the X,Y coordiante you picked is:', X_goal, Y_goal)

    # this means we control the robot relative to it's current position
    local2globalCoordiates(X_goal, Y_goal)

    global X_mainGoal
    global Y_mainGoal

    X_mainGoal = X_goal
    Y_mainGoal = Y_goal

    global setCoordinates
    setCoordinates = True


def objExsist(area):  # checks "areas" for objects mostly
    firstDetect = False
    # 0 = boolean of object exsiting   1= left of area   2 = right of area   3 = range
    for i in range(area[2], area[1]):
        if distances[i] <= area[5] and distances[i] > 0.1:
            area[0] = True
            global objectSpotted
            objectSpotted = True
            if firstDetect is False:
                area[4] = i  # rightFirstDetect
                firstDetect = True
            area[3] = i  # leftLastDetect

    # remeber the laser starts at the right and works its way left


def resetAreaBoolean():  # resets all the areas as empty
    front[0] = False
    left[0] = False
    right[0] = False
    global objectSpotted
    objectSpotted = False


def areaObjectFinder():  # setup for areas and start userCooridnates()
    # set up fror distance and area parameters

    # Set up ranges
    Range = 0.8

    # Robot clearnace width
    robotWidth = 0.7

    # Set up angles of areas
    # 180/pi is to convert from radians to degrees.
    # 4 gets is from a 180 to 720 reference.
    # the 90 is the center of what we are doing
    leftDiagFront = int((180/pi)*4*((pi/2)+atan((robotWidth/2)/Range)))
    rightDiagFront = int((180/pi)*4*((pi/2)-atan((robotWidth/2)/Range)))
    rightMax = 0
    leftMax = 719
    leftLastDetect = 0
    rightFirstDetect = 0

    # Set up the different areas and initialise them as having no obstacles.
    global front
    global right
    global left
    front = [False, leftDiagFront, rightDiagFront,
             leftLastDetect, rightFirstDetect, Range]
    right = [False, rightDiagFront, rightMax,
             leftLastDetect, rightFirstDetect, Range]
    left = [False, leftMax, leftDiagFront,
            leftLastDetect, rightFirstDetect, Range]
    # We then check this area for objects
    objExsist(front)
    objExsist(left)
    objExsist(right)


def p2pWriter():  # moves robot to X_goal and Y_goal
    inc_x = X_goal - x
    inc_y = Y_goal - y
    angle_to_goal = atan2(inc_y, inc_x)

    # avoids gittering of the robot as it' unsure if goal is at 3.14 or -3.14
    if abs(angle_to_goal) >= 3.0:
        angle_to_goal = abs(angle_to_goal)

    # print('the angle_to_goal = ', angle_to_goal)
    if pi >= (angle_to_goal - theta) >= -pi:

        if abs(angle_to_goal - theta) > 0.1 and (angle_to_goal - theta) >= 0:
            vel_msg.linear.x = 0.0
            vel_msg.angular.z = 0.4  # turns left
        elif abs(angle_to_goal - theta) > 0.1 and (angle_to_goal - theta) < 0:
            vel_msg.linear.x = 0.0
            vel_msg.angular.z = -0.4  # turns right
        else:
            vel_msg.linear.x = 0.4
            vel_msg.angular.z = 0.0

    elif (angle_to_goal - theta) < -pi or (angle_to_goal - theta) > pi:
        if abs(angle_to_goal + theta) > 0.1 and (angle_to_goal + theta) <= 0:
            vel_msg.linear.x = 0.0
            vel_msg.angular.z = -0.4  # turns right
        elif abs(angle_to_goal + theta) > 0.1 and (angle_to_goal + theta) > 0:
            vel_msg.linear.x = 0.0
            vel_msg.angular.z = 0.4  # turns left
        else:
            print('correct heading, going straight')
            vel_msg.linear.x = 0.4
            vel_msg.angular.z = 0.0


def seekTheTea():  # main body of code thats called in infinit loop. Manages program
    inc_x = X_goal - x
    inc_y = Y_goal - y

    # we check if objects exsist in areas
    areaObjectFinder()
    # we check if we are at the goal
    if sqrt(inc_y*inc_y + inc_x*inc_x) <= 0.1:
        global temTraj
        if temTraj is False:
            global setCoordinates
            setCoordinates = False
            print ('we have arrived at the goal')
            vel_msg.linear.x = 0.0
            vel_msg.angular.z = 0.0
        else:
            temTraj = False
            print ('we have arrived at the subGoal')
            vel_msg.linear.x = 0.0
            vel_msg.angular.z = 0.0
            backToGlobalPlan()

    # when an object is in the way we start object avoidance
    elif objectSpotted is True and temTraj is False:
        print('in spotted object', objectSpotted)
        vel_msg.linear.x = 0.0
        vel_msg.angular.z = 0.0
        velocity_publisher.publish(vel_msg)
        newTempPath()

    # the normal behavior of the program is to go towards the goal
    else:
        p2pWriter()
    resetAreaBoolean()
    velocity_publisher.publish(vel_msg)


def newTempPath():  # deals with obstacles by making temporary SubGoals
    step = 0.3  # how far we shift in meters

    # this signifies we have started a temporary goal
    global temTraj
    temTraj = True

    # here we check the angles of the different areas
    if front[0] is True:
        angleOfObject(step, front)

    # here we make the logic decisions
    # if front[0] is True and left[0] is True and right[0] is True:

    # what to do if stuck in leftward corner
    if front[0] is True and left[0] is True:
        vel_msg.linear.x = 0
        # set truning speed
        vel_msg.angular.z = 0.4  # turns left
        temTraj = False

    # what do do if stuck in right ward corner
    elif front[0] is True and right[0] is True:
        vel_msg.linear.x = 0
        # set truning speed
        vel_msg.angular.z = -0.4  # turns right
        temTraj = False

    # LEFT what to do if folling a wall or having one clip your boundaries
    elif left[0] is True:
        vel_msg.linear.x = 0.2
        # set truning speed
        vel_msg.angular.z = -0.1  # turns left
        temTraj = False
    # RIGHT what to do if folling a wall or having one clip your boundaries
    elif right[0] is True:
        vel_msg.linear.x = 0.2
        # set truning speed
        vel_msg.angular.z = 0.1  # turns right
        temTraj = False
    # What to do if wall is infront of you
    elif front[0] is True:
        angleOfObject(step, front)
        local2globalCoordiates(depth, direction)

    print('heading to new goal')
    print('@@@@@@ we can set new goal to go for : X ', X_goal, ' Y ', Y_goal)


def angleOfObject(step, area):  # test the angle of object in area near robot
    # for now we deal with objects that are in front
    global depth
    global direction
    print('area4: ', area[4])
    print('area3: ', area[3])

    right = distances[area[4]]
    left = distances[area[3]]
    print('right: ', right)
    print('left: ', left)

    # backups incase of issues
    if (area[3] - area[4]) <= 10:
        objExsist(area)
        right = distances[area[4]]
        left = distances[area[3]]
    if (area[3] - area[4]) <= 10:
        depth = step*tan(0)
        imagRight = 0
    else:
        # trigonometry ..
        pp2 = 0.0
        imagRight = 0.0
        c = 0.0
        h = sqrt(left*left+pp2*pp2)
        angle = (area[3] - area[4])*0.00436940183863
        print('h: ', h)
        print('angle: ', angle)
        wallAngle = 0.0
        pp2 = left*tan(angle)
        print('pp2: ', pp2)
        imagRight = right - h
        print('imagRight: ', imagRight)
        c = sqrt(left*left+right*right-2*left*right*cos(angle))
        print('c: ', c)
        num = (pp2*pp2+c*c-(imagRight*imagRight))/(pp2*c*2)-0.005
        print('num: ', num)
        if num >= 0:
            depth = step*tan(0)
            imagRight = 0
        else:
            wallAngle = acos(num)
            print('wallAngle: ', wallAngle)
            depth = step*tan(wallAngle)

    # decision making
    if imagRight <= 0:
        direction = step
        depth = -depth
    else:
        direction = -step

    print('print depth  ', depth)
    print('print direction  ', direction)


# translate from robot coordiants to global
def local2globalCoordiates(X_local, Y_local):
    # for X_local
    xy = X_local*sin(theta)  # the y
    xx = X_local*cos(theta)  # the x

    # for Y_local
    yy = Y_local*sin(theta+(pi/2))  # the y
    yx = Y_local*cos(theta+(pi/2))  # the x

    # set goals
    global X_goal
    global Y_goal

    # we add as well x,y of the robots position (as we don't move from 00)
    Y_goal = xy + yy + y
    X_goal = xx + yx + x
    print('xx: ', xx, ' yx: ', yx, ' x: ', x)

    print('INSIDE LOCAL2GLOBAL .. we can set new goal to go for : X ',
          X_goal, ' Y ', Y_goal)


def backToGlobalPlan():  # resets from sub goal to a main goal
    global X_goal
    global Y_goal
    X_goal = X_mainGoal
    Y_goal = Y_mainGoal


while not rospy.is_shutdown():
    rospy.sleep(0.1)
    if setCoordinates is False:
        userCooridnates()
    seekTheTea()
