# coding: utf-8

# # Time Optimal Velocity Profiles
#
# ***
#
# When the maze solver commands that the robot go forward, it can say that it must go forward one or more squares depending on what it knows about the maze. When we don't know what is after the square we pass through, we must be going slow enough to handle any scenario. In other words, there is some $V_f$ that we must reach by the end of our motion. We also begin motions at this speed, since between we arrived where we are we required that we reach $V_f$ to get there. Therefore, we start and end at $V_f$, and we want to cover some distance $d$ in the fast possible time. To do so, we accelerate at our fixed $a$ until we reach max speed, or until we need to start slowing down (whichever comes first). This gives us a trapezoid shaped velocity profile.

# ## Going Straight

# In[5]:

get_ipython().run_line_magic('load_ext', 'tikzmagic')

# In[6]:

get_ipython().run_cell_magic('tikz', '-s 400,400', '\\draw[->] (0,0) -- (10,0);\n\\draw[->] (0,0) -- (0,5);\n\n\\draw[line width=1] (0,0.5) -- (2.5,3);\n\\draw[line width=1] (2.5,3) -- (5.5,3);\n\\draw[line width=1] (5.5,3) -- (8,0.5);\n\\draw[dashed] (0,0.5) -- (10,0.5);\n\\draw[dashed] (0,3) -- (10,3);\n\\draw[dashed] (2.5,0) -- (2.5,5);\n\\draw[dashed] (5.5,0) -- (5.5,5);\n\\draw[dashed] (8,0) -- (8,5);\n\n\\draw (-0.5, 0.5) node {$V_{f}$};\n\\draw (-0.5, 3) node {$V_{max}$};\n\\draw (2.5, -0.5) node {$t_b$};\n\\draw (5.5, -0.5) node {$t_f-t_b$};\n\\draw (8, -0.5) node {$t_f$};')

# The time to accelerate from $V_f$ to $V_{max}$ is $t_b = \frac{V-V_f}{a}$. We can substitute this into newtons first equation of motion as follows.
#
# \begin{align}
# d &= Vt_b - \frac{1}{2}a{t_b}^2 \\
#   &= V\Big(\frac{V-V_f}{a}\Big) - \frac{1}{2}a\Big(\frac{V-V_f}{a}\Big)^2 \\
#   &= \Big(\frac{V^2-VV_f}{a}\Big) - \Big(\frac{a(V-V_f)^2}{2a^2}\Big) \\
#   &= \Big(\frac{2V^2-2VV_f}{2a}\Big) - \Big(\frac{V^2-2VV_f+{V_f}^2}{2a}\Big) \\
#   &= \frac{2V^2-2VV_f - V^2 + 2VV_f - {V_f}^2}{2a} \\
# d &= \frac{V^2-{V_f}^2}{2a} \\
# \end{align}
#
# For example, if you're at starting at $V_f=0.2\frac{m}{s}$, and you're ramping up to $V=0.5\frac{m}{s}$, and you're acceleration is fixed at the $a=2\frac{m}{s^2}$, the distance you'll need to do that is $d = \frac{0.5 - 0.2}{2*2} = 0.075m$

# ## Code that proves it

# In[7]:

# dependencies and global setup
import numpy as np
import matplotlib.pyplot as plt
np.set_printoptions(suppress=True, precision=3, linewidth=100)

LOG_LVL = 2

def debug(*args):
    if LOG_LVL <= 0:
        print(*args)

def info(*args):
    if LOG_LVL <= 1:
        print(*args)

def warning(*args):
    if LOG_LVL <= 2:
        print(*args)

def log(*args):
    if LOG_LVL < 100:
        print(*args)

# In[8]:

def profile(V0, Vf, Vmax, d, A, buffer=3e-4):
    v = V0
    x = 0
    a = A
    vs = [v]
    xs = [x]
    a_s = [a]

    dt = 0.001
    while x < d:
        x = x + v*dt + a*dt*dt/2.0
        v = v + a*dt
        ramp_d = (v*v+ - Vf*Vf) / (2.0*A)
        if (d-x) < ramp_d + buffer:
            a = -A
        elif v < Vmax:
            a = A
        else:
            a = 0

        if v > Vmax:
            v = Vmax
        elif v < Vf:
            v = Vf

        xs.append(x)
        vs.append(v)
        a_s.append(a)

    return xs, vs, a_s

def graph(title, idx):
    plt.figure()
    plt.title(title)
    # various max speeds
    Vs = [0.35, 0.5, 0.75, 1, 2]
    Vf = 0.02
    V0 = 0.2
    d = 0.35
    a = 2
    for V in Vs:
        results  = profile(V0, Vf, V, d, a)
        vs = results[1]
        if V == 2: # make V=2 dashed so we can see it over V=1
            plt.plot(results[idx], label='V={}'.format(V), linestyle='dashed')
        else:
            plt.plot(results[idx], label='V={}'.format(V))
        plt.legend(bbox_to_anchor=(1, 1), loc=2)

graph("position", 0)
graph("velocity", 1)
graph("acceleration", 2)
plt.show()

# ## General Form Trajectory Planning

# Let's start out with a generating trajectories that are not time optimal, but rely on specifying the final time $v_f$. For smartmouse, our state space is $[x, y, \theta]$, and a turn can be defined as starting at a point $[x_0, y_0, \theta_0]$ and going to $[x_f, y_f, \theta_0]$. Of course, we also want to specify the velocities at these point, $[\dot{x}_0, \dot{y}_0,\dot{\theta}_0]$ and $[\dot{x}_f, \dot{y}_f,\dot{\theta}_f]$. We have four constraints, so if we want to fit a smooth polynomial to those points we need a 4th order polynomial.
#
# $$q(t) = a_0 + a_1t + a_2t^2 + a_3t^3$$
# $$\dot{q}(t) = a_1 + 2a_2t + 3a_3t^2$$
#
# If we sub in our constraints, we get the following system of equations.
#
# \begin{align}
# q(0) &= a_0 \\
# \dot{q}(0) &= a_1 \\
# q(t_f) &= a_0 + a_1t_f + a_2{t_f}^2 + a_3{t_f}^3\\
# \dot{q}(t_f) &= a_1 + 2a_2t_f + 3a_3{t_f}^2\\
# \end{align}
#
# In matrix form that looks like:
# \begin{equation}
# \begin{bmatrix}
# 1 & 0 & 0 & 0 \\
# 0 & 1 & 0 & 0 \\
# 1 & t_f & t_f^2 & t_f^3 \\
# 0 & 1 & 2t_f & 3t_f^2 \\
# \end{bmatrix}
# \begin{bmatrix}
# a_0 \\
# a_1 \\
# a_2 \\
# a_3 \\
# \end{bmatrix} =
# \begin{bmatrix}
# q(0) \\
# \dot{q}(0) \\
# q(t_f) \\
# \dot{q}(t_f) \\
# \end{bmatrix}
# \end{equation}
#
# It can be shown that the matrix on the left is invertable, so long as $t_f-t_0 > 0$. So we can invert and solve this equation and get all the $a$ coefficients. We can then use this polynomial to generate the $q(t)$ and $\dot{q}(t)$ -- our trajectory.

# In[9]:

def simple_traj_solve(q_0, q_f, q_dot_0, q_dot_t_f, t_f):
    # Example: you are a point in space (one dimension) go from rest at the origin to at rest at (0.18, 0, 0) in 1 second
    q_0 = np.array([0])
    q_dot_0 = np.array([0])
    q_t_f = np.array([0.18])
    q_dot_t_f = np.array([0])

    b = np.array([q_0, q_dot_0, q_t_f, q_dot_t_f])
    a = np.array([[1,0,0,0],[0,1,0,0],[1, t_f, pow(t_f,2),pow(t_f,3)],[0,1,2*t_f,3*pow(t_f,2)]])
    log(a, b)
    coeff = np.linalg.solve(a, b)
    log(coeff)

    return coeff

simple_traj_info = (0, 0, 0.18, 0, 1)
simple_traj_coeff = simple_traj_solve(*simple_traj_info)

# Here you can see that the resulting coeffictions are $a_0=0$, $a_1=0$, $a_2=0.54$, $a_0=-0.36$. Intuitively, this says that we're going to have positive acceleration, but our acceleration is going to slow down over time. Let's graph it!

# In[10]:

def simple_traj_plot(coeff, t_f):
    dt = 0.01
    ts = np.array([[1, t, pow(t,2), pow(t,3)] for t in np.arange(0, t_f+dt,  dt)])
    qs = ts@coeff
    plt.plot(ts[:,1], qs, label="x")
    plt.xlabel("time (seconds)")
    plt.xlabel("X (meters)")
    plt.legend(bbox_to_anchor=(1,1), loc=2)
    plt.show()

simple_traj_plot(simple_traj_coeff, simple_traj_info[-1])

# **ooooooooooh so pretty**
#
# Let's try another example, now with our full state space of $[x, y, \theta]$.

# In[11]:

def no_dynamics():
    # In this example, we go from (0.18, 0.09, 0) to (0.27,0.18, -1.5707). Our starting and ending velocities are zero
    q_0 = np.array([0.09,0.09,0])
    q_dot_0 = np.array([0,0,0])
    q_f = np.array([0.27,0.18,-1.5707])
    q_dot_f = np.array([0,0,0])
    t_f = 1

    b = np.array([q_0, q_dot_0, q_f, q_dot_f])
    a = np.array([[1,0,0,0],[0,1,0,0],[1, t_f, pow(t_f,2),pow(t_f,3)],[0,1,2*t_f,3*pow(t_f,2)]])
    coeff = np.linalg.solve(a, b)
    log(coeff)

    dt = 0.1
    ts = np.array([[1, t, pow(t,2), pow(t,3)] for t in np.arange(0, t_f+dt,  dt)])
    qs = ts@coeff

    plt.rc('text', usetex=True)
    plt.rc('font', family='serif')
    plt.gca().set_adjustable("box")
    plt.subplot(221)
    plt.plot(ts[:,1], qs[:,0])
    plt.xlabel("time (seconds)")
    plt.title("x")
    plt.subplot(222)
    plt.plot(ts[:,1], qs[:,1])
    plt.xlabel("time (seconds)")
    plt.title("y")
    plt.subplot(223)
    plt.plot(ts[:,1], qs[:,2])
    plt.xlabel("time (seconds)")
    plt.title(r"$\theta$")
    plt.subplot(224)
    plt.scatter(qs[:,0], qs[:,1])
    plt.axis('equal')
    plt.xlabel("X")
    plt.ylabel("Y")
    plt.tight_layout()
    plt.show()

no_dynamics()

# Well, they are smooth, but these are not possible to execute! The robot cannot simply translate sideways.

# # Trajectory Planning With a Simple Dynamics Model
#
# ***

# In[12]:

get_ipython().run_cell_magic('tikz', '-s 100,100', '\n\\draw [rotate around={-45:(0,0)}] (-.5,-1) rectangle (0.5,1);\n\\filldraw (0,0) circle (0.125);\n\n\\draw [->] (0,0) -- (0,1.5);\n\\draw [->] (0,0) -- (1.5,0);\n\\draw [->] (0,0) -- (1.5,1.5);\n\\draw (1.2, -0.2) node {$x$};\n\\draw (-0.2, 1.2) node {$y$};\n\\draw (1, 1.2) node {$v$};')

