import numpy as np
import matplotlib
matplotlib.use('TkAgg')
from matplotlib import pyplot as plt


# This script is used to get an intuition of certain speed controller functions
# The dependence on the measured distance is investigated here 


# input to the speed controller:
v   = 12
v_v = 10 
PS  = 10
IPD = 20
ds  = np.linspace(0.*IPD, 1.7*IPD, num = 1000)

IPD_TOL = 0.05*IPD

# output speed values (2different controllers) 
v_newsAdv = []
v_newsLin = []

def checkInbound(argument, target, delta):  # Ueberall ist ein delta von 0.01 hinterlegt
    if argument > target + delta:
        return False
    elif argument < target - delta:
        return False
    else:
        return True

def variante_adv(d):
  DIST_POW  = 2
  SPEED_POW = 0.5
  v_new = (v_v * (d/(IPD))**DIST_POW * (PS/v_v)**SPEED_POW)
  return v_new

def variante_lin(d):
  v_new = (v_v * (d/(IPD))**1 * (PS/v_v)**0.5)
  return v_new

# compute all values
for d in ds:
  v_newsAdv.append(variante_adv(d))
  v_newsLin.append(variante_lin(d))

# plot
plt.plot(ds, v_newsAdv,'b-', label='v_new (exp)')
plt.plot(ds, v_newsLin,'b:', label='v_new (lin)')
plt.plot(ds, [v_v for d in ds],'tab:orange', label='v_v')
plt.plot(ds, [PS  for d in ds], 'g', label='PS')
plt.plot(ds, [v  for d in ds], 'b--', label='v')
plt.plot([IPD, IPD], [0, 1.1*max(max(v_newsAdv),PS,v_v,v)], 'r', label='IPD')
plt.plot([IPD - IPD_TOL, IPD - IPD_TOL], [0, 1.1*max(max(v_newsAdv),PS,v_v,v)], 'r--', label='IPD-')
plt.plot([IPD + IPD_TOL, IPD + IPD_TOL], [0, 1.1*max(max(v_newsAdv),PS,v_v,v)], 'r--', label='IPD+')
plt.legend()
plt.show()