#
#
#

import sys
import math

sys.path.insert(0, "../lib")

from phidias.Types import *
from phidias.Main import *
from phidias.Lib import *

class mark(Belief): pass
class candidate(SingletonBelief): pass
class size(SingletonBelief): pass
class generate(Procedure): pass
class obstacle(Procedure): pass
class nf1(Procedure): pass
class do_mark(Procedure): pass
class plan(Procedure): pass
class path(Procedure): pass
class generate_candidate(Procedure): pass
class select_candidate(Procedure): pass

class show_map(Procedure): pass

class test(Procedure): pass

class compute_distance(Action):
    def execute(self, xs, ys, x, y, d):
        distance = math.hypot(xs() - x(), ys() - y())
        d(distance)

class distance_less(ActiveBelief):
    def evaluate(self, xs, ys, x, y, d):
        distance = math.hypot(xs() - x(), ys() - y())
        #print(xs(), ys(), x(), y(), distance, d())
        return distance < d()

def_vars('N', 'X', 'Y', 'D', 'XP', 'XM', 'YP', 'YM', 'XS', 'YS', 'XE', 'YE', 'XC', 'YC', 'DC', 'DS')

generate() / size(N) >> [ generate(N, N) ]

generate(0, Y) >> []
generate(X, 0) / size(N) >> [ "X=X-1", generate(X, N) ]
generate(X, Y) / mark(X, Y, D) >> [ -mark(X, Y, D), +mark(X,Y,0), "Y=Y-1", generate(X,Y) ]
generate(X, Y) >> [ +mark(X,Y,0), "Y=Y-1", generate(X,Y) ]

obstacle(XS, YS, XE, YE)  >> [ obstacle(XS, YS, XS, YS, XE, YE) ]
obstacle(X, Y, XS, YS, XE, YE) / gt(Y, YE) >> [ ]
obstacle(X, Y, XS, YS, XE, YE) / gt(X, XE) >> [ "Y = Y + 1", obstacle(XS, Y, XS, YS, XE, YE) ]
obstacle(X, Y, XS, YS, XE, YE) / mark(X, Y, D) >> \
  [
      -mark(X, Y, D),
      "X = X + 1",
      obstacle(X, Y, XS, YS, XE, YE)
  ]


path(XS, YS, XE, YE) / mark(XE, YE, 0) >> \
  [
    -mark(XE, YE, 0), +mark(XE, YE, -1),
    nf1(XE, YE),
    show_map(),
    plan(XS, YS, XE, YE)
  ]


nf1(X,Y) / (mark(X, Y, D) & lt(D, 0))>> \
  [
      -mark(X, Y, D),
      "D = -D",
      +mark(X, Y, D),
      "D = D + 1",
      "XP = X + 1", "XM = X - 1",
      "YP = Y + 1", "YM = Y - 1",
      do_mark(XP, Y, D),
      do_mark(XM, Y, D),
      do_mark(X, YP, D),
      do_mark(X, YM, D),
      nf1(XP, Y),
      nf1(XM, Y),
      nf1(X, YP),
      nf1(X, YM)
  ]

do_mark(X, Y, D) / mark(X, Y, 0) >> [ -mark(X, Y, 0), "D = -D", +mark(X, Y, D) ]
do_mark(X, Y, D) / (mark(X, Y, DS) & lt(D, DS)) >> [ -mark(X, Y, DS), "D = -D", +mark(X, Y, D) ]

plan(XE, YE, XE, YE) / mark(XE, YE, D) >> [ show_line("Node ", XE, ",", YE), -mark(XE, YE, D), +mark(XE, YE, -1) ]
plan(XS, YS, XE, YE) / mark(XE, YE, D) >> \
  [
      show_line("Node ", XE, ",", YE),
      "XP = XE + 1", "XM = XE - 1",
      "YP = YE + 1", "YM = YE - 1",
      generate_candidate(XS, YS, XP, YE),
      generate_candidate(XS, YS, XM, YE),
      generate_candidate(XS, YS, XE, YP),
      generate_candidate(XS, YS, XE, YM),
      generate_candidate(XS, YS, XP, YP),
      generate_candidate(XS, YS, XP, YM),
      generate_candidate(XS, YS, XM, YP),
      generate_candidate(XS, YS, XM, YM),
      select_candidate(XS, YS, XE, YE)
  ]

generate_candidate(XS, YS, X, Y) / (candidate(XC, YC, DC, DS) & mark(X, Y, D) & gt(D, DC)) >>  \
  [
      compute_distance(XS, YS, X, Y, DS),
      +candidate(X, Y, D, DS)
  ]
generate_candidate(XS, YS, X, Y) / (candidate(XC, YC, DC, DS) & mark(X, Y, D) & eq(D, DC) & distance_less(XS, YS, X, Y, DS)) >> \
  [
      compute_distance(XS, YS, X, Y, DS),
      +candidate(X, Y, D, DS)
  ]
generate_candidate(XS, YS, X, Y) / candidate(XC, YC, DC, DS)  >>  \
  [
  ]
generate_candidate(XS, YS, X, Y) / (mark(X, Y, D) & gt(D, 0)) >> \
   [
      "DS = 0",
      compute_distance(XS, YS, X, Y, DS),
      +candidate(X, Y, D, DS)
   ]

select_candidate(XS, YS, XE, YE) / (candidate(XC, YC, DC, DS) & mark(XE, YE, D))  >> \
  [
      -candidate(XC, YC, DC, DS),
      -mark(XE, YE, D),
      +mark(XE, YE, -1), # mark as path node
      plan(XS, YS, XC, YC)
  ]


show_map() / size(N) >> [ show_line(""), show_map(1, 1, N, N) ]

show_map(X, Y, XE, YE) / gt(Y,YE) >> [ show_line("") ]
show_map(X, Y, XE, YE) / gt(X, XE) >> [ "Y=Y+1", show_line(""), show_map(1, Y, XE, YE) ]
show_map(X, Y, XE, YE) / mark(X, Y, -1) >> [ show(" ** |"), "X=X+1", show_map(X, Y, XE, YE) ]
show_map(X, Y, XE, YE) / mark(X, Y, D) >> [ show_fmt("%3d |", D), "X=X+1", show_map(X, Y, XE, YE) ]
show_map(X, Y, XE, YE) >> [ show("XXXX|"), "X=X+1", show_map(X,Y,XE,YE) ]

test() >> \
  [ +size(10),
    generate(),
    obstacle(2,2,3,3),
    obstacle(6,6,8,10),
    path(1,1,10,10),
    show_map() ]

PHIDIAS.run()
PHIDIAS.shell(globals())