#
# We need to change our constraints to the system of equations. Specifically, we need our dynamics model. For now, let's assume a simplified car model.
#
# $$ \dot{x} = v\cos(\theta) $$
# $$ \dot{y} = v\sin(\theta) $$
#
# This basically claims that for any instant in time the robot is moving a constant velocity along $\theta$. This isn't very accurate, but let's just start with that since the real dynamics of our robot are more complex.
#
# First we will bring in the constraints from before. We must satisfy specific initial and final positions in $[x, y, \theta]$. I've used new letters for cofficients to avoid confusion.
#
# \begin{align}
# x_0 &= c_0 + c_1(0) + c_2(0)^2 + c_3(0)^3 + c_3(0)^4 + c_3(0)^5 \\
# y_0 &= d_0 + d_1(0) + d_2(0)^2 + d_3(0)^3 + d_3(0)^4 + d_3(0)^5 \\
# x_{t_f} &= c_0 + c_1(t_f) + c_2(t_f)^2 + c_3(t_f)^3 + c_3(t_f)^4 + c_3(t_f)^5 \\
# y_{t_f} &= d_0 + d_1(t_f) + d_2(t_f)^2 + d_3(t_f)^3 + d_3(t_f)^4 + d_3(t_f)^5 \\
# \end{align}
#
# Notice here we have 12 unknowns, $c_0 \dots c_5$ and $d_0 \dots d_5$. So we're gonna need more equations for there to be a unique solution. Also notice we haven't defined any constraints related to our dynamics model. That would be a good place to get our other equations!
#
# First, we want to be able to specify initial velocity $v_0$ and final velocity $v_{t_f}$. It is easlier to just constrain $\dot{x}_0$,  $\dot{y}_0$,  $\dot{x}_{t_f}$,  $\dot{y}_{t_f}$. So if we want to specify that we start facing $\tfrac{\pi}{2}$ going 1m/s, we'd just specify $cos(\tfrac{\pi}{2})$ for $\dot{x}_0$ and $sin(\tfrac{\pi}{2})$ for $\dot{y}_0$.
#
# \begin{align}
# \dot{x}_0 &= c_1 \\
# \dot{y}_0 &= d_1 \\
# \dot{x}_{t_f} &= (0)c_0 + (1)c_1 + 2t_fc_2 + 3{t_f}^2c_3 + 4{t_f}^3c_4 + 5{t_f}^4c_5 \\
# \dot{y}_{t_f} &= (0)d_0 + (1)d_1 + 2t_fd_2 + 3{t_f}^2d_3 + 4{t_f}^3d_4 + 5{t_f}^4d_5
# \end{align}
#
# Let's also make sure x and y components obey trigonometry.
#
# \begin{align}
#   v\cos(\theta)\sin(\theta) + v\cos(\theta)\sin(\theta) &= v\sin(2\theta) \\
#   \dot{x}\sin(\theta) + \dot{y}\sin(\theta) &= v\sin(2\theta)
# \end{align}
#
# We can get two equations out of this by specifying initial and final velocities
#
# \begin{align}
# v_0\sin(2\theta_0) &= \dot{x}_0\sin(\theta_0) + \dot{y}_0\cos(\theta_0) \\
# v_{t_f}\sin(2\theta_{t_f}) &= \dot{x}_{t_f}\sin(\theta_{t_f}) + \dot{y}_{t_f}\cos(\theta_{t_f})
# \end{align}
#
# We should write out the full form though, to make things in terms of our coefficients.
#
# \begin{align}
# v(0)\sin(2\theta_0) &= \Big[c_1 + 2(0)c_2 + 3(0)^2c_3 + 4(0)^3c_4 + 5(0)^4c_5\Big]\sin(\theta_0) + \Big[d_1 + 2(0)d_2 + 3(0)^2d_3 + 4(0)^3d_4 + 5(0)^4d_5\Big]\cos(\theta_0) \\
# v(0)\sin(2\theta_0) &= \sin(\theta_0)c_1 + \cos(\theta_0)d_1
# \end{align}
#
# \begin{align}
# v(t_f)\sin(2\theta_{t_f}) &= \Big[c_1 + 2(t_f)c_2 + 3(t_f)^2c_3 + 4(t_f)^3c_4\ + 5(t_f)^4c_5\Big]\sin(\theta_{t_f}) + \Big[d_1 + 2(t_f)d_2 + 3(t_f)^2d_3 + 4(t_f)^3d_4 + 5(t_f)^4d_5\Big]\cos(\theta_{t_f}) \\
# v(t_f)\sin(2\theta_{t_f}) &= \sin(\theta_{t_f})c_1 + 2\sin(\theta_{t_f})t_fc_2 + 3\sin(\theta_{t_f}){t_f}^2c_3 + 4\sin(\theta_{t_f}){t_f}^3c_4  + 5\sin(\theta_{t_f}){t_f}^4c_5 + \cos(\theta_{t_f})d_1 + 2\cos(\theta_{t_f})t_fd_2 + 3\cos(\theta_{t_f}){t_f}^2d_3 + 4\cos(\theta_{t_f}){t_f}^3d_4 + 5\cos(\theta_{t_f}){t_f}^4d_5 \\
# \end{align}
#
# The last two equations constrains the robot from moving in any direction other than its heading. Of course it must relate $\dot{x}$ to $\dot{y}$. Still not totally sure how we got this equation so I'm just copying it from some slides$\dots$. However you can plug in some example values and check. For instance translating sideways violates this equation: set $\dot{x}=1$, $\dot{y}=0$, $v=1$, $\theta=\tfrac{\pi}{2}$.
#
# \begin{align}
# v\cos(\theta)\sin(\theta) - v\cos(\theta)\sin(\theta) &= 0 \\
# v\cos(\theta)\sin(\theta) - v\sin(\theta)\cos(\theta) &= 0 \\
# \dot{x}\sin(\theta) - \dot{y}\cos(\theta) &= 0
# \end{align}
#
# and again written out fully in terms of our coefficients
#
# \begin{align}
# \Big[c_1 + 2(0)c_2 + 3(0)^2c_3 + 4(0)^3c_4 + 5(0)^4c_5\Big]\sin(\theta_0) - \Big[d_1 + 2(0)d_2 + 3(0)^2d_3 + 4(0)^3d_4 + 5(0)^4d_5\Big]\cos(\theta_0) &= 0 \\
# \sin(\theta_0)c_1 - \cos(\theta_0)d_1 &= 0
# \end{align}
#
# \begin{align}
# \Big[c_1 + 2(t_f)c_2 + 3(t_f)^2c_3 + 4(t_f)^3c_4 + 5(t_f)^4c_5\Big]\sin(\theta_{t_f}) - \Big[d_1 + 2(t_f)d_2 + 3(t_f)^2d_3 + 4(t_f)^3d_4 + 5(t_f)^4d_5\Big]\cos(\theta_{t_f}) &= 0 \\
# \sin(\theta_{t_f})c_1 + 2\sin(\theta_{t_f})t_fc_2 + 3\sin(\theta_{t_f}){t_f}^2c_3 + 4\sin(\theta_{t_f}){t_f}^3c_4  + 5\sin(\theta_{t_f}){t_f}^4c_5 - \cos(\theta_{t_f})d_1 - 2\cos(\theta_{t_f})t_fd_2 - 3\cos(\theta_{t_f}){t_f}^2d_3 - 4\cos(\theta_{t_f}){t_f}^3d_4 - 5\cos(\theta_{t_f}){t_f}^4d_5 &= 0
# \end{align}
#
# Ok, that should work. Now let's write it out in matrix form. We use $c$ and $s$ to shorten $\sin$ and $\cos$.
#
# \setcounter{MaxMatrixCols}{20}
# \begin{equation}
# \begin{bmatrix}
# 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\
# 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 \\
# 0 & s(\theta_0) & 0 & 0 & 0 & 0 & 0 & c(\theta_0) & 0 & 0 & 0 & 0\\
# 0 & s(\theta_0) & 0 & 0 & 0 & 0 & 0 & -c(\theta_0) & 0 & 0 & 0 & 0\\
# 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\
# 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0\\
# 1 & t & {t_f}^2 & {t_f}^3 & {t_f}^4 & {t_f}^5 & 0 & 0 & 0 & 0 & 0 & 0 \\
# 0 & 0 & 0 & 0 & 0 & 0 & 1 & t_f & {t_f}^2 & {t_f}^3 & {t_f}^4 & {t_f}^5 \\
# 0 & s(\theta_{t_f}) & 2s(\theta_{t_f})t_f & 3s(\theta_{t_f}){t_f}^2 & 4s(\theta_{t_f}){t_f}^3 & 5s(\theta_{t_f}){t_f}^4 & 0 & c(\theta_{t_f}) & 2c(\theta_{t_f}){t_f} & 3c(\theta_{t_f}){t_f}^2 & 4c(\theta_{t_f}){t_f}^3 & 5c(\theta_{t_f}){t_f}^4 \\
# 0 & s(\theta_{t_f}) & 2s(\theta_{t_f})t_f & 3s(\theta_{t_f}){t_f}^2 & 4s(\theta_{t_f}){t_f}^3 & 5s(\theta_{t_f}){t_f}^4 & 0 & -c(\theta_{t_f}) & -2c(\theta_{t_f}){t_f} & -3c(\theta_{t_f}){t_f}^2 & -4c(\theta_{t_f}){t_f}^3 & -5c(\theta_{t_f}){t_f}^4 \\
# 0 & 1 & 2t_f & 3{t_f}^2 & 4{t_f}^3 & 5{t_f}^4 & 0 & 0 & 0 & 0 & 0 & 0 \\
# 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 2t_f & 3{t_f}^2 & 4{t_f}^3 & 5{t_f}^4
# \end{bmatrix}
# \begin{bmatrix}
# c_0 \\
# c_1 \\
# c_2 \\
# c_3 \\
# c_4 \\
# c_5 \\
# d_0 \\
# d_1 \\
# d_2 \\
# d_3 \\
# d_4 \\
# d_5
# \end{bmatrix} =
# \begin{bmatrix}
# x_0 \\
# y_0 \\
# 0 \\
# v_0s(2\theta_0) \\
# c(\theta_0)v_0 \\
# s(\theta_0)v_0 \\
# x_{t_f} \\
# y_{t_f} \\
# 0 \\
# v_{t_f}s(2\theta_{t_f}) \\
# c(\theta_{t_f})v_{t_f} \\
# s(\theta_{t_f})v_{t_f} \\
# \end{bmatrix}
# \end{equation}

# In[13]:

# Let's solve this in code like we did before
def plot_vars(traj_plan):
    dt = 0.001
    T = np.arange(0, traj_plan.get_t_f()+dt, dt)
    xts = np.array([[1, t, pow(t,2), pow(t,3), pow(t,4), pow(t,5), 0, 0, 0, 0, 0, 0] for t in T])
    xdts = np.array([[0, 1, 2*t, 3*pow(t,2), 4*pow(t,3), 5*pow(t,4), 0, 0, 0, 0, 0, 0] for t in T])
    yts = np.array([[0, 0, 0, 0, 0, 0, 1, t, pow(t,2), pow(t,3), pow(t,4), pow(t,5)] for t in T])
    ydts = np.array([[0, 0, 0, 0, 0, 0, 0, 1, 2*t, 3*pow(t,2), 4*pow(t,3), 5*pow(t,4)] for t in T])
    xs = xts@traj_plan.get_coeff()
    ys = yts@traj_plan.get_coeff()
    xds = xdts@traj_plan.get_coeff()
    yds = ydts@traj_plan.get_coeff()

    plt.rc('text', usetex=True)
    plt.rc('font', family='serif')
    plt.rc('axes.formatter', useoffset=False)
    plt.figure(figsize=(10, 2.5))

    plt.subplot(141)
    plt.plot(T, xs, linewidth=3)
    plt.xlabel("time (seconds)")
    plt.title("X")

    plt.subplot(142)
    plt.plot(T, ys, linewidth=3, color='r')
    plt.xlabel("time (seconds)")
    plt.title("Y")

    plt.subplot(143)
    plt.plot(T, xds, linewidth=3, color='g')
    plt.xlabel("time (seconds)")
    plt.title("$\dot{x}$")
    plt.tight_layout()

    plt.subplot(144)
    plt.plot(T,yds, linewidth=3, color='y')
    plt.xlabel("time (seconds)")
    plt.title("$\dot{y}$")
    plt.tight_layout()
    plt.show()

def plot_traj(traj_plan):
    dt = 0.03
    T = np.arange(0, traj_plan.get_t_f()+dt, dt)
    xts = np.array([[1, t, pow(t,2), pow(t,3), pow(t,4), pow(t,5), 0, 0, 0, 0, 0, 0] for t in T])
    yts = np.array([[0, 0, 0, 0, 0, 0, 1, t, pow(t,2), pow(t,3), pow(t,4), pow(t,5)] for t in T])
    xs = xts@traj_plan.get_coeff()
    ys = yts@traj_plan.get_coeff()
    plot_traj_pts(xs, ys, T, traj_plan.waypoints)

def plot_traj_pts(xs, ys, T, waypoints):
    plt.figure(figsize=(5, 5))
    plt.title("Trajectory")
    plt.xlabel("X")
    plt.ylabel("Y")
    W = 2
    plt.xlim(0, W * 0.18)
    plt.ylim(0, W * 0.18)
    plt.xticks(np.arange(2*W+1)*0.09)
    plt.yticks(np.arange(2*W+1)*0.09)
    plt.grid(True)
    plt.gca().set_axisbelow(True)

    for t, pt in waypoints:
        arrow_dx = cos(pt.theta) * (pt.v) * 0.1
        arrow_dy = sin(pt.theta) * (pt.v) * 0.1
        plt.arrow(pt.x, pt.y, arrow_dx, arrow_dy, head_width=0.005, head_length=0.005, width=0.001, fc='k', ec='k')

    plt.scatter(xs, ys, marker='.', linewidth=0)

    plt.show()

# In[14]:

from math import sin, cos, pi
from collections import namedtuple

WayPoint = namedtuple('WayPoint', ['x', 'y', 'theta', 'v'])

class TrajPlan:
    def x_constraint(t):
        return [1, t, pow(t, 2), pow(t, 3), pow(t, 4), pow(t, 5), 0, 0, 0, 0, 0, 0]

    def y_constraint(t):
        return [0, 0, 0, 0, 0, 0, 1, t, pow(t, 2), pow(t, 3), pow(t, 4), pow(t, 5)]

    def non_holonomic_constraint(theta_t, t):
        s_t = sin(theta_t)
        c_t = cos(theta_t)
        t_2 = pow(t, 2)
        t_3 = pow(t, 3)
        t_4 = pow(t, 4)
        return [0, s_t, 2 * s_t * t, 3 * s_t * t_2, 4 * s_t * t_3, 5 * s_t * t_4, 0, c_t, 2 * c_t * t, 3 * c_t * t_2, 4 * c_t * t_3, 5 * c_t * t_4]

    def trig_constraint(theta_t, t):
        s_t = sin(theta_t)
        c_t = cos(theta_t)
        t_2 = pow(t, 2)
        t_3 = pow(t, 3)
        t_4 = pow(t, 4)
        return [0, s_t, 2 * s_t * t, 3 * s_t * t_2, 4 * s_t * t_3, 5 * s_t * t_4, 0, -c_t, -2 * c_t * t, -3 * c_t * t_2, -4 * c_t * t_3, -5 * c_t * t_4]

    def x_dot_constraint(t):
        return [0, 1, 2 * t, 3 * pow(t, 2), 4 * pow(t, 3), 5 * pow(t, 4), 0, 0, 0, 0, 0, 0]

    def y_dot_constraint(t):
        return [0, 0, 0, 0, 0, 0, 0, 1, 2 * t, 3 * pow(t, 2), 4 * pow(t, 3), 5 * pow(t, 4)]

    def solve(self, waypoints):
        # Setup the matrices to match the equation above
        A = []
        b = []

        for t, pt in waypoints:
            A += [TrajPlan.x_constraint(t),
                      TrajPlan.y_constraint(t),
                      TrajPlan.non_holonomic_constraint(pt.theta, t),
                      TrajPlan.trig_constraint(pt.theta, t),
                      TrajPlan.x_dot_constraint(t),
                      TrajPlan.y_dot_constraint(t)]
            b += [pt.x,
                      pt.y,
                      0,
                      pt.v*sin(2*pt.theta),
                      cos(pt.theta)*pt.v,
                      sin(pt.theta)*pt.v]

        A = np.array(A)
        b = np.array(b)
        rank = np.linalg.matrix_rank(A)

        if rank == A.shape[1]:
            if A.shape[0] == A.shape[1]:
                coeff = np.linalg.solve(A, b)
            else:
                warning("not square, using least squares.".format(A.shape))
                coeff, resid, rank, s = np.linalg.lstsq(A, b)
        else:
            warning("Ranks don't match! {} equations {} variables, using least squares".format(rank, A.shape[1]))
            coeff, resid, rank, s = np.linalg.lstsq(A, b)

        debug("rank {}".format(rank))
        debug("A: \n{}".format(A))
        debug("coeff: \n{}".format(coeff))
        error = np.sum(np.power(A@coeff - b, 2))
        if error > 1e-10:
            info("These two vectors should be equal! But there is error.")
            info("b is: \n{}".format(b))
            info("A@coeff is: \n{}".format(A@coeff))
        info("RMS Error of solution to equations")
        info(error)

        self.coeff = coeff
        self.waypoints = waypoints

    def get_coeff(self):
        return self.coeff

    def get_t_f(self):
        return self.waypoints[-1][0]

# ## Example Plots

# In[15]:

# forward 1 cell, start from rest, end at 40cm/s, do it in .5 seconds
LOG_LVL = 5
fwd_1 = TrajPlan()
fwd_1.solve([(0, WayPoint(0.09, 0.09, pi/2, 0)), (0.5, WayPoint(0.09, 0.27, pi/2, 0.6))])
plot_vars(fwd_1)
plot_traj(fwd_1)

# In[16]:

# continue by turning right 90 degrees
LOG_LVL = 1
turn_right = TrajPlan()
turn_right.solve([(0, WayPoint(0.09, 0.18, pi/2, 0.4)), (0.5, WayPoint(0.18, 0.27, 0, 0.4))])
plot_vars(turn_right)
plot_traj(turn_right)

# In[17]:

# 3 waypoints!
LOG_LVL = 1
turn_right = TrajPlan()
turn_right.solve([(0, WayPoint(0.09, 0.09, pi/2, 0.0)), (0.5, WayPoint(0.18, 0.18, 0, 0.35)), (1, WayPoint(0.27, 0.27, pi/2, 0))])
plot_vars(turn_right)
plot_traj(turn_right)

# **Note for this system of equations with 3 waypoints, there is no solution. However, the error of the solution found is very small.**
#
# Now let's find one that really sucks!

# In[18]:

# 4 waypoints!
LOG_LVL = 1
turn_right = TrajPlan()
turn_right.solve([(0, WayPoint(0.09, 0.0, pi/2, 0.1)),
                  (1, WayPoint(0.09, 0.18, pi/2, 0.1)),
                  (2, WayPoint(0.18, 0.27, 0, 0.1)),
                  (3, WayPoint(0.27, 0.27, 0, 0.1))])
