(load "Matrix.scm")
(load "writeln.scm")

(define LightCycle_Dumb_Robot
  (lambda ()
    (define cycle_max 12) ;0 through 11
    (define grid (matrix 170 170))
    (define lastDirVector (vector 0 0 0))
    (define livingCycles -1)
    (define livingTeams 4)
    (define livingBits (- (expt 2 cycle_max) 1))
    (define grid_max 169)
    (define x 0)
    (define y 0)
    (define p_old 0)
    (define idx 0)
    (define dir_right 1)
    (define dir_down 2)
    (define dir_left 4)
    (define dir_up 8)
    (define dir_start_code 16) 
    
    (define mask_right (bitwise-xor 15 dir_right))
    (define mask_down (bitwise-xor 15 dir_down))
    (define mask_left (bitwise-xor 15 dir_left))
    (define mask_up (bitwise-xor 15 dir_up))
    (define dirBits 15)
    (define dir 0)
    (define firstCall #t)
    
    
   
    
   (define cycleVectorOld 
      (vector 
        (vector 169  45) (vector 169  85) (vector 169 125)
        (vector  45 169) (vector  85 169) (vector 125 169)
        (vector   0  45) (vector   0  85) (vector   0 125)
        (vector  45   0) (vector  85   0) (vector 125   0)
      )
    )
    

    (define moveToVector 
      (vector 
         (vector 0 0) 
         (vector 0 0)
         (vector 0 0)
       )
    )

    (define DumbRobot
      (lambda (teamNum cycleVector)
        (define startIdx (* 3 teamNum))
        (updateCycleLocations cycleVector)
        (do ((i 0 (+ i 1))) ((> i 2))
        (set! idx (+ startIdx i))
        (set! p_old (vector-ref cycleVector idx))
        (set! dirBits 15)
        (set! x (vector-ref p_old 0))
        (set! y (vector-ref p_old 1))
        (if (and (>= x 0) (>= y 0)) 
          (begin
            (if (or (= x grid_max) 
                (> (matrix-ref grid (+ x 1) y) 0)
              )
              (set! dirBits (bitwise-and dirBits mask_right))
            )
      
           (if (or (= x 0)   
                (> (matrix-ref grid (- x 1) y) 0)
              )
              (set! dirBits (bitwise-and dirBits mask_left))
            )
      
            (if (or (= y grid_max) 
                (> (matrix-ref grid x (+ y 1)) 0)
              )
              (set! dirBits (bitwise-and dirBits mask_down))
            )
      
            (if (or (= y 0)
                (> (matrix-ref grid x (- y 1)) 0)
              )
              (set! dirBits (bitwise-and dirBits mask_up))
            )
      
            (if (> (bitwise-and (vector-ref lastDirVector i) dirBits) 0) 
              (set! dir (vector-ref lastDirVector i))  
              (set! dir (getRandomDir dirBits))
            )
            ;(writeln "DumbRobot:: <" x ", " y ">  dirBits=" dirBits ", dir=" dir)
      
      
      
            (cond 
              ((= dir dir_right) (set! x (+ x 1)))
              ((= dir dir_left) (set! x (- x 1)))
              ((= dir dir_down) (set! y (+ y 1)))
              (else  (set! y (- y 1)))  
            )
            (point-set! moveToVector i x y)
            (vector-set! lastDirVector i dir)
          )
          (vector-set! moveToVector i (vector x y)) ;else dead
        )
      )

      (copyCycleVectorByValue cycleVectorOld cycleVector)
      ;(do ((i 0 (+ i 1))) ((>= i cycle_max))
      ; (vector-set! cycleVectorOld i (vector-ref cycleVector i))
      ;)
      
      (set! firstCall #f)
      moveToVector
    )
  )
    
 (define point-set! 
    (lambda (cycleVectorNew cycleIdx x y)
      (define point (vector-ref cycleVectorNew cycleIdx)) 
      (vector-set! point 0 x)
      (vector-set! point 1 y)
    )
  )

        
    (define copyCycleVectorByValue 
      (lambda (sourceVector targetVector)
        (define x 0)
        (define y 0)
        (do ((i 0 (+ i 1))) ((>= i cycle_max))
           (set! x (vector-ref (vector-ref sourceVector i) 0))
           (set! y (vector-ref (vector-ref sourceVector i) 1)) 
           (vector-set! (vector-ref targetVector i) 0 x)
           (vector-set! (vector-ref targetVector i) 1 y)
           ;(writeln "copy x y <" x ", " y ">")
        )
      )        
    )
    (define countBits 
      (lambda (n)
       (define count 0)
       (if (> (bitwise-and n 1) 0) (set! count (+ count 1)))
       (if (> (bitwise-and n 2) 0) (set! count (+ count 1)))
       (if (> (bitwise-and n 4) 0) (set! count (+ count 1)))
       (if (> (bitwise-and n 8) 0) (set! count (+ count 1)))
       count
    ))
      
    (define getNthBit 
      (lambda (bits n)
       (define dir 0)
        
       (if (> (bitwise-and n 1) 0) (set! count (+ count 1)))
       (if (> (bitwise-and n 2) 0) (set! count (+ count 1)))
       (if (> (bitwise-and n 4) 0) (set! count (+ count 1)))
       (if (> (bitwise-and n 8) 0) (set! count (+ count 1)))
    ))
    
    
  (define terminateCycle 
    (lambda (idx)
      (define point 0)
      (define x1 0)
      (define y1 0)
      (define x2 0)
      (define y2 0)
      (define dir 0)
     
      (if (> (bitwise-and livingBits (expt 2 idx)) 0)
        (begin
          (set! livingBits (- livingBits (expt 2 idx)))
          (set! point (vector-ref cycleVectorOld idx))
          (set! x1 (vector-ref point 0))
          (set! y1 (vector-ref point 1))
          (set! x2 x1)
          (set! y2 y1)
          ;(writeln "Dumb_Robot: idx=" idx " x1=" x1 ", y1=" y1 ", x2=" x2 ", y2=" y2)
          (do ((i 0 (+ i 1))) ((= dir dir_start_code))

             (set! dir (matrix-ref grid x1 y1))
             
             (matrix-set! grid x1 y1 0)
             (cond
                ;To follow the trail backward, reverse the directions
                ((= dir dir_right) (set! x2 (- x1 1))) 
                ((= dir dir_left)  (set! x2 (+ x1 1)))
                ((= dir dir_up)    (set! y2 (+ y1 1)))
                ((= dir dir_down)  (set! y2 (- y1 1)))
                ;else dir must equal dir_start_code
                ;and we are done.
             )
             ;(writeln "Dumb Robot: x1=" x1 ", y1=" y1 ", x2=" x2 ", y2=" y2 ", dir=" dir)
             (set! x1 x2)
             (set! y1 y2)
          )
        )
      )
    )
  )

  (define updateCycleLocations 
    (lambda (cycleVector)
      
      (define point1 (vector 0 0))
      (define point2 (vector 0 0))
      (define x1 0)
      (define y1 0)
      (define x2 0)
      (define y2 0)
      (define dir 0)
      (do ((i 0 (+ i 1))) ((>= i cycle_max))
        (cond 
          ((= (bitwise-and livingBits (expt 2 i)) 0)
            (vector-set! cycleVector i (vector -1 -1))
          )
          (else
            (set! point1 (vector-ref cycleVectorOld i))
            (set! point2 (vector-ref cycleVector i))
            (set! x1 (vector-ref point1 0))
            (set! y1 (vector-ref point1 1))
            (set! x2 (vector-ref point2 0))
            (set! y2 (vector-ref point2 1))
            (cond 
              ((or (< x2 0) (< y2 0))
                  (terminateCycle i)
              )
              (firstCall (matrix-set! grid x2 y2 dir_start_code))
              (else 
                (cond
                  ((> x2 x1) (set! dir dir_right))
                  ((< x2 x1) (set! dir dir_left))
                  ((> y2 y1) (set! dir dir_down))
                  ((< y2 y1) (set! dir dir_up))
                )
                ;(writeln "x1=" x1 ", y1=" y1 ", x2=" x2 ", y2=" y2 ", dir=" dir)
                (matrix-set! grid x2 y2 dir)
              )
            )
          )
        )
      )
    )
  )                     
    
    (define getRandomDir 
      (lambda (bits)
        (define dir 0)
        (define okDirCount (countBits bits))
        (define r (if (= okDirCount 0) 0 (random okDirCount)))
        (cond 
          ((and (= r 0) (= (bitwise-and bits 1) 1)) (set! dir dir_right))
          ((and (= r 0) (= (bitwise-and bits 2) 2)) (set! dir dir_down))
          ((and (= r 0) (= (bitwise-and bits 4) 4)) (set! dir dir_left))
          ((= r 0) (set! dir dir_up))
          ((and (= r 1) (= (bitwise-and bits 3) 3)) (set! dir dir_down))
          ((and (= r 1) (= (bitwise-and bits 5) 5)) (set! dir dir_left))
          ((and (= r 1) (= (bitwise-and bits 6) 6)) (set! dir dir_left))
          ((= r 1) (set! dir dir_up))
          ((and (= r 2) (= (bitwise-and bits 7) 7)) (set! dir dir_left))
          (else (set! dir dir_up))
        )
        ;(writeln "DumbRobot::getRandomDir:: " cycleNum cycleVector ", dir=" dir ", bits=" bits)
        dir
      )
    )
    
    DumbRobot
  )
)