#!/usr/bin/env python
# http://wiki.ros.org/actionlib#Python_SimpleActionServer
import rospy
import smach
import smach_ros
import tf
import math
from baseFuncs.searchFor import searchFor
from baseFuncs.moveTo import moveTo


machine = smach.StateMachine(outcomes=['success', 'fail'])


def isCollinear(p1, p2, p3):
     a = p1[0] * (p2[1] - p3[1]) + p2[0] * \
                  (p3[1] - p1[1]) + p3[0] * (p1[1] - p2[1])
    if (a < 10): 
        return True
    else: 
        return False

with machine:
    # Add states to the container

    # Gate finding
    smach.StateMachine.add('findGates',
                            searchFor(
                                targets=['red buoy', 'green buoy', 'white buoy']),
                            transitions={'found': 'findGates', 'partial_found': 'fail', 'timeout': 'fail'},
                            remapping={"objects":"data"})
    
    def find_beacons(data):
        # Extract IDs of start and end beacons
        items = []
        listener = tf.TransformListener()
        for i in data:
            # im assuming this is the relevant thing?
            (trans, rot) = listener.lookupTransform(
                '/'+i.frame_id, '/base_link', rospy.Time(0))
            dist = math.sqrt(trans[0] ** 2 + trans[1] ** 2)
            items.append((trans, i.best_guess, i.frame_id, dist))
        # figre out where the 4 entrance gates are: 4 buoys in line. 
        gates=False
        for i in items:
         for j in items:
          if i != j:
           for k in items:
            if k!=i and k!=j:
             if isCollinear(i[0],j[0],k[0]):
              for l in items:
               if l!=i and l!=j and l!=k:
                if isCollinear(i[0],j[0],l[0]):
                 gates=[i,j,k,l]
                 # yay we found it after 23i423 nestings.
        # Order the gates by their position along the line (just one of the x or y axes will do... take the larger one!)
        xory, value = max(enumerate([gates[0][0][0]-gates[1][0][0],gates[0][0][1]-gates[1][0][1]]), key=lambda i: i[1])
        items = sorted(gates, key=lambda i: i[0][xory])
        # stop here and return
        return items

    def chooseGate(data):
        # Extract IDs of start and end beacons
        items = []
        listener = tf.TransformListener()
        for i in data:
            # im assuming this is the relevant thing?
            listener.waitForTransform(
                '/'+i.frame_id, '/base_link', rospy.Time(0), rospy.Duration(4.0))
            (trans, rot) = listener.lookupTransform(
                '/'+i.frame_id, '/base_link', rospy.Time(0))
            dist = math.sqrt(trans[0] ** 2 + trans[1] ** 2)
            items.append((dist, i.best_guess, i.frame_id))
        # sort to give us closest items.
        items = sorted(items, key=lambda i: i[0])
        firstRed = None
        firstGreen = None
        for i in items:
            if firstRed is None and i[1].find("red"):
                firstRed = i
            if firstGreen is None and i[1].find("green"):
                firstGreen = i
        (rtrans, rot) = listener.lookupTransform(
            firstRed[2], '/map', rospy.Time(0))
        (gtrans, rot) = listener.lookupTransform(
            firstGreen[2], '/map', rospy.Time(0))
        midpoint = (-(rtrans[0]+gtrans[0])/2, -(rtrans[1]+gtrans[1])/2)
        angle = math.atan2((rtrans[0]-gtrans[0]),
                            (rtrans[1]-gtrans[1]))+math.pi/4
        mba = PoseStamped()
        mba.pose.position.x = midpoint[0]
        mba.pose.position.y = midpoint[1]
        mba.pose.position.z = 0.0
        # fix quarternion
        mba.pose.orientation.x = 0
        mba.pose.orientation.y = 0
        mba.pose.orientation.z = 0
        mba.pose.orientation.w = angle
        print(mba)
        print(rtrans)
        print(gtrans)
        return mba  # my code has a degree yay


    smach.StateMachine.add('findGates', executor(find_beacons),transitions={'done': 'pickGate', 'error': 'fail'},remapping={"result":"gatesFound"})

    smach.StateMachine.add('chooseGate', executor(find_beacons),transitions={'done': 'pickGate', 'error': 'fail'},remapping={"gatesFound":"data"})
    smach.StateMachine.add('navigate_to_start', moveTo(),transitions={'succeeded': 'find_end', 'preempted': 'fail', 'aborted': 'fail'})

    def midpointEnd(data):
        # Extract IDs of start and end beacons
        items = []
        listener = tf.TransformListener()
        for i in data:
            # im assuming this is the relevant thing?
            listener.waitForTransform(
                '/'+i.frame_id, '/map', rospy.Time(0), rospy.Duration(4.0))
            (trans, rot) = listener.lookupTransform(
                '/'+i.frame_id, '/map', rospy.Time(0))
            dist = math.sqrt(trans[0] ** 2 + trans[1] ** 2)
            items.append((dist, i.best_guess, i.frame_id))
        # sort to give us closest items.
        items = sorted(items, key=lambda i: i[0],reverse=True)
        firstRed = None
        firstGreen = None
        for i in items:
            if firstRed is None and i[1].find("red"):
                firstRed = i
            if firstGreen is None and i[1].find("green"):
                firstGreen = i
        (rtrans, rot) = listener.lookupTransform(
            firstRed[2], '/map', rospy.Time(0))
        (gtrans, rot) = listener.lookupTransform(
            firstGreen[2], '/map', rospy.Time(0))
        midpoint = (-(rtrans[0]+gtrans[0])/2, -(rtrans[1]+gtrans[1])/2)
        angle = math.atan2((rtrans[0]-gtrans[0]),
                            (rtrans[1]-gtrans[1]))+math.pi/4
        mba = PoseStamped()
        mba.pose.position.x = midpoint[0]
        mba.pose.position.y = midpoint[1]
        mba.pose.position.z = 0.0
        # fix quarternion
        mba.pose.orientation.x = 0
        mba.pose.orientation.y = 0
        mba.pose.orientation.z = 0
        mba.pose.orientation.w = angle
        print(mba)
        print(rtrans)
        print(gtrans)
        return mba  # my code has a degree yay
    smach.StateMachine.add('find_end', executor(midpointEnd),transitions={'done': 'navigate_to_end', 'error': 'fail'},remapping={"result":"goal"})
    smach.StateMachine.add('navigate_to_end', moveTo(),transitions={'succeeded': 'success', 'preempted': 'fail', 'aborted': 'fail'})

if __name__ == '__main__':
    rospy.init_node('smach_demonav_state_machine')
    # start introspection server
    # Create and start the introspection server
    if rospy.get_param("machineDebug",False)==True:
        sis = smach_ros.IntrospectionServer('server_name', machine, '/SM_ROOT')
        sis.start()
    # Execute the state machine
    outcome = machine.execute()
    # Wait for ctrl-c to stop the application
    rospy.spin()
    sis.stop()