plot_traj(turn_right)

# # Trajectory Following
#
# ***
#
# Now that we have a trajectory, we want to design a controller that will follow it as closely as possible. To do this, I'm just going to do a proportional controller. Later we will design an optimal controller. We want to make sure the robot is on the path, facing along the path, and going the right speed. When all of these are true the change in speed should be zero. Let's come up with an equation to relate current pose and velocity to the desired pose and velocity. Let our outputs be the linear velocity $v$ and the rotational velocity $w$.
#
# $$ w = \bar{w} + d*P_1 + (\bar{\theta} - \theta)P_2$$
# $$ v = \bar{v} + l*P_3$$
#
# where $v_d$ is desired velocity, $\theta_d$ is the desired angle, $d$ is signed distance to the planned trajectory (to the right of the plan is positive), $v_d$ and $w_d$ are the desired velocities of the robot, and $P_1$, $P_2$, and $P_3$ are constants. Essentially what we're saying with the first equation is that when you're far off the trajectory you need to turn harder to get back on to it, but you also need to be aligned with it. The second equation says if you're lagging behind your plan speed up, or slow down if you're overshooting.

# In[19]:

from math import atan2, sqrt

LOG_LVL = 5
def simulate(q_0, waypoints, P_1, P_2, P_3, A=3):
    traj = TrajPlan()
    traj.solve(waypoints)

    dt = 0.01
    x = q_0[0]
    y = q_0[1]
    theta = q_0[2]
    v = q_0[3]
    w = q_0[4]
    actual_v = q_0[3]
    actual_w = q_0[4]
    v_acc = A * dt
    TRACK_WIDTH_M = 0.0633
    w_acc = v_acc / (TRACK_WIDTH_M/2)
    T = np.arange(0, traj.get_t_f()+dt, dt)
    x_bar_list = []
    y_bar_list = []
    x_list = []
    y_list = []
    for t in T:
        x_bar = [1, t, pow(t,2), pow(t,3), pow(t,4), pow(t,5), 0, 0, 0, 0, 0, 0] @ traj.get_coeff()
        dx_bar = [0, 1, 2*t, 3*pow(t,2), 4*pow(t,3), 5*pow(t,4), 0, 0, 0, 0, 0, 0] @ traj.get_coeff()
        ddx_bar = [0, 0, 0, 0, 0, 0, 0, 0, 2, 6*t, 12*pow(t,2), 20*pow(t,3)] @ traj.get_coeff()
        y_bar = [0, 0, 0, 0, 0, 0, 1, t, pow(t,2), pow(t,3), pow(t,4), pow(t,5)] @ traj.get_coeff()
        dy_bar = [0, 0, 0, 0, 0, 0, 0, 1, 2*t, 3*pow(t,2), 4*pow(t,3), 5*pow(t,4)] @ traj.get_coeff()
        ddy_bar = [0, 0, 0, 0, 0, 0, 0, 0, 2, 6*t, 12*pow(t,2), 20*pow(t,3)] @ traj.get_coeff()
        theta_bar = atan2(dy_bar, dx_bar)
        v_bar = sqrt(dx_bar*dx_bar + dy_bar*dy_bar)
        w_bar = 1/v_bar * (ddy_bar*cos(theta_bar) - ddx_bar*sin(theta_bar));

        # simple Dubin's Car forward kinematics
        x += cos(theta) * actual_v * dt
        y += sin(theta) * actual_v * dt
        theta += actual_w * dt

        # control
        euclidian_error = np.sqrt(pow(x_bar - x, 2) + pow(y_bar - y, 2))
        transformed_x = (x - x_bar) * cos(-theta_bar) + (y - y_bar) * -sin(-theta_bar)
        transformed_y = (x - x_bar) * sin(-theta_bar) + (y - y_bar) * cos(-theta_bar)
        right_of_traj = transformed_y < 0
        signed_euclidian_error = euclidian_error if right_of_traj else -euclidian_error
        lag_error = -transformed_x
        w = w_bar + signed_euclidian_error * P_1 + (theta_bar - theta) * P_2
        v = v_bar + lag_error * P_3

        # simple acceleration model
        if v < actual_v:
            actual_v = max(v, actual_v - v_acc)
        elif v > actual_v:
            actual_v = min(v, actual_v + v_acc)
        if w < actual_w:
            actual_w = max(w, actual_w - w_acc)
        elif w > actual_w:
            actual_w = min(w, actual_w + w_acc)

        x_bar_list.append(x_bar)
        y_bar_list.append(y_bar)
        x_list.append(x)
        y_list.append(y)

    plt.figure(figsize=(5, 5))
    W = 3
    plt.scatter(x_bar_list, y_bar_list, marker='.', linewidth=0, c='black', label='desired traj')
    plt.scatter(x_list, y_list, marker='.', linewidth=0, c=T, label='robot traj')
    plt.xlim(0, W * 0.18)
    plt.ylim(0, W * 0.18)
    plt.xticks(np.arange(2*W+1)*0.09)
    plt.yticks(np.arange(2*W+1)*0.09)
    plt.grid(True)
    plt.gca().set_axisbelow(True)
    plt.xlabel("X")
    plt.ylabel("Y")
    plt.title("Trajectory Tracking")
    plt.legend(bbox_to_anchor=(1,1), loc=2)

# In[20]:

test_P_1=300
test_P_2=50
test_P_3=10
robot_q_0 = (0.08, 0.18, pi/2, 0.3, 0)
traj = [(0, WayPoint(0.09, 0.18, pi/2, 0.5)), (0.5, WayPoint(0.18, 0.27, 0, 0.35)), (1, WayPoint(0.27, 0.36, pi/2, 0))]
simulate(robot_q_0, traj, test_P_1, test_P_2, test_P_3)
plt.show()

# In[21]:

robot_q_0 = (0.11, 0.18, pi/2, 0.2, 5)
traj = [(0, WayPoint(0.09, 0.18, pi/2, 0.2)), (1, WayPoint(0.18, 0.27, 0, 0.35))]
simulate(robot_q_0, traj, test_P_1, test_P_2, test_P_3)
plt.show()

# In[22]:

robot_q_0 = (0.0, 0.25, 0, 0.2, 0)
traj = [(0, WayPoint(0.0, 0.27, 0, 0.2)), (1.25, WayPoint(0.54, 0.27, 0, 0.2))]
simulate(robot_q_0, traj, test_P_1, test_P_2, test_P_3)
plt.show()

# In[23]:

robot_q_0 = (0.45, 0.05, pi+0.25, 0.3, 0)
traj = [(0, WayPoint(0.45, 0.09, pi, 0.4)), (0.75, WayPoint(0.27, 0.27, pi/2, 0.4))]
simulate(robot_q_0, traj, test_P_1, test_P_2, test_P_3)
plt.show()

# In[24]:

robot_q_0 = (0.0, 0.25, 0, 0.2, -5)
traj = [(0, WayPoint(0.0, 0.27, 0, 0.2)), (2, WayPoint(0.48, 0.36, pi/2, 0.2))]
simulate(robot_q_0, traj, test_P_1, test_P_2, test_P_3)
plt.show()

# In[25]:

robot_q_0 = (0.25, 0.28, -pi*4/7, 0.5, 0)
traj = [(0, WayPoint(0.27, 0.27, -pi/2, 0.8)), (0.35, WayPoint(0.45, 0.09, 0, 0.8))]
simulate(robot_q_0, traj, test_P_1, test_P_2, test_P_3, A=6)
plt.show()

# In[26]:

# no initial error
robot_q_0 = (0.11, 0.18, pi/2, 0.8, 0)
traj = [(0, WayPoint(0.09, 0.18, pi/2, 0.8)), (0.25, WayPoint(0.18, 0.27, 0, 0.6)), (.5, WayPoint(0.27, 0.36, pi/2, 0.4))]
simulate(robot_q_0, traj, 10, 1000, 6, A=5)
plt.show()

# **Note**: The code above has a bug if I use `-pi` instead of `pi` in `robot_q_0`

# # LQR - The Optimal Controller
#
# ***
#
# ## Overview of the Steps:
#
# ### 1. Write out the non-linear dynamics $\dot{\vec{x}} = f(\vec{x}, \vec{u})$
#
# Here we are interested in the full blown system dynamics of the actual smartmouse robot. The forward kinematics, which depend on the current state $x$, $y$, and $\theta$ and the velocity inputs of the wheels $v_l$, and $v_r$ are as follows. In the general case where the two wheels have different velocities, we have this:
#
# \begin{align}
#   R &= \frac{W(v_l+v_r)}{2(v_r-v_l)} && \text{radius of turn} \\
#   \theta &\leftarrow \theta + \dfrac{v_l}{R-\frac{W}{2}}\Delta t \\
#   x &\leftarrow x-R\Bigg(\sin{\Big(\frac{v_r-v_l}{W}\Delta t-\theta\Big)}+\sin{\theta}\Bigg) \\
#   y &\leftarrow y-R\Bigg(\cos{\Big(\frac{v_r-v_l}{W}\Delta t-\theta\Big)}-\cos{\theta}\Bigg)
# \end{align}
#
# And in the special case where we're going perfectly straight:
#
# \begin{align}
#   \theta &\leftarrow \theta \\
#   x &\leftarrow x + v\Delta t\cos(\theta) \\
#   y &\leftarrow y + v\Delta t\sin(\theta) \\
# \end{align}
#
# We can take these equations and write them in the form of $\dot{\vec{x}} = f(\vec{x},\vec{u})$. Confusingly, $\vec{x}$ here is the full state vector $[x, y, \theta]$. Most controls texts simply use $\vec{x}$, so I'm sticking with that. Also, we defined $u = [v_l, v_r]$
#
# \begin{align}
#   \dot{x} &= \begin{bmatrix}\dot{x}\\ \dot{y}\\ \dot{\theta}\end{bmatrix} \\
#           &= \begin{bmatrix}
#                 -R\Bigg(\sin{\Big(\frac{v_r-v_l}{W}\Delta t-\theta\Big)}+\sin{\theta}\Bigg) \\
#                 -R\Bigg(\cos{\Big(\frac{v_r-v_l}{W}\Delta t-\theta\Big)}-\cos{\theta}\Bigg) \\
#                 \frac{v_l}{R-\frac{W}{2}}\Delta t \\
#                 \end{bmatrix}
# \end{align}
#
# ### 2. Identify the points around which we linearize our system, $(\bar{u}, \bar{x})$
#
# Because we are tracking a trajectory, we want to linearize around the trajectory we are trying to track. That means $\bar{u}$ is the control input associated with the trajectory, which means solving for the feed forward inputs. Specifically, that means we need to compute the $v_l$ and $v_r$ that would follow the trajectory at the point $\bar{x}$. To do this we must pick velocities that make instantaneous turning radius $R$ equal the instantaneous radius of the trajcetory at $\bar{x}$, and make the linear velocity at $\bar{x}$ equal the instantaneous linear velocity of the robot center $v$. To do this, we go back to our basic kinematics equations which rely on the fact that all points on the robot (center, left wheel, right wheel) have the same rotational velocity $\omega$ around the ICC.
#
# \begin{align}
#   \omega = \frac{v}{R} &= \frac{v_l}{R-\frac{W}{2}} \\
#   \frac{v}{R}\bigg(R - \frac{W}{2}\bigg) &= v_l \\
#   \omega = \frac{v}{R} &= \frac{v_r}{R+\frac{W}{2}} \\
#   \frac{v}{R}\bigg(R + \frac{W}{2}\bigg) &= v_r \\
# \end{align}
#
# Using these equations we can solve for the velocities of the wheels, which together make up $\bar{u}$. We just need the $R$ and $v$. These should be derived from the equation of the trajectory we are tracking. These are well studied equations, for which [a proof can be found other places on the internet](http://mathworld.wolfram.com/Curvature.html).
#
# $$ R = \frac{\dot{x}\ddot{y}-\dot{y}\ddot{x}}{{\big({\dot{x}}^2 + {\dot{y}}^2\big)}^{\frac{3}{2}}} = \frac{(c_1+2c_2t+3c_3t^2+4c_4t^3)(2d_2+6d_3t+12d_4t^2) - (d_1+2d_2t+3d_3t^3+4d_4t^3)(2c_2+6c_3t+12c_4t^2)}{{\big({(c_1+2c_2t+3c_3t^2+4c_4t^3)}^2 + {(d_1+2d_2t+3d_3t^3+4d_4t^3)}^2\big)}^{\frac{3}{2}}} $$
#
# $$ v = \sqrt{{(\dot{x})}^2 + {(\dot{y})}^2}  = \sqrt{{(c_1+2c_2t+3c_3t^2+4c_4t^3)}^2 + {(d_1+2d_2t+3d_3t^3+4d_4t^3)}^2} $$
#
# We can plug in the coefficients of our polynomials and get values for $v$ and $R$. Then, we can plus these into the equations just above and get the feed forward wheel velocities.
#
# ### 3. Write the linearized dynamics around $\bar{x}$ as $\dot{\vec{x}} \approx A\delta_x + B\delta_u$, where $\delta_x = (\vec{x} - \bar{x})$ and $\delta_u = (\vec{u} - \bar{u})$
#
# To do this, we need the partial derivitive matrixes $A$ and $B$.
#
# $$ A = \begin{bmatrix}
#          \frac{\partial f_1}{\partial x}\big|_{\bar{x}\bar{u}} &
#          \frac{\partial f_1}{\partial y}\big|_{\bar{x}\bar{u}} &
#          \frac{\partial f_1}{\partial \theta}\big|_{\bar{x}\bar{u}} \\
#          \frac{\partial f_2}{\partial x}\big|_{\bar{x}\bar{u}} &
#          \frac{\partial f_2}{\partial y}\big|_{\bar{x}\bar{u}} &
#          \frac{\partial f_2}{\partial \theta}\big|_{\bar{x}\bar{u}} \\
#          \frac{\partial f_3}{\partial x}\big|_{\bar{x}\bar{u}} &
#          \frac{\partial f_3}{\partial y}\big|_{\bar{x}\bar{u}} &
#          \frac{\partial f_3}{\partial \theta}\big|_{\bar{x}\bar{u}} \\
#        \end{bmatrix}
#      = \begin{bmatrix}
#         0 & 0 & R\bigg(\cos\Big(\frac{\bar{v}_r-\bar{v}_l}{W}\Delta t - \bar{\theta}\Big) - \cos(\bar{\theta})\bigg) \\
#         0 & 0 & -R\bigg(\sin\Big(\frac{\bar{v}_r-\bar{v}_l}{W}\Delta t - \bar{\theta}\Big) + \sin(\bar{\theta})\bigg) \\
#         0 & 0 & 0 \\
#        \end{bmatrix}
# $$
#
# $$ B = \begin{bmatrix}
#          \frac{\partial f_1}{\partial v_l}\big|_{\bar{x}\bar{u}} &
#          \frac{\partial f_1}{\partial v_r}\big|_{\bar{x}\bar{u}} \\
#          \frac{\partial f_2}{\partial v_l}\big|_{\bar{x}\bar{u}} &
#          \frac{\partial f_2}{\partial v_r}\big|_{\bar{x}\bar{u}} \\
#          \frac{\partial f_3}{\partial v_l}\big|_{\bar{x}\bar{u}} &
#          \frac{\partial f_3}{\partial v_r}\big|_{\bar{x}\bar{u}} \\
#        \end{bmatrix}
#      = \begin{bmatrix}
#           R\cos\Big(\frac{(\bar{v}_r-\bar{v}_l)\Delta t}{W}-\bar{\theta}\Big)\frac{\Delta t}{W} &
#           -R\cos\Big(\frac{(\bar{v}_r-\bar{v}_l)\Delta t}{W}-\bar{\theta}\Big)\frac{\Delta t}{W} \\
#           -R\sin\Big(\frac{(\bar{v}_r-\bar{v}_l)\Delta t}{W}-\bar{\theta}\Big)\frac{\Delta t}{W} &
#           R\sin\Big(\frac{(\bar{v}_r-\bar{v}_l)\Delta t}{W}-\bar{\theta}\Big)\frac{\Delta t}{W} \\
#           \frac{\Delta t}{R-\frac{W}{2}} &
#           0 \\
#        \end{bmatrix}
# $$
#
# ### 4. Check if our system is controllable by looking at the rank of the controllability matrix $C = [B, AB, A^2B, \dots, A^{n-1}B]$
#
# We have three state variables so $n = 3$, which means $C = [B, AB, A^2B]$.
#
# $$
# AB = \begin{bmatrix}
#        R\bigg(\cos\Big(\frac{v_r-v_l}{W}\Delta t - \theta\Big) - \cos(\theta)\bigg)\frac{\Delta t}{R-\frac{W}{2}} & 0 \\
#        -R\bigg(\sin\Big(\frac{v_r-v_l}{W}\Delta t - \theta\Big) + \sin(\theta)\bigg)\frac{\Delta t}{R-\frac{W}{2}} & 0 \\
#        0 & 0 \\
#      \end{bmatrix}
# $$
#
#
# $$
# A^2B =
#      \begin{bmatrix}
#        0 & 0 & 0\\
#        0 & 0 & 0\\
#        0 & 0 & 0\\
#      \end{bmatrix}
#      * B
#      = \begin{bmatrix}
#        0 & 0 & 0\\
#        0 & 0 & 0\\
#        0 & 0 & 0\\
#      \end{bmatrix}
# $$
#
# $$ C = \begin{bmatrix}
#          \begin{bmatrix}
#            R\cos\Big(\frac{(\bar{v_r}-\bar{v_l})\Delta t}{W}-\theta\Big)\frac{\Delta t}{W} &
#            -R\cos\Big(\frac{(\bar{v_r}-\bar{v_l})\Delta t}{W}-\theta\Big)\frac{\Delta t}{W} \\
#            -R\sin\Big(\frac{(\bar{v_r}-\bar{v_l})\Delta t}{W}-\theta\Big)\frac{\Delta t}{W} &
#            R\sin\Big(\frac{(\bar{v_r}-\bar{v_l})\Delta t}{W}-\theta\Big)\frac{\Delta t}{W} \\
#            \frac{\Delta t}{R-\frac{W}{2}} &
#            0 \\
#          \end{bmatrix} &
#          \begin{bmatrix}
#            R\bigg(\cos\Big(\frac{v_r-v_l}{W}\Delta t - \theta\Big) - \cos(\theta)\bigg)\frac{\Delta t}{R-\frac{W}{2}} & 0 \\
#            -R\bigg(\sin\Big(\frac{v_r-v_l}{W}\Delta t - \theta\Big) + \sin(\theta)\bigg)\frac{\Delta t}{R-\frac{W}{2}} & 0 \\
#            0 & 0 \\
#          \end{bmatrix} &
#          \begin{bmatrix}
#            0 & 0 & 0\\
#            0 & 0 & 0\\
#            0 & 0 & 0\\
#          \end{bmatrix}
#        \end{bmatrix}
# $$
#
# What is the rank of this matrix? It seems to depend on specific values of $x, y, \theta$.
#
# ### 5. Pick cost parameters $Q$ and $R$
#
# These need to be tuned on the simulation or real system, but the identity matrices $I$ are good starting points.
#
# ### 6. Solve for $K$ given $LQR(A, B, Q, R)$
#
# We want to minimize the quadratice cost function $J$, which is defined as follows.
#
# $$ J = \sum_0^N(\vec{x}_t - \bar{x}_t)^TQ(\vec{x}_t - \bar{x}_t) + \sum_0^N(\vec{u}_t - \bar{u}_t)^TR(\vec{u}_t - \bar{u}_t) $$
#
# We can do this with least squares, or dynamics programming. DP is more efficient O(Nn^3). N is some finite horizon, and n is the number of state dimensions (3 for us).
#
# maybe we compute K once instead of at every time step. could be consistent within one motion primitive.
#
# ### 7. Apply our new controller of the form $\vec{u} = -K(\vec{x} - \bar{x}) + \bar{u}$
#

