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

import random
import math

color_strings = [ 'blue', 'white', 'green', 'red' ]

class cube(Belief): pass
class follows(Belief): pass
class find_first_color(Procedure): pass
class find_prev(Procedure): pass
class find_nearest_cube(Procedure): pass
class find_nearest(Procedure): pass

# populate the KB
for i in range(0,20):
    x = int(random.uniform(0,100))
    y = int(random.uniform(0,100))
    color_val = int(random.uniform(0, 120)) % 4
    PHIDIAS.assert_belief(cube(x, y, color_strings[color_val]))

# green, red, white, blue
PHIDIAS.assert_belief(follows('red', 'white'))
PHIDIAS.assert_belief(follows('green', 'red'))
PHIDIAS.assert_belief(follows('white', 'blue'))

class VerifyNew(ActiveBelief):
   def evaluate(self, robot_x, robot_y, cand_x, cand_y, new_x, new_y):
      distance_from_candidate = \
		math.hypot(robot_x() - cand_x(), robot_y() - cand_y())
      distance_from_new = \
		math.hypot(robot_x() - new_x(), robot_y() - new_y())
      return distance_from_new < distance_from_candidate


def_vars('X', 'Y', 'Prev', 'Color', 'RobotX', 'RobotY', 'CandX', 'CandY', 'NextColor')

find_first_color() / follows(X, Y) >> [ find_prev(X) ]
find_prev(X) / follows(Prev, X) >> [ find_prev(Prev) ]
find_prev(X) >> [ show_line("il primo colore e' ", X), 
			find_nearest_cube(X, 0, 0) ]

find_nearest_cube(Color, RobotX, RobotY) / cube(X, Y, Color) >> \
  [ find_nearest(Color, RobotX, RobotY, X, Y) ]
find_nearest_cube(Color, RobotX, RobotY) / follows(Color, NextColor)>>\
  [ find_nearest_cube(NextColor, RobotX, RobotY) ]
find_nearest_cube(Color, RobotX, RobotY) >> [ show_line('ho finito') ]

find_nearest(Color, RobotX, RobotY, CandX, CandY) /  \
  (cube(X, Y, Color) & VerifyNew(RobotX, RobotY, CandX, CandY, X, Y) )\
>> [ find_nearest(Color, RobotX, RobotY, X, Y) ]

find_nearest(Color, RobotX, RobotY, CandX, CandY) >> \
  [ show_line('Catturo cubo ', Color, ' in posizione ', CandX, ' ', CandY), -cube(CandX, CandY, Color), 
	find_nearest_cube(Color, CandX, CandY) ]


# instantiate the engine
PHIDIAS.run()
# run the engine shell
PHIDIAS.shell(globals())