# vim: fdm=indent ts=4
"""
Library for this simulation
"""
import numpy as np

from earsim import Antenna

def sine_beacon(f, t, t0=0, amplitude=1, baseline=0, phase=0):
    """
    Return a sine appropriate as a beacon
    """
    return amplitude * np.cos(2*np.pi*f*(t+t0) + phase) + baseline


def distance(x1, x2):
    """
    Calculate the Euclidean distance between two locations x1 and x2
    """

    assert type(x1) in [Antenna]
    x1 = np.array([x1.x, x1.y, x1.z])

    assert type(x2) in [Antenna]
    x2 = np.array([x2.x, x2.y, x2.z])

    return np.sqrt( np.sum( (x1-x2)**2 ) )

def beacon_from(tx, rx, f, t=0, t0=0, c_light=3e8, radiate_rsq=True, amplitude=1,**kwargs):
    dist = distance(tx,rx)
    t0 = t0 + dist/c_light

    if radiate_rsq:
        if np.isclose(dist, 0):
            dist = 1
        amplitude *= 1/(dist**2)

    return sine_beacon(f, t, t0=t0, amplitude=amplitude,**kwargs)


def phase_field_from_tx(x, y, tx, f_beacon, c_light=3e8, t0=0, wrap_phase=True, return_meshgrid=True):
    xs, ys = np.meshgrid(x, y, sparse=True)

    x_distances = (tx.x - xs)**2
    y_distances = (tx.y - ys)**2

    dist = np.sqrt( x_distances + y_distances )

    phase = (dist/c_light + t0) * f_beacon*2*np.pi
    if wrap_phase:
        phase = phase_mod(phase)

    if return_meshgrid:
        return phase, (xs, ys)
    else:
        return phase, (np.repeat(xs, len(ys), axis=0), np.repeat(ys, len(xs[0]), axis=1))


def phase_mod(phase, low=np.pi):
    """
    Modulo phase such that it falls within the
    interval $[-low, 2\pi - low)$.
    """
    return (phase + low) % (2*np.pi) - low