# In[27]:

from math import atan2
import scipy.linalg

# source: http://www.kostasalexis.com/lqr-control.html
def dlqr(A,B,Q,R):
    """Solve the discrete time lqr controller.

    p[k+1] = A p[k] + B u[k]

    cost = sum p[k].T*Q*p[k] + u[k].T*R*u[k]
    """
    #ref Bertsekas, p.151

    #first, try to solve the ricatti equation
    P = np.matrix(scipy.linalg.solve_discrete_are(A, B, Q, R))

    #compute the LQR gain
    K = np.matrix(scipy.linalg.inv(B.T*P*B+R)*(B.T*P*A))

    eigVals, eigVecs = scipy.linalg.eig(A-B*K)

    return K, P, eigVals

def follow_plan(q_0, waypoints, P_1, P_2, P_3):
    traj = TrajPlan()
    traj.solve(waypoints)

    dt = 0.01
    x = q_0[0]
    y = q_0[1]
    theta = q_0[2]
    vl = q_0[3]
    vr = q_0[3]
    actual_vl = vl
    actual_vr = vr
    v_acc = 2 * dt
    W = 0.0633
    T = np.arange(0, traj.get_t_f()+dt, dt)
    x_bar_list = []
    y_bar_list = []
    x_list = []
    y_list = []
    vl_list = []
    vr_list = []
    actual_vl_list = []
    actual_vr_list = []
    for t in T:
        x_bar = [1, t, pow(t,2), pow(t,3), pow(t,4), pow(t,5), 0, 0, 0, 0, 0, 0] @ traj.get_coeff()
        dx_bar = [0, 1, 2*t, 3*pow(t,2), 4*pow(t,3), 5*pow(t,4), 0, 0, 0, 0, 0, 0] @ traj.get_coeff()
        ddx_bar = [0, 0, 0, 0, 0, 0, 0, 0, 2, 6*t, 12*pow(t,2), 20*pow(t,3)] @ traj.get_coeff()
        y_bar = [0, 0, 0, 0, 0, 0, 1, t, pow(t,2), pow(t,3), pow(t,4), pow(t,5)] @ traj.get_coeff()
        dy_bar = [0, 0, 0, 0, 0, 0, 0, 1, 2*t, 3*pow(t,2), 4*pow(t,3), 5*pow(t,4)] @ traj.get_coeff()
        ddy_bar = [0, 0, 0, 0, 0, 0, 0, 0, 2, 6*t, 12*pow(t,2), 20*pow(t,3)] @ traj.get_coeff()
        theta_bar = atan2(dy_bar, dx_bar)

        # full forward kinematics
        if vr - vl < 1e-5:
            x = x + cos(theta) * vl * dt
            y = y + sin(theta) * vl * dt
        else:
            R = W*(vl + vr)/(2*(vr - vl))
            x = x - R * (sin((vr-vl)*dt/W - theta) + sin(theta))
            y = y - R * (cos((vr-vl)*dt/W - theta) - cos(theta))
            theta = theta + vl / (R - W/2) * dt

        # compute instanteneous Radius
        R_bar = (dx_bar*ddy_bar - dy_bar*ddy_bar)/pow((pow(dx_bar, 2) + pow(dy_bar, 2)), 3/2)

        # feed forward inputs
        v_bar = np.sqrt(dx_bar*dx_bar + dy_bar*dy_bar)
        vl_bar = v_bar/R_bar*(R_bar-W/2)
        vr_bar = v_bar/R_bar*(R_bar+W/2)

        A = np.array([[0, 0, R_bar*(cos((vr_bar - vl_bar)*dt/W - theta_bar) - cos(theta_bar))],
                      [0, 0, -R_bar*(sin((vr_bar - vl_bar)*dt/W - theta_bar) + sin(theta_bar))],
                      [0, 0, 0]])

        B = np.array([[R_bar*cos((vr_bar - vl_bar)*dt/W - theta_bar)*dt/W, -R_bar*cos((vr_bar - vl_bar)*dt/W - theta_bar)*dt/W],
                      [-R_bar*sin((vr_bar - vl_bar)*dt/W - theta_bar)*dt/W, R_bar*sin((vr_bar - vl_bar)*dt/W - theta_bar)*dt/W],
                      [dt/(R_bar-W/2), 0]]);

        Q= np.eye(3);
        R = np.eye(2);

        K, P, eigs = dlqr(A, B, Q, R)
        eigs = np.linalg.eig(A - B*K)
#         info("eigs", eigs[0])
#         debug("K", K)
        x_vec = np.array([[x],[y],[theta]])
        x_bar_vec = np.array([[x_bar],[y_bar],[theta_bar]])
        u = -K * (x_vec - x_bar_vec) + np.array([[vl_bar],[vr_bar]]);
        vl = u[0,0]
        vr = u[1,0]

        # simple acceleration model
        if vl < actual_vl:
            actual_vl = max(vl, actual_vl - v_acc)
        elif vl > actual_vl:
            actual_vl = min(vl, actual_vl + v_acc)
        if vr < actual_vr:
            actual_vr = max(vr, actual_vr - v_acc)
        elif vr > actual_vr:
            actual_vr = min(vr, actual_vr + v_acc)

        x_bar_list.append(x_bar)
        y_bar_list.append(y_bar)
        x_list.append(x)
        y_list.append(y)
        vr_list.append(vr)
        vl_list.append(vl)
        actual_vr_list.append(actual_vr)
        actual_vl_list.append(actual_vl)

    plt.figure(figsize=(5, 5))
    CELL_COUNT = 3
    plt.scatter(x_bar_list, y_bar_list, marker='.', linewidth=0, c='black', label='desired traj')
    plt.scatter(x_list, y_list, marker='.', linewidth=0, label='robot traj')
    plt.xlim(0, CELL_COUNT * 0.18)
    plt.ylim(0, CELL_COUNT * 0.18)
    plt.xticks(np.arange(CELL_COUNT+1)*0.18)
    plt.yticks(np.arange(CELL_COUNT+1)*0.18)
    plt.grid(True)
    plt.gca().set_axisbelow(True)
    plt.xlabel("X")
    plt.ylabel("Y")
    plt.title("LQR Trajectory Tracking")
    plt.legend(bbox_to_anchor=(1,1), loc=2)

    plt.figure()
    plt.plot(vr_list, label="vr")
    plt.plot(vl_list, label="vl")
    plt.plot(actual_vr_list, label="actual vr")
    plt.plot(actual_vl_list, label="actual vl")
    plt.legend(bbox_to_anchor=(1,1), loc=2)

# In[28]:

LOG_LVL=1
robot_q_0 = (0.08, 0.18, pi/2, 0.3)
traj = [(0, WayPoint(0.09, 0.18, pi/2, 0.5)), (0.5, WayPoint(0.18, 0.27, 0, 0.35))]
follow_plan(robot_q_0, traj, test_P_1, test_P_2, test_P_3)
plt.show()

# In[29]:

LOG_LVL=1
robot_q_0 = (0.07, 0.18, pi/2, 0.2)
traj = [(0, WayPoint(0.09, 0.18, pi/2, 0.2)), (0.5, WayPoint(0.09, 0.36, pi/2, 0.2))]
follow_plan(robot_q_0, traj, test_P_1, test_P_2, test_P_3)
plt.show()

# # Tuning new PIDs

# In[30]:

import csv
reader = csv.reader(open("./pid_data.csv", 'r'))
setpoints = []
speeds = []
for row in reader:
    setpoints.append(float(row[0]))
    speeds.append(float(row[1]))

t = np.arange(0, len(setpoints)/100, 0.01)
plt.plot(t, setpoints, label="setpoint")
plt.plot(t, speeds, label="actual speed")
plt.xlabel("time (s)")
plt.ylabel("speed (m/s)")
plt.title("PID Performance")
plt.show()

# In[ ]:

docs/ipynb_gen_py/Time Optimal Smartmouse Controls.py


import numpy

class Polynomial(object):
    def __init__(self, coefficients):
        self.coeff = coefficients

    def __call__(self, x):
        """Evaluate the polynomial."""
        s = 0
        for i in range(len(self.coeff)):
            s += self.coeff[i]*x**i
        return s

    def __add__(self, other):
        """Return self + other as Polynomial object."""
        # Two cases:
        #
        # self:   X X X X X X X
        # other:  X X X
        #
        # or:
        #
        # self:   X X X X X
        # other:  X X X X X X X X

        # Start with the longest list and add in the other
        if len(self.coeff) > len(other.coeff):
            result_coeff = self.coeff[:]  # copy!
            for i in range(len(other.coeff)):
                result_coeff[i] += other.coeff[i]
        else:
            result_coeff = other.coeff[:] # copy!
            for i in range(len(self.coeff)):
                result_coeff[i] += self.coeff[i]
        return Polynomial(result_coeff)

    def __mul__(self, other):
        c = self.coeff
        d = other.coeff
        M = len(c) - 1
        N = len(d) - 1
        result_coeff = numpy.zeros(M+N+1)
        for i in range(0, M+1):
            for j in range(0, N+1):
                result_coeff[i+j] += c[i]*d[j]
        return Polynomial(result_coeff)

    def differentiate(self):
        """Differentiate this polynomial in-place."""
        for i in range(1, len(self.coeff)):
            self.coeff[i-1] = i*self.coeff[i]
        del self.coeff[-1]

    def derivative(self):
        """Copy this polynomial and return its derivative."""
        dpdx = Polynomial(self.coeff[:])  # make a copy
        dpdx.differentiate()
        return dpdx

    def __str__(self):
        s = ''
        for i in range(0, len(self.coeff)):
            if self.coeff[i] != 0:
                s += ' + %g*x^%d' % (self.coeff[i], i)
        # Fix layout
        s = s.replace('+ -', '- ')
        s = s.replace('x^0', '1')
        s = s.replace(' 1*', ' ')
        s = s.replace('x^1 ', 'x ')
        #s = s.replace('x^1', 'x') # will replace x^100 by x^00
        if s[0:3] == ' + ':  # remove initial +
            s = s[3:]
        if s[0:3] == ' - ':  # fix spaces for initial -
            s = '-' + s[3:]
        return s

    def simplestr(self):
        s = ''
        for i in range(0, len(self.coeff)):
            s += ' + %g*x^%d' % (self.coeff[i], i)
        return s

