"""follow_modes.py -- Follow mode geometry for saltybot social navigation."""

import math
from enum import Enum
import numpy as np


class FollowMode(Enum):
    SHADOW = 'shadow'    # stay directly behind at follow_distance
    LEAD   = 'lead'      # move ahead of person by lead_distance
    SIDE   = 'side'      # stay to the right (or left) at side_offset
    ORBIT  = 'orbit'     # circle around person at orbit_radius
    LOOSE  = 'loose'     # general follow, larger tolerance
    TIGHT  = 'tight'     # close follow, small tolerance


def compute_shadow_target(person_pos, person_bearing_deg, follow_dist=1.2):
    """Target position: behind person along their movement direction."""
    bearing_rad = math.radians(person_bearing_deg + 180.0)
    tx = person_pos[0] + follow_dist * math.sin(bearing_rad)
    ty = person_pos[1] + follow_dist * math.cos(bearing_rad)
    return (tx, ty, person_pos[2])


def compute_lead_target(person_pos, person_bearing_deg, lead_dist=2.0):
    """Target position: ahead of person."""
    bearing_rad = math.radians(person_bearing_deg)
    tx = person_pos[0] + lead_dist * math.sin(bearing_rad)
    ty = person_pos[1] + lead_dist * math.cos(bearing_rad)
    return (tx, ty, person_pos[2])


def compute_side_target(person_pos, person_bearing_deg, side_dist=1.0, right=True):
    """Target position: to the right (or left) of person."""
    sign = 1.0 if right else -1.0
    bearing_rad = math.radians(person_bearing_deg + sign * 90.0)
    tx = person_pos[0] + side_dist * math.sin(bearing_rad)
    ty = person_pos[1] + side_dist * math.cos(bearing_rad)
    return (tx, ty, person_pos[2])


def compute_orbit_target(person_pos, orbit_angle_deg, orbit_radius=1.5):
    """Target on circle of radius orbit_radius around person."""
    angle_rad = math.radians(orbit_angle_deg)
    tx = person_pos[0] + orbit_radius * math.sin(angle_rad)
    ty = person_pos[1] + orbit_radius * math.cos(angle_rad)
    return (tx, ty, person_pos[2])


def compute_loose_target(person_pos, robot_pos, follow_dist=2.0, tolerance=0.8):
    """Only move if farther than follow_dist + tolerance."""
    dx = person_pos[0] - robot_pos[0]
    dy = person_pos[1] - robot_pos[1]
    dist = math.hypot(dx, dy)
    if dist <= follow_dist + tolerance:
        return robot_pos
    # Target at follow_dist behind person (toward robot)
    scale = (dist - follow_dist) / dist
    return (robot_pos[0] + dx * scale, robot_pos[1] + dy * scale, person_pos[2])


def compute_tight_target(person_pos, follow_dist=0.6):
    """Close follow: stay very near person."""
    return (person_pos[0], person_pos[1] - follow_dist, person_pos[2])


MODE_VOICE_COMMANDS = {
    'shadow': FollowMode.SHADOW,
    'follow me': FollowMode.SHADOW,
    'behind me': FollowMode.SHADOW,
    'lead': FollowMode.LEAD,
    'go ahead': FollowMode.LEAD,
    'lead me': FollowMode.LEAD,
    'side': FollowMode.SIDE,
    'stay beside': FollowMode.SIDE,
    'orbit': FollowMode.ORBIT,
    'circle me': FollowMode.ORBIT,
    'loose': FollowMode.LOOSE,
    'give me space': FollowMode.LOOSE,
    'tight': FollowMode.TIGHT,
    'stay close': FollowMode.TIGHT,
}
