{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Optimal parade route\n",
    "\n",
    "# Topic references\n",
    "- [Iterated weighted $\\ell_1$ heuristic](http://stanford.edu/class/ee364b/lectures/l1_ext_slides.pdf)\n",
    "\n",
    "# Problem statement\n",
    "\n",
    "In this notebook, we'll tackle the problem of trying to protect a parade route by placing a limited number of guards.\n",
    "\n",
    "The parade route is discritized into $m$ points. There are $n$ possible guard locations with associated decision variable $x \\in \n",
    "\\lbrace 0,1\\rbrace^n$, where $x_i = 1$ if and only if a guard is placed at location $i$. Associated with guard location $i$ is a *coverage vector* $a_i \\in \\mathbf{R}^m$, which describes how well a guard placed at location $i$ would 'cover' each point in the parade route. We will assume that guard coverage is additive, so that the vector describing the total coverage of every edge is given by $Ax$, where $A \\in \\mathbf{R}^{m \\times n}$ has $a_i$ as its $i$th column.\n",
    "\n",
    "The parade route is only as secure as its least well-covered point. Our goal is to place $k$ guards to maximize the minimum coverage over the points in the route.\n",
    "\n",
    "# Optimization formulation\n",
    "\n",
    "We can formulate this as the optimization problem\n",
    "\n",
    "\\\\[\n",
    "\\begin{array}{ll}\n",
    "  \\mbox{maximize} & t \\\\\n",
    "  \\mbox{subject to} & t \\leq Ax\\\\\n",
    "  & x \\in \\lbrace 0,1 \\rbrace^n \\\\\n",
    "  & \\mathbf{1}^Tx = k.\n",
    "\\end{array}\n",
    "\\\\]\n",
    "\n",
    "This problem is nonconvex and, in general, NP-hard due to the Boolean decision variable.\n",
    "\n",
    "# Relaxation\n",
    "\n",
    "We can try to approach the problem with convex optimization by first forming the convex relaxation\n",
    "\n",
    "\\\\[\n",
    "\\begin{array}{ll}\n",
    "  \\mbox{maximize} & t \\\\\n",
    "  \\mbox{subject to} & t \\leq Ax\\\\\n",
    "  & 0 \\leq x \\leq 1 \\\\\n",
    "  & \\mathbf{1}^Tx = k,\n",
    "\\end{array}\n",
    "\\\\]\n",
    "by constraining $x \\in [0,1]^n$.\n",
    "\n",
    "In general, the solution to this problem, $x^\\star$, will have fractional values. As we want a Boolean allocation, we can use an [iterated weighted $\\ell_1$ heuristic](http://stanford.edu/class/ee364b/lectures/l1_ext_slides.pdf) to try to recover a Boolean solution.\n",
    "\n",
    "# Iterated weighted $\\ell_1$ heuristic\n",
    "\n",
    "To try and recover a Boolean solution, we will solve a sequence of convex problems where we add a linear term $-w^Tx$ to the objective, picking the weight vector $w \\in \\mathbf{R}^n_+$ at each iteration to try and induce a sparse solution vector $x^\\star$. The details can be found in the [Stanford EE364B lecture notes](http://stanford.edu/class/ee364b/lectures/l1_ext_slides.pdf).\n",
    "\n",
    "The algorithm consists of initializing $w = 0$ and repeating the two steps\n",
    "\n",
    "1. \\\\[\n",
    "\\begin{array}{ll}\n",
    "  \\mbox{maximize} & t  - w^Tx\\\\\n",
    "  \\mbox{subject to} & t \\leq Ax\\\\\n",
    "  & 0 \\leq x \\leq 1 \\\\\n",
    "  & \\mathbf{1}^Tx = k,\n",
    "\\end{array}\n",
    "\\\\]\n",
    "2. Let $w_i = \\alpha/(\\tau + x_i) \\forall i$\n",
    "\n",
    "until we reach a Boolean solution. Here, $\\alpha$ and $\\tau$ are adjusted to promote a sparse solution. Typical choices would be $\\alpha = 1$ and $\\tau = 10^{-4}$.\n",
    "\n",
    "Intuitively, the weight vector $w$ is incentivizing elements of $x$ which were close to zero in the last iteration towards zero in the next iteration.\n",
    "\n",
    "# Example\n",
    "\n",
    "We create a parade route in the unit square $[0,1] \\times [0,1]$ by generating points along a connected sequence of line segments. Possible guard locations are a set of randomly placed points in the unit square. Guards' coverage of points in the parade route is a function of distance between the two points. We also add buildings in the unit square to obstruct the guards' view. The guard has no effect on a point if his line of sight is obstructed. We generate the $A$ matrix for this problem instance below."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "import cvxpy as cp\n",
    "\n",
    "def form_path(points,n):\n",
    "    x, y = [], []\n",
    "    pold = points[0]\n",
    "    for p in points[1:]:\n",
    "        x += list(np.linspace(pold[0],p[0],n))\n",
    "        y += list(np.linspace(pold[1],p[1],n))\n",
    "        pold = p\n",
    "\n",
    "    path = np.array([x,y]).T\n",
    "    return path\n",
    "\n",
    "def form_grid(k):\n",
    "    xs = list(np.linspace(0,1,k))\n",
    "    ys = list(np.linspace(0,1,k))\n",
    "\n",
    "    locations = []\n",
    "    for x in xs:\n",
    "        for y in ys:\n",
    "            locations.append(np.array((x,y)))\n",
    "    return np.array(locations).T\n",
    "\n",
    "def guard_sets(k,num,noise):\n",
    "    guard_set = []\n",
    "    grid = form_grid(k)\n",
    "    for i in range(num):\n",
    "        pert = noise*np.random.randn(*grid.shape)\n",
    "        guard_set.append( grid+pert )\n",
    "    return np.hstack(guard_set)\n",
    "\n",
    "def inRect(p,rect):\n",
    "    x,y,w,h = rect\n",
    "    return x <= p[0] and p[0] <= x + w and y <= p[1] and p[1] <= y + h\n",
    "\n",
    "def remove_guards(guards,buildings):\n",
    "    '''Remove guards inside buildings and outside unit square.'''\n",
    "    outside = []\n",
    "    for i, guard in enumerate(guards.T):\n",
    "        inside = False\n",
    "        for build in buildings:\n",
    "            if inRect(guard,build):\n",
    "                inside = True\n",
    "                break\n",
    "            if not inRect(guard,(0,0,1,1)):\n",
    "                inside = True\n",
    "                break\n",
    "        if not inside:\n",
    "            outside.append(i)\n",
    "    \n",
    "    return guards[:,outside]\n",
    "\n",
    "def intersect(p1,p2,xmin,xmax,ymin,ymax):\n",
    "    '''determine if a rectangle given by xy limits blocks the line of sight between p1 and p2'''\n",
    "\n",
    "    block = False\n",
    "    \n",
    "    # if either point inside block\n",
    "    for p in [p1,p1]:\n",
    "        if xmin <= p[0] and p[0] <= xmax and ymin <= p[1] and p[1] <= ymax:\n",
    "            return True\n",
    "    \n",
    "    # if the two points are equal at this stage, then they are outside the block\n",
    "    if p1[0] == p2[0] and p1[1] == p2[1]:\n",
    "        return False\n",
    "    \n",
    "    \n",
    "    if p2[0] != p1[0]:\n",
    "        for x in [xmin,xmax]:\n",
    "            alpha = (x-p1[0])/(p2[0] - p1[0])\n",
    "            y = p1[1] + alpha*(p2[1] - p1[1])\n",
    "\n",
    "            if 0 <= alpha and alpha <= 1 and ymin <= y and y <= ymax:\n",
    "                return True\n",
    "            \n",
    "    if p2[1] != p1[1]:\n",
    "        for y in [ymin,ymax]:\n",
    "            alpha = (y-p1[1])/(p2[1] - p1[1])\n",
    "            x = p1[0] + alpha*(p2[0] - p1[0])\n",
    "\n",
    "            if 0 <= alpha and alpha <= 1 and xmin <= x and x <= xmax:\n",
    "                return True\n",
    "        \n",
    "    return False\n",
    "\n",
    "def p_evade(x,y,r=.5,minval=.1):\n",
    "    d = np.linalg.norm(x-y)\n",
    "    if d > r:\n",
    "        return 1\n",
    "    return (1-minval)*d/r + minval\n",
    "\n",
    "def get_guard_effects(path, guards, buildings, evade_func):\n",
    "    guard_effects = []\n",
    "    for guard in guards.T:\n",
    "        guard_effect = []\n",
    "        for p in path:\n",
    "            prob = 1\n",
    "            if not np.any([intersect(p,guard,x,x+w,y,y+h) for x,y,w,h in buildings]):\n",
    "                prob = evade_func(p,guard)\n",
    "            guard_effect.append(prob)\n",
    "        guard_effects.append(guard_effect)\n",
    "    return np.array(guard_effects).T\n",
    "\n",
    "    locations = []\n",
    "    for x in xs:\n",
    "        for y in ys:\n",
    "            point = np.array((x,y))\n",
    "            detect_p = []\n",
    "            for r in path:\n",
    "                detect_p.append(p_evade(point,r,r=.5,m=0))\n",
    "            locations.append((point,np.array(detect_p)))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "[<matplotlib.lines.Line2D at 0x105d17cc0>]"
      ]
     },
     "execution_count": 2,
     "metadata": {},
     "output_type": "execute_result"
    },
    {
     "data": {
      "image/png": "[encoded data, 37160 characters]\n",
      "text/plain": [
       "<Figure size 720x720 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "np.random.seed(0)\n",
    "\n",
    "buildings = [(.1,.1,.4,.1),\n",
    "             (.6,.1,.1,.4),\n",
    "             (.1,.3,.4,.1),\n",
    "             (.1,.5,.4,.1),\n",
    "             (.4,.7,.4,.1),\n",
    "             (.8,.1,.1,.3),\n",
    "             (.8,.5,.2,.1),\n",
    "             (.2,.7,.1,.3),\n",
    "             (.0,.7,.1,.1),\n",
    "             (.6,.9,.1,.1),\n",
    "             (.9,.7,.1,.2)]\n",
    "\n",
    "n = 10\n",
    "\n",
    "points = [(.05,0),(.05,.25),(.55,.25),(.55,.6),(.75,.6),(.75,.05),(.95,.05), (.95,.45),(.75,.45), (.75,.65),(.85,.65),\n",
    "          (.85,.85),(.35,.85),(.35,.65),(.15,.65),(.15,1)]\n",
    "\n",
    "path = form_path(points,n)\n",
    "\n",
    "g = guard_sets(12,4,.02)\n",
    "g = remove_guards(g,buildings)\n",
    "\n",
    "guard_effects = get_guard_effects(path, g, buildings, p_evade)\n",
    "\n",
    "A = 1 - np.log(guard_effects)\n",
    "\n",
    "fig = plt.figure(figsize=(10,10))\n",
    "ax = plt.subplot(111,aspect='equal')\n",
    "for x,y,w,h in buildings:\n",
    "    rect = plt.Rectangle((x,y),w,h,fc='y',alpha=.3)\n",
    "    ax.add_patch(rect)\n",
    "\n",
    "ax.plot(path[:,0],path[:,1],'o')\n",
    "\n",
    "ax.plot(g[0,:],g[1,:],'ro',alpha=.3)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We perform the iterative algorithm below. At each step, we plot the vector $x$, demonstrating that it becomes increasingly sparse at each iteration."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "final objective value: -10.27091799207174\n"
     ]
    },
    {
     "data": {
      "image/png": "[encoded data, 8752 characters]\n",
      "text/plain": [
       "<Figure size 360x360 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    },
    {
     "data": {
      "image/png": "[encoded data, 6976 characters]\n",
      "text/plain": [
       "<Figure size 360x360 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    },
    {
     "data": {
      "image/png": "[encoded data, 6844 characters]\n",
      "text/plain": [
       "<Figure size 360x360 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "num_guards = 12\n",
    "tau = 1e-2\n",
    "\n",
    "m,n = A.shape\n",
    "\n",
    "w = np.zeros(n)\n",
    "\n",
    "for i in range(3):\n",
    "    x = cp.Variable(shape=n)\n",
    "    t = cp.Variable(shape=1)\n",
    "\n",
    "    objective = cp.Maximize(t - x.T*w)\n",
    "    constr = [0 <=x, x <= 1, t <= A*x, cp.sum(x) == num_guards]\n",
    "    cp.Problem(objective, constr).solve(verbose=False)\n",
    "    x = np.array(x.value).flatten()\n",
    "    w = 2/(tau+np.abs(x))\n",
    "    fig = plt.figure(figsize=(5,5))\n",
    "    ax = fig.add_subplot(111)\n",
    "    ax.plot(x,'o')\n",
    "\n",
    "xsol = x\n",
    "print(\"final objective value: {}\".format(objective.value))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Below, we plot the final Boolean allocation. The blue dots represent the parade route. The red dots represent the possible guard placement locations. The green dots show the actual guard placements. Yellow rectangles are buildings which obstruct the guards' view."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "[<matplotlib.lines.Line2D at 0xb1853f1d0>]"
      ]
     },
     "execution_count": 4,
     "metadata": {},
     "output_type": "execute_result"
    },
    {
     "data": {
      "image/png": "[encoded data, 47452 characters]\n",
      "text/plain": [
       "<Figure size 720x720 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "fig = plt.figure(figsize=(10,10))\n",
    "ax = plt.subplot(111,aspect='equal')\n",
    "for x,y,w,h in buildings:\n",
    "    rect = plt.Rectangle((x,y), w, h, fc='y', alpha=.3)\n",
    "    ax.add_patch(rect)\n",
    "\n",
    "ax.plot(path[:,0], path[:,1], 'o')\n",
    "\n",
    "ax.plot(g[0,:], g[1,:], 'ro', alpha=.3)\n",
    "ax.plot(g[0,xsol > .5], g[1,xsol > .5], 'go', markersize=20, alpha=.5)"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.6"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 1
}