def test_Polynomial():
    p1 = Polynomial([1, -1])
    p2 = Polynomial([0, 1, 0, 0, -6, -1])
    p3 = p1 + p2
    p3_exact = Polynomial([1, 0, 0, 0, -6, -1])
    msg = 'p1 = %s, p2 = %s\np3=p1+p2 = %s\nbut wrong p3 = %s'%\
          (p1, p2, p3_exact, p3)
    assert p3.coeff == p3_exact.coeff, msg
    # Note __add__ applies lists only, here with integers, so
    # == for comparing lists is not subject to round-off errors

    p4 = p1*p2
    # p4.coeff becomes a numpy array, see __mul__
    p4_exact = Polynomial(numpy.array([0,  1, -1,  0, -6,  5,  1]))
    msg = 'p1 = %s, p2 = %s\np4=p1*p2 = %s\ngot wrong p4 = %s'%\
          (p1, p2, p4_exact, p4)
    assert numpy.allclose(p4.coeff, p4_exact.coeff, rtol=1E-14), msg

    p5 = p2.derivative()
    p5_exact = Polynomial([1, 0, 0, -24, -5])
    msg = 'p2 = %s\np5 = p2.derivative() = %s\ngot wrong p5 = %s'%\
          (p2, p5_exact, p5)
    assert p5.coeff == p5_exact.coeff, msg

    p6 = Polynomial([0, 1, 0, 0, -6, -1])  # p2
    p6.differentiate()
    p6_exact = p5_exact
    msg = 'p6 = %s\p6.differentiate() = %s\ngot wrong p6 = %s'%\
          (p2, p6_exact, p6)
    assert p6.coeff == p6_exact.coeff, msg

if __name__ == '__main__':
    import sys
    if len(sys.argv) >= 2 and sys.argv[1] == 'verify':
        test_Polynomial()


# coding: utf-8
import numpy

def calculateStep(a, b, n):
    return (b-a)/n # retorna o valor de h, onde a é o limite inferior, b o superior e n é a quantidade de intervalos

def trapezeRule(step, data):
    return (step/2) * (data[0] + 2 * sum(data[1:-1]) + data[-1]) #Calcula o método utilizando h/2 * (y0 + (y1 + y2 + y3 + ... + yn-1) + yn)

def simpsonByThree(step, data):
    even = [data[i] for i in range(2,len(data) - 1, 2)] #Separa os números que estão nos índices pares do vetor de valores
    odd = [data[i] for i in range(1,len(data) - 1, 2)]  #Separa os números que estão nos índices ímpares do vetor de valores

    return (step/3) * (data[0] + 4 * sum(odd) + 2 * sum(even) + data[-1]) #Calcula o método utilizando a fórmula h/3 * (y0 + 4 * (y1 + y3 + y5 + ... + y2n-1) + 2 * (y2 + y4 + y6 + ... + y2n) + yn)

option = 0
i = 0
j = 0
k = 0

while (k == 0):

    while (i == 0):
        i = int(input("\nEntre com 1 para calcular a partir de uma funcao f(x) e limites\nEntre com 2 para calcular a partir de uma tabela de dados\nSua opcao: "))
        if i == 1:
            a = float(input("\nEntre com o limite inferior: "))
            b = float(input("Entre com o limite superior: "))
            interval = float(input("Entre com o numero de intervalos desejados: "))
            func = input("Entre com a funcao desejada no seguinte formato - lambda x: f(x) \n-> ")
            funcFinal = eval(func)
            step = calculateStep(a, b, interval)
            values = [funcFinal(i) for i in numpy.arange(a, b + step, step)]
        elif i == 2:
            step = float(input("Insira o valor do passo: "))
            print("Insira os valores de f(x) separados por espaco: ")
            values = list(map(float,input().split()))
        else:
            i = 0

    while(j == 0):
        j = int(input("\nEntre com 1 para o metodo do trapezio\nEntre com 2 para 1/3 de Simpson\nSua opcao: "))
        if j == 1:
            option = 1
        elif j == 2:
            option = 2
        else:
            j = 0

    if option == 1:
        print("\n\nO resultado da integral definida pelo metodo dos trapezios para %d intervalos e f(x) ou tabela fornecidos sera\n-> %.6f" % ((len(values) -1 ), trapezeRule(step, values)))
    else:
        print("\n\nO resultado da integral definida pelo metodo de 1/3 de Simpson para %d intervalos e f(x) ou tabela fornecidos sera\n-> %.6f" % ((len(values) -1 ), simpsonByThree(step, values)))

    if k == 0:
        x = int(input("\n\nDeseja calcular uma nova integral?\n1 - Sim, calcular outra integral\n2 - Encerrar o programa\nInsira sua opcao: "))
        if x == 1:
            i = 0
            j = 0
            k = 0
        else:
            k = 1


"""
Function to return the theta (dimensionless temperature) profile at a certain
dimensionless point (r) in a 1D solid sphere, cylinder, or slab shape.

References:
1) Recktenwald 2006
2) Bergman, Lavine, Incropera, Dewitt 2011 from Ch. 5, pg.299-304
"""

# Modules and Other Required Functions
#------------------------------------------------------------------------------

import numpy as np
import scipy.special as sp
from funcRoots import roots

# First and Second Terms of the Theta Function
#------------------------------------------------------------------------------

def funcCn(root, b):
    """
    First term in the theta function
    root = root from the zeta, Bi equation
    b = shape factor where 2 sphere, 1 cylinder, 0 slab
    """
    if b == 2:
        Cn = 4*(np.sin(root)-root*np.cos(root)) / (2*root-np.sin(2*root))
    elif b == 1:
        Cn = (2/root) * (sp.j1(root) / (sp.j0(root)**2 + sp.j1(root)**2))
    elif b == 0:
        Cn = (4*np.sin(root)) / (2*root + np.sin(2*root))
    return Cn

def funcDn(r, root, b):
    """
    Second term in the theta function
    root = root from the zeta, Bi equation
    b = shape factor where 2 sphere, 1 cylinder, 0 slab
    """
    if b == 2:
        Dn = (1/(root*r)) * np.sin(root*r)
    elif b == 1:
        Dn = sp.j0(root * r)
    elif b == 0:
        Dn = np.cos(root * r)
    return Dn

# Theta Function
#------------------------------------------------------------------------------

def theta(r, b, z, Bi, Fo):
    """
    Dimensionless temperature for analytical solution of 1D transient heat
    conduction for a solid sphere, cylinder, or slab.
    r = dimensionaless length term to evaluate theta, (-)
    b = shape factor where 2 sphere or 1 cylinder or 0 slab, (-)
    z = range of zeta values to evaluate zeta, Bi equation for positive roots
    Bi = Biot number h*L/k, (-)
    Fo = Fourier number alpha*t/L^2, (-)
    """

    rts = roots(z, b, Bi)   # positive roots of the zeta, Bi equation
    n = len(rts)            # number of positive roots

    # initial dimensionless temperature at first root
    theta = funcCn(rts[0], b)*np.exp(-rts[0]**2 * Fo)*funcDn(r, rts[0], b)

    # summation of theta for the remaining roots
    for i in range(1, n):
        dTheta_o = funcCn(rts[i], b)*np.exp(-rts[i]**2 * Fo)*funcDn(r, rts[i], b)
        theta = theta + dTheta_o

    return theta    # theta temperature profile evaluated at r


import matplotlib.pyplot as plt
import numpy as np
from scipy.fftpack import fft, fftshift
from scipy import signal

M = 32
N = 128
hN = N//2
hM = M//2
fftbuffer = np.zeros(N)
w = signal.blackman(M)

plt.figure(1, figsize=(9.5, 6))

plt.subplot(3,2,1)
plt.plot(np.arange(-hM, hM), w, 'b', lw=1.5)
plt.axis([-hM, hM-1, 0, 1.05])
plt.title('w1, M=32')

