<commit_before>! Copyright (C) 2010 <NAME>.
! See http://factorcode.org/license.txt for BSD license.
USING: accessors assocs hash-sets heaps kernel math sequences sets shuffle ;
IN: path-finding

! This implements the A* algorithm. See http://en.wikipedia.org/wiki/A*

TUPLE: astar g in-closed-set ;
GENERIC: cost ( from to astar -- n )
GENERIC: heuristic ( from to astar -- n )
GENERIC: neighbours ( node astar -- seq )

<PRIVATE

TUPLE: (astar) astar goal origin in-open-set open-set ;

: (add-to-open-set) ( h node astar -- )
    2dup in-open-set>> at* [ over open-set>> heap-delete ] [ drop ] if
    [ swapd open-set>> heap-push* ] [ in-open-set>> set-at ] 2bi ;

: add-to-open-set ( node astar -- )
    [ astar>> g>> at ] 2keep
    [ [ goal>> ] [ astar>> heuristic ] bi + ] 2keep
    (add-to-open-set) ;

: ?add-to-open-set ( node astar -- )
    2dup astar>> in-closed-set>> in? [ 2drop ] [ add-to-open-set ] if ;

: move-to-closed-set ( node astar -- )
    [ astar>> in-closed-set>> adjoin ] [ in-open-set>> delete-at ] 2bi ;

: get-first ( astar -- node )
    [ open-set>> heap-pop drop dup ] [ move-to-closed-set ] bi ;

: set-g ( origin g node astar -- )
    [ [ origin>> set-at ] [ astar>> g>> set-at ] bi-curry bi-curry bi* ] [ ?add-to-open-set ] 2bi ;

: cost-through ( origin node astar -- cost )
    [ astar>> cost ] [ nip astar>> g>> at ] 3bi + ;

: ?set-g ( origin node astar -- )
    [ cost-through ] 3keep [ swap ] 2dip
    3dup astar>> g>> at [ 1/0. ] unless* > [ 4drop ] [ set-g ] if ;

: build-path ( target astar -- path )
    [ over ] [ over [ [ origin>> at ] keep ] dip ] produce 2nip reverse ;

: handle ( node astar -- )
    dupd [ astar>> neighbours ] keep [ ?set-g ] curry with each ;

: (find-path) ( astar -- path/f )
    dup open-set>> heap-empty? [
        drop f
    ] [
        [ get-first ] keep 2dup goal>> = [ build-path ] [ [ handle ] [ (find-path) ] bi ] if
    ] if ;

: (init) ( from to astar -- )
    swap >>goal
    H{ } clone over astar>> g<<
    { } <hash-set> over astar>> in-closed-set<<
    H{ } clone >>origin
    H{ } clone >>in-open-set
    <min-heap> >>open-set
    [ 0 ] 2dip [ (add-to-open-set) ] [ astar>> g>> set-at ] 3bi ;<commit_msg>Fix comparison to avoid infinite loop with zero cost paths<commit_after>! Copyright (C) 2010 <NAME>.
! See http://factorcode.org/license.txt for BSD license.
USING: accessors assocs hash-sets heaps kernel math sequences sets shuffle ;
IN: path-finding

! This implements the A* algorithm. See http://en.wikipedia.org/wiki/A*

TUPLE: astar g in-closed-set ;
GENERIC: cost ( from to astar -- n )
GENERIC: heuristic ( from to astar -- n )
GENERIC: neighbours ( node astar -- seq )

<PRIVATE

TUPLE: (astar) astar goal origin in-open-set open-set ;

: (add-to-open-set) ( h node astar -- )
    2dup in-open-set>> at* [ over open-set>> heap-delete ] [ drop ] if
    [ swapd open-set>> heap-push* ] [ in-open-set>> set-at ] 2bi ;

: add-to-open-set ( node astar -- )
    [ astar>> g>> at ] 2keep
    [ [ goal>> ] [ astar>> heuristic ] bi + ] 2keep
    (add-to-open-set) ;

: ?add-to-open-set ( node astar -- )
    2dup astar>> in-closed-set>> in? [ 2drop ] [ add-to-open-set ] if ;

: move-to-closed-set ( node astar -- )
    [ astar>> in-closed-set>> adjoin ] [ in-open-set>> delete-at ] 2bi ;

: get-first ( astar -- node )
    [ open-set>> heap-pop drop dup ] [ move-to-closed-set ] bi ;

: set-g ( origin g node astar -- )
    [ [ origin>> set-at ] [ astar>> g>> set-at ] bi-curry bi-curry bi* ] [ ?add-to-open-set ] 2bi ;

: cost-through ( origin node astar -- cost )
    [ astar>> cost ] [ nip astar>> g>> at ] 3bi + ;

: ?set-g ( origin node astar -- )
    [ cost-through ] 3keep [ swap ] 2dip
    3dup astar>> g>> at [ 1/0. ] unless* >= [ 4drop ] [ set-g ] if ;

: build-path ( target astar -- path )
    [ over ] [ over [ [ origin>> at ] keep ] dip ] produce 2nip reverse ;

: handle ( node astar -- )
    dupd [ astar>> neighbours ] keep [ ?set-g ] curry with each ;

: (find-path) ( astar -- path/f )
    dup open-set>> heap-empty? [
        drop f
    ] [
        [ get-first ] keep 2dup goal>> = [ build-path ] [ [ handle ] [ (find-path) ] bi ] if
    ] if ;

: (init) ( from to astar -- )
    swap >>goal
    H{ } clone over astar>> g<<
    { } <hash-set> over astar>> in-closed-set<<
    H{ } clone >>origin
    H{ } clone >>in-open-set
    <min-heap> >>open-set
    [ 0 ] 2dip [ (add-to-open-set) ] [ astar>> g>> set-at ] 3bi ;
{'fraction_non_alphanumeric': 0.17999096657633243, 'fraction_numerical': 0.01084010840108401, 'mean_word_length': 2.380916030534351, 'pattern_counts': {'":': 0, '<': 19, '<?xml version=': 0, '>': 147, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '235045', 'n_tokens_mistral': 1726, 'n_tokens_neox': 1657, 'n_words': 475}