#!r6rs

(import (rnrs (6)) ; Imports R6RS base and standard libraries.
        (only (srfi :43 vectors) vector-copy vector-swap!)
        (prefix (ai problem) problem:)
        (prefix (ai successor) successor:)
        (prefix (ai search) search:)
        (prefix (ai frontier) frontier:)
        ;; Contains some helpers to print a node, a list of nodes, the path
        ;; to from initial-state to the state of the given node, etc.
        (ai printer)) 



;;; Puzzle ADT ------------------------------------------------------------

;; Return a new puzzle which contains the elements provided in the list
;; CONTENTS.
(define (make-puzzle contents)
  ;; TODO: Make a vector and fill it with CONTENTS.
  (list->vector contents)
  )

;; Return #t if both puzzles are equal, and #f otherwise.
(define (puzzle=? puzzle1 puzzle2)
  ;; You can compare 2 vectors containing symbols with equal?.
  (equal? puzzle1 puzzle2)) 

;; Return a new puzzle equal to PUZZLE but with the contents at LOC1 and
;; LOC2 swapped. LOC1 and LOC2 are zero-based indices.
(define (puzzle-swap puzzle left right)
  
  (define p (vector (vector-ref puzzle 0) (vector-ref puzzle 1) (vector-ref puzzle 2) (vector-ref puzzle 3) (vector-ref puzzle 4)))
  (define temp (vector-ref p left))
  (vector-set! p left (vector-ref p right))
  (vector-set! p right temp)
  p
  
  )

;; Return the location of the first occurrence of PATTERN in PUZZLE, and #f
;; otherwise.
(define (puzzle-find-pattern puzzle pattern)
  ;; TODO: implement.
  (let ((puzzle-lengte (vector-length puzzle))
        (pattern-lengte (vector-length pattern)))
    (let loop ((puzzle-place 0)
               (pattern-place 0))
      (cond
        ((> (+ puzzle-place pattern-lengte) puzzle-lengte) #f)
        ((> pattern-place (- pattern-lengte 1)) puzzle-place)
        ((eq? (vector-ref puzzle (+ puzzle-place pattern-place)) (vector-ref pattern  pattern-place)) (loop puzzle-place (+ pattern-place 1)))
        (else (loop (+ puzzle-place 1) 0))
        
        )
      )
    )
  
  
  )

;; Some test functions demonstrating how the ADT is used. Throws an error
;; if any test failed.
(define (puzzle-test)
  (let ((p (make-puzzle '(o o - x x))))
    (assert (puzzle=? (make-puzzle '(o o x - x))
                      (puzzle-swap p 2 3)))
    (assert (= 2 (puzzle-find-pattern p '#(- x))))
    (assert (not (puzzle-find-pattern p '#(- o x))))
    (assert (not (puzzle-find-pattern p '#(o x -))))))



;;; The puzzle as a search problem ----------------------------------------

(define (initial)
  (make-puzzle '(o o - x x)))

(define (step-cost old-state action new-state)
  1)

(define GOAL-STATE (make-puzzle '(x x - o o)))

(define (is-goal? state)
  (puzzle=? state GOAL-STATE))

(define (successors state)
  (let ((result '()))
    (for-each (lambda (pattern delta)
                ;; TODO: Add a new successor to the list RESULT if PATTERN
                ;; is found in STATE. The new state has the elements on
                ;; position i an j swapped where i is the position where
                ;; PATTERN is found in STATE and j is i + DELTA.
                (let ((i (puzzle-find-pattern state pattern)))
                (if i (set! result (cons (successor:new (puzzle-swap state i (+ i delta))
                                                      pattern) result))))
                )
              (list '#(o -) '#(- x) '#(o x -) '#(- o x))
              ;; If pattern is found at position i, we need to swap element
              ;; at position i with the element at position i + delta. The
              ;; deltas are given in the following list.
              (list 1 1 2 2))
    result))

(define puzzle (problem:new initial step-cost is-goal? successors))

(define (on-iteration nof-nodes-handled current frontier children closed-list-size)
  (display nof-nodes-handled) (newline)
  (display "current: ") (print-node current) (newline)
  (display "frontier: ") (print-nodes (frontier:nodes frontier)) (newline)
  (display "children: ") (print-nodes children)
  (newline))

;; Run depth-first-tree-search, and print the path to the initial-state to
;; goal-state.
(print-path-to (search:depth-first-tree-search puzzle on-iteration))