import org.apache.commons.math3.geometry.euclidean.threed.Segment as Segment
import org.apache.commons.math3.geometry.euclidean.threed.Vector3D as Vector3D
import org.apache.commons.math3.geometry.euclidean.threed.Line as Line3D

RECOVER_DESC = "mRecovery"
RECOVER_TOOL = TOOL_DEFAULT

known_segments = [ ("pHome",         "pPark",         50), \
                   ("pHome",         "pGonio",       30), \
                   ("pHome",         "pScan",         25), \
                   ("pHome",         "pHeater",       75), \
                   ("pHome",         "pDewar",        10), \
                   ("pHome",         "pHelium",      230), \
                   ("pGonio",        "pGonioG",       10), \
                   ("pPark",         "pHeat",         40), \
                   ("pHeater",       "pHeatB",        10), \
                   ("pPark",         "pAux",          50), \
                 ]

def get_dist_to_line(segment):
    tolerance = segment[2]
    p1, p2 = robot.get_pnt(segment[0]), robot.get_pnt(segment[1])
    p =  robot.get_cartesian_pos()
    v = Vector3D(p[0], p[1], p[2])
    v1  = Vector3D(p1[0], p1[1], p1[2])
    v2  = Vector3D(p2[0], p2[1], p2[2])
    l = Line3D(v1, v2, 0.01)
    return l.distance(v)


def get_dist_to_segment(segment):
    tolerance = segment[2]
    p1, p2 = robot.get_pnt(segment[0]), robot.get_pnt(segment[1])
    p =  robot.get_cartesian_pos()
    v = Vector3D(p[0], p[1], p[2])
    v1  = Vector3D(p1[0], p1[1], p1[2])
    v2  = Vector3D(p2[0], p2[1], p2[2])
    l = Line3D(v1, v2, 0.01)
    d = l.distance(v)    

    pj = get_pojection_at_line(segment)
    
    d1, d2 = v1.distance(v), v2.distance(v)
    dp1, dp2 = v1.distance(pj), v2.distance(pj)
    d12 = v1.distance(v2)
    
    
    if (dp1 + dp2) > (d12 + tolerance):
        d = max(d,min(d1,d2))
    return d
    
def is_on_segment(segment):
    tolerance = segment[2]
    d = get_dist_to_segment(segment)

    if d > tolerance:
        #print "Current robot position " + str(p) + " not on segment " + str(segment) + " - distance=" + str(d)
        return False
    #print "Current robot position " + str(p) + " on segment " + str(segment) + " - distance=" + str(d)
    return True

def get_pojection_at_line(segment):
    tolerance = segment[2]
    p1, p2 = robot.get_pnt(segment[0]), robot.get_pnt(segment[1])
    p =  robot.get_cartesian_pos()
    v = Vector3D(p[0], p[1], p[2])
    v1  = Vector3D(p1[0], p1[1], p1[2])
    v2  = Vector3D(p2[0], p2[1], p2[2])
    l = Line3D(v1, v2, 0.01)
    a = l.getAbscissa(v)
    lv = l.pointAt(a) 
    return lv
    

def get_current_segment():
    for segment in known_segments:
        if is_on_segment(segment):    
            return segment    
    return None

def get_current_distance():    
    for segment in known_segments:
        if is_on_segment(segment):  
            return get_dist_to_segment(segment)   
    return None


def move_to_segment(segment):
    tolerance = segment[2]
    p =  robot.get_cartesian_pos()
    v = Vector3D(p[0], p[1], p[2])
    lv = get_pojection_at_line(segment)
    dlv = lv.distance(v)
    if dlv> (tolerance + 0.1):
        raise Exception( "Error moving from " + str(p) + " to segment - distance=" + str(dlv))
    d =  [lv.x, lv.y, lv.z, p[3], p[4], p[5]]
    print "Moving from " + str(p) + " to segment " + str(segment) + " - distance=" + str(dlv)  + " - dest=" + str(d)
        
    try: 
        robot.movel(d, tool=RECOVER_TOOL, desc=RECOVER_DESC, sync=True)       
        print "Done" 
    except:
        print sys.exc_info()[1]

#Moves to first point of the segment ehich is safer, unless in the vicinity of the second
def move_to_safest_point(segment, vicinity_tolerance = 100):
    d1, d2 = robot.get_distance_to_pnt(segment[0]),  robot.get_distance_to_pnt(segment[1])
    #Always moving to primary point
    if False : #(d2<=d1) and (d2 <= vicinity_tolerance):
        print "Moving to secondary point " + str(segment[1] + " - d1=" + str(d1) + " d2=" + str(d2) )
        robot.movel(segment[1], tool=RECOVER_TOOL, desc=RECOVER_DESC, sync=True) 
    else:
        print "Moving to primary point " + str(segment[0] + " - d1=" + str(d1) + " d2=" + str(d2) )
        robot.movel(segment[0], tool=RECOVER_TOOL, desc=RECOVER_DESC, sync=True) 
    print "Done" 
    #print "Recovered to point " + str(robot.get_curjoint_or_pointrent_point())

     
def recover():
    #Initial checks
    robot.assert_no_task()
    robot.reset_motion()
    robot.wait_ready()

    if robot.get_current_point() is not None:
        raise Exception("Robot is in known location")

    set_status("Recovering")
    
    #Enabling
    enable_motion()


    is_on_known_segment = False
    for segment in known_segments:
        if is_on_segment(segment):
            #try:
            #    robot.set_monitor_speed(5)
                is_on_known_segment = True
                move_to_segment(segment)
                move_to_safest_point(segment)
                location = robot.get_current_point()
                if location is None:
                    raise Exception("Robot didn't reach known point")
                return "Success recovering to point: " +  str(location)
            #finally:     
            #    robot.set_default_speed()
    if not is_on_known_segment:
        raise Exception("Robot is not in known segment")