(load "graphblocksUMB.scm")

(define initial '( (A B C) () () ) )
(define goal '( () () (A B C) ) )
(define alex_goal '( (C B A) () () ))
(define dave_goal '( () () (C B A)))

(define flatten4init '((a b c d) () () ) )
(define flatten4goal '((a) (b) (d c)))

(define flatten5init '((a b c d e) () ()) )
(define flatten5goal '((a) (d b) (e c)) )

(define flatten6init '((a b c d e f) () ()) )
(define flatten6goal '((d a) (e b) (f c)) )


(define (at-left-edge? colnum state) (equal? colnum 1))

(define (at-right-edge? colnum state) (equal? colnum (totalnumcols state)))

(define (element index L) 
        (if (equal? index 1) (car L) 
                             (element (- index 1) (cdr L) )
        )
)

(define (empty? L) (null? L))

(define (member? element list)
        (if (equal? (length list) 0)
            #f
            (or (equal? element (car list))
                (member? element (cdr list))
            )
        )
  )


(define (block-at? col state)  (not (empty? (element col state))))

(define (totalnumcols state) (length state))

(define (get-column colnum state) 
        (element colnum state)
)

(define (legal-move? direction colnum state)
        (if (empty? (get-column colnum state))
            #f
            (if (equal? direction 'left)
                (not (at-left-edge? colnum state))
                (not (at-right-edge? colnum state))
            )
        )
)


(define (replaceCOL column colnum state)
        (insertCOL column colnum (deleteCOL colnum state))
)

(define (insertCOL column colnum state)
        (if (equal? colnum 1) 
            (cons column state)
            (cons (car state)
                  (insertCOL column (- colnum 1) (cdr state))
            )
        )
)

(define (deleteCOL colnum state)
        (if (equal? colnum 1)
            (cdr state)
            (cons (car state) 
                  (deleteCOL (- colnum 1) (cdr state))
            ) 
        )
)


(define (left colnum state)
        (replaceCOL (cdr (get-column colnum state))
                    colnum
                    (replaceCOL (cons (car (get-column colnum state))
                                      (get-column (- colnum 1) state))
                                (- colnum 1)
                                state
                    )                    
        )
)


(define (right colnum state)
        (replaceCOL (cdr (get-column colnum state))
                    colnum
                    (replaceCOL (cons (car (get-column colnum state))
                                      (get-column (+ colnum 1) state))
                                (+ colnum 1)
                                state
                    )                    
        )
)
        
;  The main function to call.  Given an initial and a goal state, solve
;    returns a plan to change the initial to the goal, if possible.  It
;    calls search to do a breadth-first search of all possible successor
;    states (offspring) of the initial state.

(define (solve initial goal)
        (search (list (list 'quote initial)) goal))

;  Success returns true if the EVALUATED (i.e., executed) plan returns the goal
;  state.

(define (success? plan goal)
        (equal? (eval plan) goal))

;  Search is given a queue of plans (i.e., a list of plans) and the goal state.
;    Search then checks if the head of the queue (first element) is a successful
;    plan.  If so, it returns that plan.  Otherwise, it generates all legal and
;    safe offspring of that plan (offspring are successor states, which are
;    moves from the given state).  These offspring are themselves plans, one move
;    longer than their parent plan.  These are then added (appended) to the tail
;    of the plan queue (i.e., the end of the list of plans).  This implements
;    a breadth-first search of the state space tree, with NO checking for duplicates
;    (i.e., cycles) or other possible speedups (e.g., heuristics).

(define (search plans-q goal)
        (if (success? (car plans-q) goal)       ;  If first plan works, return it.  Done.
            (car plans-q)
            (search (append (cdr plans-q)       ;  Otherwise, continue search, recursively.
                            (offspring (car plans-q) (totalnumcols goal))
                    ) goal
            )
        )
)



;  Return a list of all successor plans of the given plan, but only those
;  successors that involve one additional LEGAL move!

(define (offspring plan numcols)
        (append (all-leftmove-successors plan numcols)
                (all-rightmove-successors plan numcols) 
        )
)

;  Return a list of all successor plans of the given plan, but only those
;  successors that involve one additional LEGAL LEFT move!

(define (all-leftmove-successors plan colnum)
        (if (equal? colnum 0) '() 
            (append (add-left-move plan colnum) 
                    (all-leftmove-successors plan (- colnum 1))
            )
        )
)

(define (add-left-move plan colnum)
        (if (legal-move? 'left colnum (eval plan))
            (list (list 'left colnum plan))
            '()
        )
)

;  Return a list of all successor plans of the given plan, but only those
;  successors that involve one additional LEGAL RIGHT move!

(define (all-rightmove-successors plan colnum)
        (if (equal? colnum 0) '() 
            (append (add-right-move plan colnum) 
                    (all-rightmove-successors plan (- colnum 1))
            )
        )
)

(define (add-right-move plan colnum)
        (if (legal-move? 'right colnum (eval plan))
            (list (list 'right colnum plan))
            '()
        )
)