fftbuffer = np.zeros(N)
fftbuffer[:hM] = w[hM:]
fftbuffer[N-hM:] = w[:hM]
X = fft(fftbuffer)
mX = 20*np.log10(abs(fftshift(X)))
plt.subplot(3,2,3)
plt.plot(np.arange(-hN, hN), mX-max(mX), 'r', lw=1.5)
plt.axis([-hN//2,hN//2,-80,1])
plt.title('mW1')

pX = np.angle(fftshift(X))
plt.subplot(3,2,5)
plt.plot(np.arange(-hN, hN), pX, 'c', lw=1.5)
plt.axis([-hN,hN-1,-np.pi,np.pi])
plt.title('pW1')

M = 31
N = 128
hN = N//2
hM = (M+1)//2
fftbuffer = np.zeros(N)
w = signal.blackman(M)

plt.subplot(3,2,2)
plt.plot(np.arange(-hM, hM-1), w, 'b', lw=1.5)
plt.axis([-hM, hM, 0, 1.05])
plt.title('w2, M=31')

fftbuffer = np.zeros(N)
fftbuffer[:hM] = w[hM-1:]
fftbuffer[N-hM+1:] = w[:hM-1]
X = fft(fftbuffer)
mX = 20*np.log10(abs(fftshift(X)))
plt.subplot(3,2,4)
plt.plot(np.arange(-hN, hN), mX-max(mX), 'r', lw=1.5)
plt.axis([-hN/2,hN/2-1,-80,1])
plt.title('mW2')

pX = np.angle(fftshift(X))
plt.subplot(3,2,6)
plt.plot(np.arange(-hN, hN), pX, 'c', lw=1.5)
plt.axis([-hN,hN-1,-np.pi,np.pi])
plt.title('pW2')

plt.tight_layout()
plt.savefig('blackman-even-odd.png')
plt.show()


from sklearn.neighbors import KernelDensity
import numpy as np
import scipy as sp

def norm_cdf_int(mu, std, LB, UB):
    """ Return P(LB < X < UB) for X Normal(mu, std) """
    rv = sp.stats.norm(mu, std)
    return rv.cdf(UB) - rv.cdf(LB)

def norm_cdf_int_approx(mu, std, LB, UB):
    """
    Return P(LB < X < UB) for X Normal(mu, std) using approximation of Normal CDF

    Input: All inputs as 1-D arrays
    """
    l = normal_cdf_approx((LB - mu)/std)
    u = normal_cdf_approx((UB - mu)/std)
    return u - l

def normal_cdf_approx(x):
    """
    Approximation of standard normal CDF

    Input: x = array

    Polynomial approximation from Abramowitz and Stegun p. 932
    http://people.math.sfu.ca/~cbm/aands/frameindex.htm

    Absolute error < 7.5*10^-8
    """
    p = 0.2316419
    b = [0.319381530, -0.356563782, 1.781477937, -1.821255978, 1.330274429]

    xx = abs(x) # Approximation only works for x > 0, return 1 - p otherwise

    t = 1/(1 + p*xx)
    Z = (1/(np.sqrt(2*np.pi)))*np.exp(-(x*x)/2)
    pol = b[0]*t + b[1]*(t**2) + b[2]*(t**3) + b[3]*(t**4) + b[4]*(t**5)

    prob = 1 - Z*pol # For x > 0
    prob[x < 0] = 1 - prob[x < 0] # Change when x < 0

    return prob

def mode_from_samples(samples, bandwidth_fraction = 0.1):
    """
    Compute the mode for each set of samples in 'samples'

    Using kernel density estimation

    Input:
    samples -- m x n array with n samples for each of m univariate random variables
    bandwidth_fraction -- kde will use bandwidth = [range of dataseries (max - min)] * bandwidth_fraction
    """

    # Function to optimize for finding the mode
    # -- the kernel density estimator
    def optfun(x, *args):
        kde = args[0]
        return -kde.score_samples(x.reshape(-1, 1))

    mode = np.zeros(samples.shape[0])

    for i in range(samples.shape[0]):
        data = samples[i].T
        min_x, max_x = data.min(), data.max()
        bandwidth = bandwidth_fraction*(max_x - min_x)

        # Fit kde
        kde = KernelDensity(kernel='gaussian', bandwidth=bandwidth).fit(data)

        # Find argmax of density
        args = (kde, )
        bounds = [(min_x, max_x)]

        res = sp.optimize.differential_evolution(optfun, bounds = bounds, args = args)

        mode[i] = res.x[0]

    return mode

def trunc_norm_moments_approx_corrfree(mu, sigma, LB, UB, inf_num = 1E100):
    """
    Correlation free approximation of truncated moments of multivariate Gaussian

    If X~N(mu, sigma), compute expectation and variance of X | LB <= X <= UB

    Input:
    mu, LB, UB : 1D numpy arrays
    sigma : numpy matrix
    inf_num : inf values are replaced with this number in calculations

    Returns:
    tmu, tvar (expectation and variance of truncated variable)
    """

    s2 = np.diag(sigma)
    s = np.sqrt(s2)
    a = (LB - mu )/s
    b = (UB - mu )/s

    # Replace inf and -inf by numbers
    a[a == float('inf')] = inf_num
    a[a == float('-inf')] = -inf_num
    b[b == float('inf')] = inf_num
    b[b == float('-inf')] = -inf_num

    phi_a = sp.stats.norm.pdf(a)
    phi_b = sp.stats.norm.pdf(b)
    PHI_diff = normal_cdf_approx(b) - normal_cdf_approx(a)

    tmu = mu + s*(phi_a - phi_b)/PHI_diff
    tvar = s2*(1 + (a*phi_a - b*phi_b)/PHI_diff - ((phi_a - phi_b)/PHI_diff)**2)

    return tmu, tvar


import numpy
from scipy.optimize import curve_fit
from scipy.stats import linregress

def exponential(x, a, b, c):
    return a * numpy.exp(b * x) + c

def fit_exponential(series, *, p0=(1.0, 0.01, 0.0), n0: float = None):
    """
    Fits an exponential to a series. First attempts an exponential fit in linear space using p0, then falls back to a
    fit in log space to attempt to find parameters p0 for a linear fit; if all else fails returns the linear fit.

    """

    if n0 is None:
        fit_fn = exponential
    else:
        def exponential_constrain_n0(x, a, b):
            return a * numpy.exp(b * x) + n0

        fit_fn = exponential_constrain_n0
        p0 = p0[:2]

    try:
        popt, pcov = curve_fit(fit_fn,
                               series.index,
                               series.values,
                               p0=p0,
                               maxfev=10000,
                               bounds=([0., 0., 0.], numpy.inf) if n0 is None else ([0., 0.], numpy.inf))

        if n0 is not None:
            popt = tuple(popt) + (n0,)
    except RuntimeError:
        pass
    else:
        slope = popt[1]
        intercept = numpy.log(1 / popt[0]) / slope
        N0 = popt[2]

        if slope >= 0:
            snr = signal_noise_ratio(series, *popt)
            return slope, intercept, N0, snr, False

    log_series = numpy.log(series[series > 0] - (n0 or 0.0))

    slope, c, *__ = linregress(log_series.index, log_series.values)
    intercept = - c / slope

    if n0 is None:
        p0 = (numpy.exp(c), slope, 0.0)
    else:
        p0 = (numpy.exp(c), slope)

    try:
        popt, pcov = curve_fit(fit_fn,
                               series.index,
                               series.values,
                               p0=p0,
                               maxfev=10000,
                               bounds=([0., 0., 0.], numpy.inf) if n0 is None else ([0., 0.], numpy.inf))

        if n0 is not None:
            popt = tuple(popt) + (n0,)
    except RuntimeError:
        snr = signal_noise_ratio(series, c, slope, 0.0)
        return slope, intercept, (n0 or 0.0), snr, True
    else:
        slope = popt[1]
        intercept = numpy.log(1 / popt[0]) / slope
        n0 = popt[2]
        snr = signal_noise_ratio(series, *popt)
        return slope, intercept, n0, snr, False

def signal_noise_ratio(series, *popt):
    fit = exponential(series.index, *popt)
    return numpy.var(fit) / numpy.var(series.values - fit)


import numpy as np

class CoordSystem(object):

    def __init__(self, a0, b0, c0, coord_type='R', method=1):
        self.coord_type = coord_type

        if method == 1:
            self.compute_matrix1(a0, b0, c0)
        elif method == 2:
            self.compute_matrix2(a0, b0, c0)

    @property
    def origin(self):
        return self._origin

    @property
    def M(self):
        return self._M

    def compute_matrix1(self, a0, b0, c0):
        self._origin = a0
        e2 = np.cross(b0 - a0, c0 - a0)
        e2 /= np.linalg.norm(e2)
        e1 = np.cross(e2, b0 - a0)
        e1 /= np.linalg.norm(e1)
        e3 = np.cross(e1, e2)
        self._M = np.array([e1, e2, e3])

    def compute_matrix2(self, a0, b0, c0):
        self._origin = a0
        e1 = b0 - a0
        e1 /= np.linalg.norm(e1)
        e3 = np.cross(e1, c0 - a0)
        e3 /= np.linalg.norm(e3)
        e2 = np.cross(e3, e1)
        self._M = np.array([e1, e2, e3])

    def get_xyz(self, xyz0, is_vector=False):

        if not is_vector:
            xyz0 = xyz0 - self._origin

        xyz = np.dot(xyz0, self._M.T)

        if self.coord_type == 'C':
            return np.array(cart2cyl(*xyz))
        elif self.coord_type == 'S':
            return np.array(cart2sph(*xyz))

        return xyz

    def get_xyz0(self, xyz, is_vector=False):

        if self.coord_type == 'C':
            xyz = cyl2cart(*xyz)
        elif self.coord_type == 'S':
            xyz = sph2cart(*xyz)

        if is_vector:
            return np.dot(xyz, self._M)
        else:
            return self._origin + np.dot(xyz, self._M)

def cart2cyl(x, y, z):
    theta = np.arctan2(y, x)
    rho = np.hypot(x, y)
    return rho, theta, z

def cyl2cart(rho, theta, z):
    x = rho * np.cos(theta)
    y = rho * np.sin(theta)
    return x, y, z

def cart2sph(x, y, z):
    hxy = np.hypot(x, y)
    r = np.hypot(hxy, z)
    el = np.arctan2(z, hxy)
    az = np.arctan2(y, x)
    return r, az, el

def sph2cart(r, az, el):
    rcos_theta = r * np.cos(el)
    x = rcos_theta * np.cos(az)
    y = rcos_theta * np.sin(az)
    z = r * np.sin(el)
    return x, y, z


#!/usr/bin/env python3
import numpy as np
from functools import reduce

def matrix_power(a, n):
    if n>=0:
        return reduce(np.matmul, (a for _ in range(n)), np.eye(a.shape[0]))
    else:
        inv = np.linalg.inv(a)
        return reduce(np.matmul, (inv for _ in range (-n)))

def main():
    a = np.array([1,2,3,4])
    a = a.reshape((2,2))
    print(a)
    print(matrix_power(a,2))

if __name__ == "__main__":
    main()

"""
def matrix_power(a, n):
    power = a
    if n<0:
        return matrix_power(np.linalg.inv(a),-n)
    if n==0:
        return np.eye(a.shape[0])
    for i in range(n-1):
        power = np.matmul(power,a)
    return power
"""


import math
import numpy as np

def mean(X):
    i = 0.0
    s = 0.0
    for xi in X:
        s += xi
        i += 1.0
    return s / i

def var(X):
    mX = mean(X)
    i = 0.0
    s = 0.0
    for xi in X:
        s += (xi - mX)**2
        i += 1.0
    return s / (i - 1.0)

def std(X):
    vX = var(X)
    return math.sqrt(vX)

def cov(X, Y):
    if len(X) != len(Y):
        raise ValueError('Inputs need same Dimensions')
    mX = mean(X)
    mY = mean(Y)
    i = 0.0
    s = 0.0
    for xi, yi in zip(X, Y):
        s += (xi - mX) * (yi - mY)
        i += 1.0
    return s / (i - 1.0)

def covmat(*params):
    dims = len(params)

    matrix = []

    for i in xrange(dims):
        row = []
        for j in xrange(dims):
            row.append(cov(params[i], params[j]))
        matrix.append(row)

    return matrix

if __name__ == '__main__':
    X = [1, -1, 4]
    Y = [2, 1, 3]
    Z = [1, 3, -1]

    dat = np.column_stack([X, Y, Z]).T
    print dat

    print 'covs'
    print 'cov(X, X): {:.2f} {:.2f}'.format(cov(X, X), var(X))
    print 'cov(Y, Y): {:.2f} {:.2f}'.format(cov(Y, Y), var(Y))
    print 'cov(Z, Z): {:.2f} {:.2f}'.format(cov(Z, Z), var(Z))

    print 'mine'
    for r in covmat(X, Y, Z):
        print (' {:> 3.2f}'*len(r)).format(*r)

    print 'numpy'
    for r in np.cov(dat):
        print (' {:> 3.2f}'*len(r)).format(*r)

    'eigenvectors'
    print np.linalg.eig(np.cov(dat))
    for r in np.linalg.eig(np.cov(dat)):
        print r
        # print (' {:> 3.2f}'*len(r)).format(*r)


import numpy as np

class LogisticRegression(object):
    # setting learning rate and iteration times
    def __init__(self, alpha=0.0005, lamb=0.1, iters=100):
        self.iters = iters
        self.alpha = alpha
        self.lamb = lamb
        # add one line for intercept
        self.theta = np.array([0.0] * (X.shape[1] + 1))

    def __normalize(self, tensor):
        return (tensor - np.mean(tensor, axis=0)) / np.std(tensor, axis=0)

    def __addIntercept(self, tensor):
        intercept = np.ones((tensor.shape[0], 1))
        return np.append(intercept, tensor, axis=1)

    def __preprocess(self, tensor):
        # feature normalize
        tensor = self.__normalize(tensor)
        # add constant column to x(to accommodate the θ0 intercept term)
        tensor = self.__addIntercept(tensor)
        return tensor

    def __sigmoid(self,tensor):
        return 1/(np.exp(-tensor)+1)

    def __hypothese(self,X,theta):
        return self.__sigmoid(np.dot(
            X,theta
            ))

    def computeCost(self,X,y,theta):
        positive = np.multiply((-y),np.log(self.__hypothese(X,theta)))
        negative = np.multiply((1-y),(1-np.log(self.__hypothese(X,theta))))
        penalty  = self.lamb/( 2*(X.shape[0]) ) * np.dot(theta[1:],theta[1:])
        return np.sum(positive-negative)/len(X)

    def __gradientDescent(self,X,y,theta):
        for i in range(self.iters):
            # use vectorization implementation to optimize performance

            # do not penalize θ0.
            theta[0:1] = theta[0:1] - (self.alpha/X.shape[0])*(
                np.dot(X[:,0:1].T,self.__hypothese(X[:,0:1],theta[0:1])-y)
            )

            # calculate others theta with regulation
            theta[1:] = theta[0:1] - (self.alpha/X.shape[0])*(
                np.dot(X[:,1:].T,self.__hypothese(X[:,1:],theta[1:])-y)
             + self.lamb * theta[1:])

            print("the cost of iteration {} is {}".format(
                i, self.computeCost(X, y, theta)))

        return theta

    def fit(self, X, y):
        X = self.__preprocess(X)
        print(X)
        self.theta = self.__gradientDescent(X, y, self.theta)

    def predict(self, X):
        X = self.__preprocess(X)
        pred = self.__hypothese(X,self.theta)
        numpy.putmask(pred,pred>=0.5,1.0)
        numpy.putmask(pred,pred<0.5,0.0)
        return pred

if __name__ == "__main__":
    pass


import sys
import scipy.io as sio
from pprint import pprint
import matplotlib.pyplot as plt
import numpy as np
from mpl_toolkits.mplot3d import Axes3D

mat_fname = "data_logistic.mat"
mat_dict = sio.loadmat(mat_fname)

samples = mat_dict["logistic_data"]

# m = features number
# n = samples number
# hθ = sigmond(θT x)

# J(θ) = -1/m * sum(Y(i) * log(hθ(X(i)) - (1 - Y(i)) * log(1 - hθ(X(i))))) + L * sum(θj ^ 2)
# θj :=  θj * (1 - (alpha * L) /m) - alpha/m * (hθ(X(i) - Y(i)) * X(i)j
# j = 0,1,..,n

def sigmoid(z):
	return 1 / (1 + np.exp(-z))

def loss(h, y):
    return (-y * np.log(h) - (1 - y) * np.log(1 - h)).mean()

X = []
Y = []
m = 0
for sample in samples:
	X.append(sample[0:2])
	Y.append(sample[2])
	m += 1
X = np.array(X)
Y = np.array(Y)

alpha = 0.01
num_iter = 10000
theta = np.zeros(X.shape[1])
L = 4800 #L2-norm effect here

for i in range(num_iter):
	z = np.dot(X, theta)
	h = sigmoid(z)
	gradient = np.dot(X.T, (h - Y)) / m
	theta = theta * (1 - (alpha * L) / m) - alpha * gradient  #L2-norm effect here

def predict(Xtest, theta, threshold=0.5):
	z = np.dot(Xtest, theta)
	h = sigmoid(z)
	print(h)
	return h >= threshold

count = 0
for sample in samples:
	x = sample[0:2]
	y = predict(x, theta)
	if y == sample[2]:
		count += 1
print(' Accuracy:', float(count/m))


"""
-Problem:
Some mathematical model answers to this equation:
 w = ax + by + cz
The following represents this model behaviour:
  a + 2.3b + 3.1c = 23.4 (1)
 2a + 4.5b + 5.2z = 41.5 (2)
 3a + 5.3b + 6.7z = 52.4 (3)
 4a + 6.1b + 7.1z = 57.8 (4)
 5a + 6.8b + 7.7z = 63.9 (5)

Find the value of constants a, b and c.

-Solution approach:
This may be a nonlinear behaviour and it's considered to be an overdetermined system of equations.
Let's cluster some of them in linear combinations to examine consistent values in the behaviour exposed.

I'll use Cramer's rule for this purpose.
"""

import numpy as np
from cramer_rule import cramer_ruleNxN

def analysis(results, equations):

	#Building matrixes
	results = np.array([results[0], results[1], results[2]]).reshape(3,1)
	equations = np.array([equations[0], equations[1], equations[2]])

	#Trying to apply the Cramer's Rule
	try:
		return cramer_ruleNxN(equations, results, 3)
	except ZeroDivisionError:
		return None

#Doing analysis of each linear combination
constants = analysis([23.4, 41.5, 52.4], [[1, 2.3, 3.1], [2, 4.5, 5.2], [3, 5.3, 6.7]])
print(f"Solution for linear combination of (1), (2) and (3): {list(map(round, constants))}", end = "\n" * 2)

constants = analysis([23.4, 41.5, 57.8], [[1, 2.3, 3.1], [2, 4.5, 5.2], [4, 6.1, 7.1]])
print(f"Solution for linear combination of (1), (2) and (4): {list(map(round, constants))}", end = "\n" * 2)

constants = analysis([23.4, 41.5, 63.9], [[1, 2.3, 3.1], [2, 4.5, 5.2], [5, 6.8, 7.7]])
print(f"Solution for linear combination of (1), (2) and (5): {list(map(round, constants))}", end = "\n" * 2)

constants = analysis([52.4, 57.8, 63.9], [[3, 5.3, 6.7], [4, 6.1, 7.1], [5, 6.8, 7.7]])
print(f"Solution for linear combination of (3), (4) and (5): {list(map(round, constants))}", end = "\n" * 2)

constants = analysis([52.4, 57.8, 23.4], [[3, 5.3, 6.7], [4, 6.1, 7.1], [1, 2.3, 3.1]])
print(f"Solution for linear combination of (3), (4) and (1): {list(map(round, constants))}", end = "\n" * 2)

constants = analysis([52.4, 57.8, 41.5], [[3, 5.3, 6.7], [4, 6.1, 7.1], [2, 4.5, 5.2]])
print(f"Solution for linear combination of (3), (4) and (2): {list(map(round, constants))}", end = "\n" * 2)

#Printing out the conclusion
print("General solution to the system: a = 1, b = 3, c = 5")


#############################################
#                                           #
# Carlos Santos                             #
# ECE 351-51                                #
# Lab 8                                     #
# 3/24/2020                                 #
#                                           #
#                                           #
#############################################

import numpy as np
import matplotlib.pyplot as plt

#%% Task 1
def a(k):
    a = (2*np.sin(np.pi*k)-np.sin(2*np.pi*k))/(np.pi*k)
    return np.round(a,2)

def b(k):
    b = (-2*np.cos(np.pi*k) + np.cos(2*np.pi*k) + 1)/(np.pi*k)
    return b

def Fourier_approx(n,t,T):
    x=0
    for i in range(1,n+1):
        x = x + b(i)*np.sin(i*(2*np.pi/T)*t)
    return x

n = np.arange(0,5)
ak = a(n)
bk = b(n)

print("a_0 = ", ak[0])
print("a_1 = ", ak[1])
print("b_1 = ", bk[1])
print("b_2 = ", bk[2])
print("b_3 = ", bk[3])

#%% Task 2

steps = 1e-3
t = np.arange(0,20+steps,steps)

f1 = Fourier_approx(1,t,8)
f3 = Fourier_approx(3,t,8)
f15 = Fourier_approx(15,t,8)
f50 = Fourier_approx(50,t,8)
f150 = Fourier_approx(150,t,8)
f1500 = Fourier_approx(1500,t,8)

plt.figure(figsize = (10, 7))
plt.subplot(3, 1, 1)
plt.plot(t, f1)
plt.title('Fourier Series Approximations of Square Wave')
plt.grid()
plt.ylabel('N = 1')

plt.subplot(3, 1, 2)
plt.plot(t, f3)
plt.grid()
plt.ylabel('N = 3')

plt.subplot(3, 1, 3)
plt.plot(t, f15)
plt.grid()
plt.xlabel('t')
plt.ylabel('N = 15')
plt.show()

plt.figure(figsize = (10, 7))
plt.subplot(3, 1, 1)
plt.plot(t, f50)
plt.title('Fourier Series Approximations of Square Wave')
plt.grid()
plt.ylabel('N = 50')

plt.subplot(3, 1, 2)
plt.plot(t, f150)
plt.grid()
plt.ylabel('N = 150')

plt.subplot(3, 1, 3)
plt.plot(t, f1500)
plt.grid()
plt.xlabel('t')
plt.ylabel('N = 1500')
plt.show()


#Newton法による非線型方程式の解法プログラム
import numpy as np  #NumPyライブラリ
import matplotlib.pyplot as plt  #データ可視化ライブラリ

#解きたい方程式
def func_f(x):
    return x**2.0 -2.0

#Newton法（方程式の関数項、探索の開始点、微小量、誤差範囲、最大反復回数）
def newton(func_f, x0, eps=1e-10, error=1e-10, max_loop=100):
    num_calc = 0  #計算回数
    print("{:3d}:  x = {:.15f}".format(num_calc, x0))

    #ずっと繰り返す
    while(True):
        #中心差分による微分値
        func_df = (func_f(x0 +eps) -func_f(x0 -eps))/(2*eps)
        if(abs(func_df) <= eps):  #傾きが0に近ければ止める
            print("error: abs(func_df) is too small (<=", eps, ").")
            quit()

        #次の解を計算
        x1 = x0 -func_f(x0)/func_df

        num_calc += 1  #計算回数を数える
        print("{:3d}:  x = {:.15f}".format(num_calc, x0))

        #「誤差範囲が一定値以下」または「計算回数が一定値以上」ならば終了
        if(abs(x1-x0)<=error or max_loop<=num_calc):
            break

        #解を更新
        x0 = x1

    #最終的に得られた解
    print("x = {:.15f}".format(x0))

    return x0

#可視化（方程式の関数項、グラフ左端、グラフ右端、方程式の解）
def visualization(func_f, x_min, x_max, x_solved):
    plt.xlabel("$x$")  #x軸の名前
    plt.ylabel("$f(x)$")  #y軸の名前
    plt.grid()  #点線の目盛りを表示
    plt.axhline(0, color='#000000')  #f(x)=0の線

    #関数
    exact_x = np.arange(x_min,x_max, (x_max-x_min)/500.0)
    exact_y = func_f(exact_x)

    plt.plot(exact_x,exact_y, label="$f(x)$", color='#ff0000')  #関数を折線グラフで表示
    plt.scatter(x_solved,0.0)  #数値解を点グラフで表示
    plt.text(x_solved,0.0, "$x$ = {:.9f}".format(x_solved), va='bottom', color='#0000ff')
    plt.show()  #グラフを表示

#メイン実行部
if (__name__ == '__main__'):
    #Newton法で非線型方程式の解を計算
    solution = newton(func_f, -2.0)

    #結果を可視化
    visualization(func_f, solution-1.0, solution+1.0, solution)


#!/usr/bin/python3
# -*- coding: utf-8 -*-

import operator
from functools import reduce
# scipy version >= 1.0.0
#  from scipy.special import perm

# combinatory
def mycomb(n, k):
    return reduce(operator.mul, range(n-k+1, n+1)) / reduce(operator.mul, range(1, k+1))

# permutation
def myperm(n, k):
    if n == k:
        return reduce(operator.mul, range(1,n+1))
    return reduce(operator.mul, range(1,n+1)) / reduce(operator.mul, range(1, n-k+1))

def main():
    print(mycomb(10, 5))
    print(myperm(4, 4))

if __name__ == "__main__":
    main()


import numpy as np

def PCA(X,dims=0,transform=True):
    if transform:
        means = np.mean(X,axis=1).reshape(X.shape[0],1)
        X = np.subtract(X,means)

    U,S,_ = np.linalg.svd(X)

    if dims > 0:
        return U[:dims,:], S[:dims]
    else:
        return U, S

def skree_plot(S,axis):
    axis.plot(S)
    axis.set_ylabel(r'$\lambda$')
    axis.set_title('Skree plot of PCs')

def cumulative_variance_plot(S,axis):
    total = np.sum(S)
    axis.plot(np.cumsum(S)/total)
    axis.set_ylabel('Fraction of variance explained')
    axis.set_xlabel('Component')
    axis.set_title('Variance explained using up to nth PC')

def convert_to_graph(X,affinity_measure='euclidean',epsilon=1,random_walk_with_self_loop=False,truncate=False,threshold=0.1,explore=False):
    _,n = X.shape
    A = np.zeros((n,n))
    for i in range(n):
        for j in range(n):
            if j >= i:
                p1 = X[:,i]
                p2 = X[:,j]
                distance = np.linalg.norm(np.subtract(p2,p1))
                A[i,j] = np.exp(-0.5*np.square(distance)/epsilon)
    if explore:
        return A.flatten()
    if truncate:
        mask = A < threshold
        A[mask] = 0
    A = A + np.triu(A,1).T
    if random_walk_with_self_loop:
        assert affinity_measure == 'euclidean', "Euclidean affinity only supported for random walk transition matrix"
        deg = np.sum(A,axis=1).reshape(n,1)
        A = np.divide(A,2*deg)
        A = A + np.diag(np.ones(n)*0.5,0)
    return A

def _diffusion_map(A):
    U,S,_ = np.linalg.svd(A)
    U = U[:,1:]
    return U

def diffusion_map(X,dims=0,affinity_measure='euclidean'):
    A = convert_to_graph(X,affinity_measure=affinity_measure,random_walk_with_self_loop=True)
    dm = _diffusion_map(A)
    if dims > 0:
        return dm[:dims,:]
    else:
        return dm

def embedding_2D(X,transform=True,method='PCA',affinity_measure='euclidean'):
    if method == 'PCA':
        X_,_ = PCA(X,dims=2,transform=transform)
        return X_[0,:],X_[1,:]
    elif method == 'DiffMap':
        X_ = diffusion_map(X,dims=2,affinity_measure=affinity_measure)
        return X_[0,:],X_[1,:]


import numpy as np
from scipy.optimize import linprog

d = [166.67, 74.08, 222.23, 268.52, 250, 120.38]
d = np.array(d)
# d *= 1.08
# Doesn't have solution if demand increases 8% !!!

c1 = np.array([1, 1, 1, 1, 1, 1])
c2 = np.array([1, 1, 1, 1, 1])
c = np.concatenate((c1 * 1000, c2 * 100))

A_eq = [
    [1, 0, 0, 0, 0, 0,
     -1, 0, 0, 0, 0],
    [0, 1, 0, 0, 0, 0,
     1, -1, 0, 0, 0],
    [0, 0, 1, 0, 0, 0,
     0, 1, -1, 0, 0],
    [0, 0, 0, 1, 0, 0,
     0, 0, 1, -1, 0],
    [0, 0, 0, 0, 1, 0,
     0, 0, 0, 1, -1],
    [0, 0, 0, 0, 0, 1,
     0, 0, 0, 0, 1]]
b_eq = [-60., 0, 0, 0, 0, 50.]
b_eq = np.array(b_eq)
b_eq += d

bounds = (
    (0, 150), (0, 195), (0, 210), (0, 255), (0, 190), (0, 220),
    (0, None), (0, None), (0, None), (0, None), (0, None))

res = linprog(c, A_eq=A_eq, b_eq=b_eq, bounds=bounds)
print(res)


# encoding:utf-8

import numpy as np
from sklearn.preprocessing import OneHotEncoder

class SoftmaxRegression(object):
    def __init__(self, reg_str=0.0):
        self.W = None
        self.error = None
        self.lam = reg_str  # regularization strength

    def _linear_hypothesis(self, W, X):
        """
        wX+b
        """
        return W.dot(X.T).T

    def _softmax(self, z):
        """
        e^zj/Σe^zk
        """
        z -= np.max(z)
        return np.exp(z) / np.sum(np.exp(z), axis=1, keepdims=True)

    def _softmax_hypothesis(self, W, X):
        """
        calculate probability
        """
        return self._softmax(self._linear_hypothesis(W, X))

    def _cal_error(self, W, X, y):
        """
        cross-entropy = [ΣΣ_k -y_k*log(h_k(x))]/m + (lam * w^2) / (2*m)
        """
        h = self._softmax_hypothesis(W, X)
        error = (np.sum(-y * np.log(h)) + np.sum(self.lam * W ** 2) / 2) / X.shape[0]
        return error

    def _cal_gradient(self, W, X, y):
        """
        g = Σ(h(x) - y)*x_j / m + lam * w / m
        """
        h = self._softmax_hypothesis(W, X)
        g = (np.dot(X.T, (h - y)).T + self.lam * W) / X.shape[0]
        return g

    def _concat_bias(self, X):
        bias = np.ones([X.shape[0], 1])
        X = np.concatenate([X, bias], axis=1)
        return X

    def fit(self, X, y, epoch=100, learning_rate=0.05, tolerance=1e-4):
        n_samples, n_features = X.shape
        n_classes = len(np.unique(y))

        # consider x_0 = 1 which is the feature val of the bias
        X = self._concat_bias(X)

        # [0,1,2] => [[0], [1], [2] ]
        y = np.reshape(y, [-1, 1])

        # [[0], [1], [2] ] => [[1,0,0], [0,1,0], [0,0,1]]
        y = OneHotEncoder(sparse=False).fit_transform(y)

        # init weights
        self.W = np.zeros([n_classes, n_features + 1])

        # mini-batch SGD
        mb = min(1000, n_samples / 10 or n_samples)

        for i in range(epoch):
            s = 0
            while s < n_samples:
                e = s + mb
                X_b = X[s:e]
                y_b = y[s:e]
                g = self._cal_gradient(self.W, X_b, y_b)
                self.W -= learning_rate * g
                self.error = self._cal_error(self.W, X, y)

                if self.error < tolerance:
                    return self
                s = e
        return self

    def predict(self, X_test):
        X = self._concat_bias(X_test)
        prob = self._softmax_hypothesis(self.W, X)
        label = np.argmax(prob, axis=1)
        return prob, label

if __name__ == '__main__':
    from sklearn.datasets import load_iris
    from sklearn.model_selection import train_test_split

    X, y = load_iris(True)
    X_train, X_test, y_train, y_test = train_test_split(X, y, test_size=0.3)

    model = SoftmaxRegression()
    model.fit(X_train, y_train)
    pred_prob, pred_label = model.predict(X_test)

    print 'Accuracy: %.2f%%' % (np.average(pred_label == y_test) * 100)


from numpy import array, searchsorted, interp
import math

def find_nearest(array,value):
    """ Finds the index of the value in a sorted `array` that most closely matches `value`

    see https://stackoverflow.com/questions/2566412/find-nearest-value-in-numpy-array
    """
    idx = searchsorted(array, value, side="left")
    if idx > 0 and (idx == len(array) or math.fabs(value - array[idx-1]) < math.fabs(value - array[idx])):
        return idx-1
    else:
        return idx

refraction_data = array([[14, 15, 17, 19, 21, 24, 27, 30, 35, 40, 45, 50, 55, 60, 65, 75, 80, 85, 90],
                         [3.8, 3.4, 3.1, 2.8, 2.5, 2.2, 1.9, 1.7, 1.4, 1.2, 1.0, .8, .7, .6, .5, .4, .3, .2, .1]])

def refraction_table(alt, interpolate=False):
    if alt < 14:
        raise ValueError('Altitude is too low for this table')
    if not interpolate:
        return refraction_data[1, find_nearest(refraction_data[0], alt)]
    else:
        return round(interp(alt, refraction_data[0], refraction_data[1], right=0), 1)

def simple_refraction_table(alt):
    if alt < 14:
        raise ValueError('Altitude is too low for this table')
    if alt > 60:
        return 0
    elif alt >= 33:
        return 1
    else:
        return refraction_data[1, find_nearest(refraction_data[0], alt)]

dip_data = array([[1, 5, 10, 15, 20, 25, 30, 40, 50, 60, 70, 80, 100, 120, 140, 160, 180, 200, 220, 240],
                  [1, 2.2, 3.1, 3.8, 4.3, 4.9, 5.3, 6.1, 6.9, 7.5, 8.1, 8.7, 9.7, 10.6, 11.5, 12.3, 13.0, 13.7, 14.4, 15.0]])

def dip_table(height_ft, interpolate=False):
    if height_ft > 240:
        raise ValueError('Height too high for this table')
    if not interpolate:
        return dip_data[1, find_nearest(dip_data[0], height_ft)]
    else:
        return round(interp(height_ft, dip_data[0], dip_data[1], left=0), 1)


'''
Given two bounding boxes, output the intersection over union
'''

import numpy as np
def iou(bbox_pred, bbox_true):

    # bbox - x, y, w, h

    x1 = bbox_pred[0]
    y1 = bbox_pred[1]
    w1 = bbox_pred[2]
    h1 = bbox_pred[3]

    x2 = bbox_true[0]
    y2 = bbox_true[1]
    w2 = bbox_true[2]
    h2 = bbox_true[3]

    intersection = (max([x1, x2]) - min( [x1+w1, x2+w2] )) * (max([y1,y2]) - min( [y1+h1 , y2+h2]))

    union = w1*h1 +w2*h2 - intersection

    iou_result = intersection/union

    return iou_result

if __name__ == "__main__":
    # Example
    bbox_pred = np.array([0,0,10,10])
    bbox_true = np.array([5,5,15,20])

    iou_out = iou(bbox_pred, bbox_true)
    print(iou_out)


import matplotlib.pyplot as plt

import numpy as np
from numpy.linalg import det, pinv
import scipy.io as sio

def visualize_data(X, title):
    x1, x2 = X.T
    plt.plot(x1, x2, 'bx')
    plt.xlabel('Latency (ms)')
    plt.ylabel('Throughput (mb/s)')
    plt.xlim([0, 30])
    plt.ylim([0, 30])
    plt.title(title)
    return plt

def visualize_fit(X, mu, sigma2):
    visualize_data(X, 'Visualizing Gaussian fit.')
    x = np.arange(0, 35, 0.5)
    x1, x2 = np.meshgrid(x, x)

    z = multivariate_gaussian(np.hstack((x1.reshape(-1,1), x2.reshape(-1,1))), mu, sigma2).reshape(x1.shape)
    plt.contour(x1, x2, z)

    return plt

def estimate_gaussian(X):

    return np.mean(X, axis=0)[:, np.newaxis], np.var(X, axis=0)[:, np.newaxis]

def multivariate_gaussian(X, mu, Sigma2):
    k = float(len(mu))
    X = np.copy(X)

    if any(s == 1 for s in Sigma2.shape):
        Sigma2 = np.diag(Sigma2.ravel())

    X -= mu.reshape(1, -1)
    return (2*np.pi)**(-k/2)*det(Sigma2)**(-0.5)*np.exp(-0.5*np.sum(np.dot(X, pinv(Sigma2))*X, axis=1))

def select_threshold(yval, pval):
    yval = yval.ravel()

    best_epsilon = 0
    best_F1 = 0
    stepsize = (np.max(pval) - np.min(pval)) / 1000
    for epsilon in np.arange(np.min(pval), np.max(pval), stepsize):
        cvPredictions = pval < epsilon

        tp = np.sum((cvPredictions == 1) & (yval == 1), dtype=float)
        fp = np.sum((cvPredictions == 1) & (yval == 0))
        fn = np.sum((cvPredictions == 0) & (yval == 1))
        recall = tp/(tp+fn)
        precision = tp/(tp+fp)

        F1 = 2*recall*precision/(recall+precision)

        if F1 > best_F1:
            best_F1, best_epsilon = F1, epsilon

    return best_epsilon, best_F1

if __name__ == '__main__':
    data = sio.loadmat('ex8data1.mat')
    X = data['X']
    Xval = data['Xval']
    yval = data['yval']

    visualize_data(X, 'Visualizing example dataset for outlier detection').show()

    mu, sigma2 = estimate_gaussian(X)
    p = multivariate_gaussian(X, mu, sigma2)
    visualize_fit(X, mu, sigma2).show()

    pval = multivariate_gaussian(Xval, mu, sigma2)
    epsilon, F1 = select_threshold(yval, pval)

    print('Best epsilon found using cross-validation: %s' % epsilon)
    print('Best F1 on Cross Validation Set:  %s' % F1)
    print('   (you should see a value epsilon of about 8.99e-05)')

    visualize_data(X, 'The classified anomalies.')
    x1, x2 = X[p < epsilon, :].T
    plt.plot(x1, x2, 'ro')
    plt.show()

    data = sio.loadmat('ex8data2.mat')
    X = data['X']
    Xval = data['Xval']
    yval = data['yval']

    mu, sigma2 = estimate_gaussian(X)

    p = multivariate_gaussian(X, mu, sigma2)
    pval = multivariate_gaussian(Xval, mu, sigma2)
    epsilon, F1 = select_threshold(yval, pval)

    print('Best epsilon found using cross-validation: %s' % epsilon)
    print('Best F1 on Cross Validation Set:  %s' % F1)
    print('# Outliers found: %s' % np.sum(p < epsilon))
    print('   (you should see a value epsilon of about 1.38e-18)')


import pandas as pd
import scipy.stats
import numpy as np

def drawdown(returns_series: pd.Series):
    '''
    Takes a time series of asset returns
    Computes and returns a DataFrame, that contains:
            the wealth index
            the previous peaks
            percent drawdowns
    '''
    wealth_index = 1000 * (1 + returns_series).cumprod()
    previous_peaks = wealth_index.cummax()
    drawdowns = (wealth_index - previous_peaks)/previous_peaks
    return pd.DataFrame({
        'Wealth': wealth_index,
        'Peaks': previous_peaks,
        'Drawdown': drawdowns
    })

def get_ffme_returns(cap_a, cap_b, label_a, label_b):
    '''
    Load the Fama-French Datasetfor the returns of the
    # Top and Bottom deciles by MarketCap
    given two deciles by MarketCap
    '''
    me_m = pd.read_csv('data/Portfolios_Formed_on_ME_monthly_EW.csv', header=0,
                   index_col=0, parse_dates=True, na_values=-99.99)
    # rets = me_m[['Lo 10', 'Hi 10']]
    rets = me_m[[cap_a, cap_b]]
    rets.columns = [label_a, label_b]
    rets = rets/100
    rets.index = pd.to_datetime(rets.index, format='%Y%m').to_period('M')

    return rets

def get_hfi_returns():
    '''
    Load and format the EDHEC Hedge Fund Index Returns
    '''
    hfi = pd.read_csv('data/edhec-hedgefundindices.csv',
                     header=0, index_col=0, parse_dates=True)
    hfi = hfi/100
    hfi.index = hfi.index.to_period('M')

    return hfi

def get_ind_returns():
    '''
    load and format the Ken French 30 Industry Portfolios Value weighted monthly Returns
    '''
    ind = pd.read_csv('data/ind30_m_vw_rets.csv', header=0, index_col=0, parse_dates=True)/100
    ind.index = pd.to_datetime(ind.index, format="%Y%m").to_period('M')
    ind.columns = ind.columns.str.strip()
    return ind

def semideviation(returns):
    '''
    Returns the semideviation aka negative semideviation or returns
    returns must be a series or a dataframe
    '''
    is_negative = returns < 0
    return returns[is_negative].std(ddof=0)

def skewness(r):
    '''
    Alternative to scipy.stats.skew()
    Coumputes the skewness of supplied Series or Dataframe
    returns a float or a Series
    '''
    demeaned_r = r - r.mean()
    # use the population standard deviation, so set ddof = 0
    sigma_r = r.std(ddof=0)
    exp = (demeaned_r ** 3).mean()
    return exp/sigma_r**3

def kurtosis(r):
    '''
    Alternative to scipy.stats.kurtosis()
    Coumputes the kurtosis of supplied Series or Dataframe
    returns a float or a Series
    '''
    demeaned_r = r - r.mean()
    # use the population standard deviation, so set ddof = 0
    sigma_r = r.std(ddof=0)
    exp = (demeaned_r ** 4).mean()
    return exp/sigma_r**4

def is_normal(r, level=0.01):
    '''
    Applies the Jarque Bera Test to determine if a series is normal or not
    Test is applies at a 1% level by default
    Returns True if the hypothesis is accepted, False otherwise
    '''
    statistics, p_value = scipy.stats.jarque_bera(r)
    return p_value > level

def var_historic(r, level=5):
    '''
    returns the historic value at Risk at a specified level
    i.e. returns the number such that "level" percent of the returns
    fall below that number, and the (100-level) percent are above
    '''
    if isinstance(r, pd.DataFrame):
        return r.aggregate(var_historic, level=level)
    elif isinstance(r, pd.Series):
        return -np.percentile(r, level)
    else:
        raise TypeError('Expected r to be a pandas Series or a DataFrame')

from scipy.stats import norm

def var_gaussian(r, level=5, modified=False):
    '''
    Returns the Parametric Gaussian VaR of a Series or DataFrame
    If "modified" is True, then the modified VaR is returned,
    using the Cornish-Fischer modification
    '''
    # compute the Z score assuming it was Gaussian
    z = norm.ppf(level/100)
    if modified:
        # modify the Z score based on observed skewness and kurtosis
        s = skewness(r)
        k = kurtosis(r)
        z = (z +
                (z**2 - 1) * s/6 +
                (z**3 - 3*z)*(k-3)/24 -
                (2*z**3 - 5*z) * (s**2)/36
            )

    return -(r.mean() + z * r.std(ddof=0))

def cvar_historic(r, level=5):
    '''
    Computes the Conditional VaR of Series or a DataFrame
    '''
    if isinstance(r, pd.Series):
        is_beyond = r<= -var_historic(r, level=level)
        return -r[is_beyond].mean()
    elif isinstance(r, pd.DataFrame):
        return r.aggregate(cvar_historic, level=level)
    else:
        raise TypeError('Expected r to be a Series or a DataFrame')

def annualize_rets(r, periods_per_year):
    '''
    Annualizes a set of returns
    We should infer the periods per year
    but that is currently left as an exercise
    '''
    compunded_growth = (1+r).prod()
    n_periods = r.shape[0]
    return compunded_growth ** (periods_per_year/n_periods) - 1

def annualize_vol(r, periods_per_year):
    '''
    Annualizes the volatility of a set of returns
    We should infer the periods per year
    but that is currently left as an exercise
    to the reader
    '''
    return r.std()*(periods_per_year ** 0.5)

def sharpe_ratio(r, riskfree_rate, periods_per_year):
    '''
    Computes the annualized sharpe ratio of a set of returns
    '''
    # convert the annual riskfree rate to per period
    rf_per_period = (1 + riskfree_rate) ** (1/periods_per_year) - 1
    excess_ret = r - rf_per_period
    ann_ex_ret = annualize_rets(excess_ret, periods_per_year)
    ann_vol = annualize_vol(r, periods_per_year)
    return ann_ex_ret/ann_vol

def portfolio_return(weights, returns):
    '''
    Weights -> Returns
    '''
    return weights.T @ returns

def portfolio_vol(weights, covmat):
    '''
    Weights -> Vol
    '''
    return (weights.T @ covmat @ weights) ** 0.5

def plot_ef2(n_points, er, cov, style='.-'):
    '''
    plots the two-asset Eifficient Frontier
    '''
    if er.shape[0] != 2:
        raise ValueError('Plot ef2 can only plot two assets')
    weights = [np.array([w, 1-w]) for w in np.linspace(0, 1, n_points)]
    rets = [portfolio_return(w, er) for w in weights]
    vols = [portfolio_vol(w, cov) for w in weights]
    ef = pd.DataFrame({'Returns': rets, 'Volatility': vols})
    return ef.plot.line(x='Volatility', y='Returns', style=style)

from scipy.optimize import minimize

def minimze_vol(target_return, er, cov):
    '''
    target_return -> W
    '''
    n = er.shape[0]
    init_guess = np.repeat(1/n, n)
    bounds = ((0.0, 1.0), ) * n
    return_is_target = {
        'type': 'eq',
        'args': (er,),
        'fun': lambda weights, er: target_return - portfolio_return(weights, er) # target_is_met
    }
    weights_sum_to_1 = {
        'type': 'eq',
        'fun': lambda weights: np.sum(weights) - 1
    }
    results = minimize(portfolio_vol, init_guess, args=(cov, ), method='SLSQP',
                      options={'disp':False},
                      constraints=(return_is_target, weights_sum_to_1), bounds=bounds)
    return results.x

def optimal_weights(n_points, er, cov):
    '''
    --> list of weights to run the optimizer on to minimize the vol
    '''
    target_rs = np.linspace(er.min(), er.max(), n_points)
    weights = [minimze_vol(target_return, er, cov) for target_return in target_rs]
    return weights

def plot_efn(n_points, er, cov, style='.-'):
    '''
    plots the n-asset Eifficient Frontier
    '''
    weights = optimal_weights(n_points, er, cov)
    rets = [portfolio_return(w, er) for w in weights]
    vols = [portfolio_vol(w, cov) for w in weights]
    ef = pd.DataFrame({'Returns': rets, 'Volatility': vols})
    return ef.plot.line(x='Volatility', y='Returns', style=style)


#! /usr/bin/env python
import argparse
from scipy.stats import chi2
import matplotlib.pyplot as plt
from scipy.stats import norm
import os
import math
import numpy as np
import random

def sampleFair():
    return random.randrange(0,6,1)

def sampleCrucket():
    return abs(random.randrange(-3,6,1))

def accumulate(n, f):
    r = [0] * 6
    for i in range(n):
        r[f()] += 1
    return r

def getChiSquared(n, f):
    r = accumulate(n, f)
    v = 0
    for x in r:
        c = (x - n/6)
        v += c * c / (n/6)
    return v

def getQuantile(alpha, df=5):
    return chi2.ppf(1-alpha, df)

def plotSampleCluster(m, n, f, outdir, alpha, desc, df=5):
    r = [getChiSquared(n, f) for x in range(m)]
    r.sort()
    fig, ax = plt.subplots(1, 1)
    x = np.linspace(0, r[-1], m)
    ax.plot(x, chi2.cdf(x, df), 'r', label='chi2 cdf')
    ax.plot(r, np.linspace(0,1,m), 'b', label='{} m={} n={} cdf'.format(desc, m, n))
    ax.plot([0,r[-1]], [1-alpha,1-alpha], 'y')
    ax.plot([getQuantile(alpha),getQuantile(alpha)], [0,1], 'b')
    ax.legend(loc='best', frameon=False)
    plt.savefig(outdir + "/chi2_{}_{}_{}.png".format(desc, m, n))

if __name__ == "__main__":
    parser = argparse.ArgumentParser(description='Empirical look into Chi Squared distributions')
    parser.add_argument('m', type=int, help='Number of samples to perform')
    parser.add_argument('alpha', nargs='?', type=float, help='significance value to mark in the distribution', default=0.05)
    parser.add_argument('outdir', nargs='?', type=str, help='output directory', default='out')
    args = parser.parse_args()
    N = [6, 30, 150]
    for n in N:
        plotSampleCluster(args.m, n, sampleFair, args.outdir, args.alpha, 'fair')
        plotSampleCluster(args.m, n, sampleCrucket, args.outdir, args.alpha, 'broken')


import numpy as np
import nanalysis as na
import support
import matplotlib.pyplot as plt
from support import BaseFunction
from support import seperatebyN

class LotkaVolterraModel( BaseFunction ):

	def __init__( self, r = 1,  a = 0.03, b = 0.025, c = 1 ):

		self.r = r
		self.a = a
		self.b = b
		self.c = c
		self.numberofvalues = 2
	def get_values( self, x, y, t ):
		return self.r * x - self.a * x * y, self.b * x * y -self.c * y

	def get_linelist( self ):
		return [ [ 0, self.r / self.a ], [ self.c / self.b, 0] ]

	def get_potential( self, x, y ):
		return -self.c * np.log( x ) + self.b * x - self.r * np.log( y ) + self.a * y

class LotkaVolterraModel2( BaseFunction ):

	def __init__( self, r = 0.03, K = 100, h = 0.1 ,a = 0.002, b = 0.012, c = 0.1, numberofvalues = 2 ):

		self.r = r
		self.K = K
		self.h = h
		self.a = a
		self.b = b
		self.c = c
		self.numberofvalues = 2
	def get_values( self, x, y, t ):
		return self.r * x * ( 1 - x /self.K ) - self.a * x * y / ( 1 + self.h * x ), self.b * x * y / ( 1 + self.h * x ) - self.c * y

	def get_linelist( self ):

		return  [ [ self.c / (self.b - self.c * self.h), 0 ] ]

	def get_additionalplots(self):
		adplots = self.make_adxrange()

		adplots.append(1 / self.a * ( 1 + self.h * adplots[0] ) * self.r * ( 1 - adplots[0] / self.K ) )

		return adplots

class BasicPandemicModel( BaseFunction ):

    def __init__( self, b = 0.01, c = 1 ):
        """
        b : float : infectious coefficient
        c : float : rate of death or getting better
        """
        self.b = b
        self.c = c
        self.numberofvalues = 2

    def get_values( self, x, y, t ):

        return -self.b * x * y, self.b * x * y - self.c * y

    def get_linelist( self ):

        return  [ [ self.c / self.b , 0 ] ]

class AndersonMayModel( BaseFunction ):

    def __init__( self, r = 0.03, b = 0.012, c = 0.1 , e = 0.1 , v = 0.1, numberofvalues = 3 ):

        self.r = r
        self.b = b
        self.c = c
        self.e = e
        self.v = v
        self.numberofvalues = 3

    def get_values( self, x, y, z, t ):

        return -self.r *( x + y + z ) - self.b * x * y - self.e * x,\
                self.b*  x * y - ( self.c + self.e + self.v ) * y,\
                self.v * y - self.e * z

if __name__ == '__main__':

    ps = support.get_base_args()
    args = ps.parse_args()

    graph_n = args.equation_number
    ad = args.argdict

    sp   = args.spoint
    ep   = args.epoint

    hl   = args.hline
    vl   = args.vline
    llist  =  args.line
    pp   =  args.pointplot
    initial_value = args.initial_value

    saveoff = support.offsaving( args.savefigoff )

    graph = { 1: LotkaVolterraModel , 3 : LotkaVolterraModel2, 4: BasicPandemicModel, 6: AndersonMayModel  }

    if ad:
        function = graph[ graph_n ](**ad)
    else:
        function = graph[ graph_n ]()

    linelist =  function.get_linelist()

    if llist:
        linelist.append(llist)

    initial_value = seperatebyN( function.numberofvalues, initial_value )

    t,xpoints, ypoints, zpoints = function.get_values_rungekutta( sp = sp, ep = ep, initial_value = initial_value, numberofvalues = function.numberofvalues )

    adplots = None

    if 'get_additionalplots' in dir(function ):

        adplots = function.get_additionalplots()

    na.graph_plot( t, xpoints = xpoints, ypoints = ypoints, zpoints = zpoints, chapter = 3 , function = function, hlines = hl, vlines = vl, linelist = linelist , savefigOn = saveoff, graph_n = graph_n, N = 1000, pointplot = pp, additionalplots = adplots )