def setHeuristic(searchNode2):
    #print "start", searchNode2
    allFoldList = list(fold_sequence)
    #print allFoldList
    completedFoldList = []
    searchNode = searchNode2
    for poly in searchNode.get_polys():
        gui.addPropCVShape(poly)
    #raw_input()
    gui.clearProposed()

    for fold in allFoldList:
        '''
        print "\n\n\n\n\n\ New SearchNode"
        child2 = simulateDrag(searchNode,10, '-x', True)
        if child2:
            print "\n\n\n\n polys of drag \n\n\n"
            for poly in child2.get_polys():
                gui.addPropCVShape(poly)
                print poly
            
            print "In drag: drawing grippers"
            for g in child2.gripPts:
                gui.drawGripper(g)
            
            raw_input()
            gui.clearProposed()
            '''
        child, gripPts, endPts = simulateFold(searchNode,fold,transFold = fold,isHeuristic= True)
        #print "Child in set heuristic" , child, gripPts, endPts
        if child:
            completedFoldList.append(fold)
            fold.gripPoints = gripPts
            fold.endPoints = endPts
            clothConfig[tuple(getTupOfCompletedFolds(completedFoldList))] = child.get_polys()
            #completedFoldList.append(fold)
            #compString = getStringOfList(completedFoldList)
            #clothConfig[compString] = child.get_polys()
            
            for poly in child.get_polys():
                gui.addPropCVShape(poly)
            #raw_input()
            gui.clearProposed()
            
    
            #print "Child in set heuristic" , child, gripPts, endPts
            maxDistance = float(max(Geometry2D.ptMagnitude(Geometry2D.ptDiff(pt1, pt2)) for pt1, pt2 in zip(gripPts, endPts)))   
            #print "Current GripPoint"
            h = 3 + (((maxDistance/util.scale_factor)/0.25))
            fold.setCost(h)
            searchNode = child
        else:
            fold.setCost(float("inf"))
            return False
    gui.UPDATE_GRAPHICS = False
    return True 
def setHeuristic(searchNode):
    print "start", searchNode
    allFoldList = list(fold_sequence)
    print allFoldList
    
    searchNode = searchNode
    for fold in allFoldList:
#        if gui.legalBlueFold(fold,searchNode.get_polys()):
        child, gripPts, endPts = simulateFold(searchNode,fold,transFold = fold,isHeuristic= True)
        foldActivePoints[fold] = (gripPts, endPts)
        #print "Child in set heuristic" , child, gripPts, endPts
        if child:
            maxDistance = float(max(Geometry2D.ptMagnitude(Geometry2D.ptDiff(pt1, pt2)) for pt1, pt2 in zip(gripPts, endPts)))   
            print "Current GripPoint"
            h = 3 + (((maxDistance/util.scale_factor)/0.25))*4 
            fold.setCost(h)
            searchNode = child
        else:
            fold.setCost(float("inf"))
            return False
    return True