import turtle
import math

def Positive(angle):
  # make every angle positive in range 0 to 360
  if angle<0:
    return 360+angle
  if angle>360:
    return angle % 360
  return angle

class Position:
  def __init__(self, x, y):
    self.x = x
    self.y = y
    
class SensorCone:
  # Proximity sensor rendering. Distance is from center of robot.
  # fov: field of view, centered on heading of robot
  def __init__(self, entity, distance, fov ):
    # param entity: robot entity to which it belongs
    self.entity = entity
    self.distance = distance
    self.fov = fov
    self.pen = turtle.Turtle()
    self.pen.hideturtle()
    self.pen.speed(0)

  def Draw(self):
    # Draw perimeter around entire robot, but then show limited fov
    self.pen.pu()
    self.pen.goto(self.entity.pos.x, self.entity.pos.y)
    self.pen.setheading(self.entity.heading_deg)
    self.pen.pd()
    self.pen.dot(self.distance*2,'yellow')
    # draw the cone
    self.pen.left(self.fov//2)
    for i in [0,1,1]:
      self.pen.right((self.fov//2)*i)
      self.pen.pd()
      self.pen.forward(self.distance)
      self.pen.pu()
      self.pen.goto(self.entity.pos.x, self.entity.pos.y)
  
class Robot:
  def __init__( self, radius, heading, pos, color ):
    self.radius = radius
    self.pos = pos
    self.heading_deg = Positive(heading)
    self.color = color
    self.pen = turtle.Turtle()
    self.pen.hideturtle()
    self.pen.speed(0)
    # default value of 3*radius for proximity sensor
    self.sensor = SensorCone( self, radius*3, 30)

  def Draw(self):
    self.pen.pu()
    self.pen.goto(self.pos.x, self.pos.y)
    self.pen.pd()
    self.pen.dot(self.radius*2,self.color)
    self.pen.pu()

  def DrawSensor(self):
    self.sensor.Draw()

  def DrawTriangle(self, width_angle, dir_angle, dist):
    # this is the triangle formed by this robot and the one being sensed
    self.pen.pu()
    self.pen.goto(self.pos.x, self.pos.y)
    self.pen.setheading(dir_angle)
    self.pen.right(width_angle)
    self.pen.pd()
    self.pen.forward(dist)
    self.pen.pu()
    self.pen.goto(self.pos.x, self.pos.y)
    self.pen.setheading(dir_angle)
    self.pen.left(width_angle)
    self.pen.pd()
    self.pen.forward(dist)

def InRange( range1, range2 ):
  print(range1)
  print(range2)
  
  # if crossing zero, adjust all angles
  if range1[0] > range1[1]:
    distToCrossing = 360-range1[0]
    range1 = [ (range1[0]+distToCrossing)%360, range1[1]+distToCrossing]
    range2 = [ (range2[0]+distToCrossing)%360, (range2[1]+distToCrossing)%360]
  if range2[0] > range2[1]:
    distToCrossing = 360-range2[0]
    range1 = [ (range1[0]+distToCrossing)%360, range1[1]+distToCrossing]
    range2 = [ (range2[0]+distToCrossing)%360, (range2[1]+distToCrossing)%360]

  # if 2nd robot spans the proximiy sensor, then in range
  if range2[0] < range1[0] and range2[1] > range1[1]:
    return True
  if range2[0] >= range1[0] and range2[0] <= range1[1]:
    return True
  if range2[1] >= range1[0] and range2[1] <= range1[1]:
    return True
  return False

def SensorReading( r_sensing, r_sensed):
  deltaX = r_sensed.pos.x-r_sensing.pos.x
  deltaY = r_sensed.pos.y-r_sensing.pos.y
  dist = (deltaX**2 + deltaY**2)**0.5
  if dist > r_sensing.sensor.distance + r_sensed.radius:
    return False
  # triangle is formed from center of r_sensing to the point along the tangents
  # to r_sensed. The height is dist and the width is r_sensed.radius*2.
  triangle_theta = Positive(math.atan(r_sensed.radius/dist)*180/3.14)
  distance_theta = Positive(math.atan2(deltaY, deltaX)*180/3.14)
  r_sensing.DrawTriangle(triangle_theta, distance_theta, dist)
  sensor_lower = r_sensing.heading_deg - r_sensing.sensor.fov//2
  sensor_upper = r_sensing.heading_deg + r_sensing.sensor.fov//2
  sensed_lower = distance_theta - triangle_theta
  sensed_upper = distance_theta + triangle_theta
  if InRange( [ Positive(sensor_lower), Positive(sensor_upper) ], \
              [ Positive(sensed_lower), Positive(sensed_upper) ] ):
    print('True: ', dist)
  else:
    print('False')

#Robot(radius, heading, pos, color)
R = Robot( 50, 0, Position(0,0), 'green')
R2 = Robot( 20, 270, Position(150,-25),'red')
R.DrawSensor()
R.Draw()
R2.Draw()
SensorReading( R, R2)