# --- GEOMETRY --------------------------------------------------------------

from math import degrees, atan2
from math import sqrt, pow
from math import radians, sin, cos

def angle(x0, y0, x1, y1):
    """ Returns the angle between two points.
    """
    return degrees( atan2(y1-y0, x1-x0) )

def distance(x0, y0, x1, y1):
    """ Returns the distance between two points.
    """
    return sqrt(pow(x1-x0, 2) + pow(y1-y0, 2))

def coordinates(x0, y0, distance, angle):
    """ Returns the coordinates of given distance and angle from a point.
    """
    return (x0 + cos(radians(angle)) * distance,
            y0 + sin(radians(angle)) * distance)

# --- WORLD -----------------------------------------------------------------

class World:
    def __init__(self):
        self.obstacles = []

# --- OBSTACLE --------------------------------------------------------------

class Obstacle:
    def __init__(self, x, y, radius):
        self.x = x
        self.y = y
        self.radius = radius

# --- CREATURE --------------------------------------------------------------

class Creature:

    def __init__(self, world, x, y, speed=1.0, size=4):

        self.x = x
        self.y = y
        self.speed = speed
        self.size = size

        self.world = world
        self.feeler_length = 25

        self._vx = 0
        self._vy = 0

    def heading(self):

        """ Returns the creature's heading as angle in degrees.
        """

        return angle(self.x, self.y, self.x+self._vx, self.y+self._vy)

    def avoid_obstacles(self, m=0.4, collision=4):

        # Find out where the creature is going.
        a = self.heading()

        for obstacle in self.world.obstacles:

            # Calculate the distance between the creature and the obstacle.
            d = distance(self.x, self.y, obstacle.x, obstacle.y)

            # Respond faster if the creature is very close to an obstacle.
            if d - obstacle.radius < 4: m *= 10

            # Check if the tip of the feeler falls inside the obstacle.
            # This is never true if the feeler length
            # is smaller than the distance to the obstable.
            if d - obstacle.radius <= self.feeler_length:
                tip_x, tip_y = coordinates(self.x, self.y, d, a)
                if distance(obstacle.x, obstacle.y,
                            tip_x, tip_y) < obstacle.radius:

                    # Nudge the creature away from the obstacle.
                    m *= self.speed

                    if tip_x < obstacle.x: self._vx -= random(m)
                    if tip_y < obstacle.y: self._vy -= random(m)
                    if tip_x > obstacle.x: self._vx += random(m)
                    if tip_y > obstacle.y: self._vy += random(m)

                    if d - obstacle.radius < 4: return

    def roam(self):

        """ Creature changes heading aimlessly.
        With its feeler it will scan for obstacles and steer away.
        """

        self.avoid_obstacles()

        v = self.speed
        self._vx += random(-v, v)
        self._vy += random(-v, v)
        self._vx = max(-v, min(self._vx, v))
        self._vy = max(-v, min(self._vy, v))

        self.x += self._vx
        self.y += self._vy

# ---------------------------------------------------------------------------

size(550, 200)

# Create a world with obstacles at random positions.
world = World()
for i in range(15):
    obstacle = Obstacle(random(WIDTH), random(HEIGHT), random(10, 30))
    world.obstacles.append(obstacle)

# Create a number of ants.
ants = []
for i in range(4):
    ant = Creature(world, 225, 100, speed=2.0)
    ants.append(ant)

speed(30)
def draw():

    background(0.2)

    # Draw all the obstacles in the world.
    fill(0.5, 0.5)
    for obstacle in world.obstacles:
        oval(obstacle.x - obstacle.radius,
             obstacle.y - obstacle.radius,
             obstacle.radius * 2,
             obstacle.radius * 2)

    # Draw each ant and its feeler.
    stroke(1)
    fill(1, 0.5)
    for ant in ants:
        push()
        transform(CORNER)
        translate(ant.x, ant.y)
        rotate(-ant.heading())
        line(0, 0, ant.feeler_length, 0)
        pop()
        oval(ant.x-ant.size*0.5, ant.y-ant.size*0.5, ant.size, ant.size)

    # Move all the ants around.
    for ant in ants:
        ant.roam()