{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This notebook shows how to use [CDD](https://www.inf.ethz.ch/personal/fukudak/cdd_home/) to compute controlled invariant sets for an hybrid system.\n",
    "We consider the `cruise_control.jl` example of HybridSystems.jl which comes from [this paper](https://dl.acm.org/citation.cfm?id=2461378)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "include(Pkg.dir(\"HybridSystems\", \"examples\", \"cruise_control.jl\"));"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "const va = 15.6\n",
    "const vb = 24.5\n",
    "const vc = 29.5\n",
    "const v = (va, vb, vc)\n",
    "const U = 4.\n",
    "const m0 = 500\n",
    "const T = 2\n",
    "const N = 10\n",
    "const M = 1\n",
    "const H = 0.8;"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "macro _time(x)\n",
    "    quote\n",
    "        y = @timed $(esc(x))\n",
    "        # y[1] is returned value\n",
    "        # y[2] is time in seconds\n",
    "        y[2]\n",
    "    end\n",
    "end"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "using Gurobi\n",
    "lpsolver = GurobiSolver(OutputFlag=0);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "function liftu(S, sys::HybridSystems.DiscreteLinearControlSystem)\n",
    "    [sys.A sys.B] \\ S\n",
    "end\n",
    "function new_constraint(hs, S, q, t)\n",
    "    @assert source(hs, t) == q\n",
    "    σ = symbol(hs, t)\n",
    "    r = target(hs, t)\n",
    "    ABset = liftu(S[1], hs.resetmaps[σ])\n",
    "    project(ABset, 1:statedim(hs, q))\n",
    "end\n",
    "function new_constraints(hs, S, q)\n",
    "    map(t -> new_constraint(hs, S, q, t), out_transitions(hs, q))\n",
    "end\n",
    "function add_hrep!(S, h::HalfSpace)\n",
    "    # I was using LP cycling errors when using CDD's LP solver\n",
    "    if issubset(S, h) # If S ⊆ h, then adding h will not change affect S\n",
    "        false\n",
    "    else\n",
    "        push!(S, SimpleHRepresentation(reshape(h.a, 1, length(h.a)), [h.β]))\n",
    "        true\n",
    "    end\n",
    "end\n",
    "function add_constraint!(S, P)\n",
    "    added = count(map(h -> add_hrep!(S, h), ineqs(P))) + count(map(h -> add_hrep!(S, h), eqs(P)))\n",
    "    removehredundancy!(S) # CDD throws LP cycling error\n",
    "    added\n",
    "end\n",
    "function add_constraints!(S::Polyhedron, Ps::Vector{<:Polyhedron})\n",
    "    sum(P -> add_constraint!(S, P), Ps)\n",
    "end\n",
    "function set_iteration!(hs, S)\n",
    "    Ps = map(q -> new_constraints(hs, S, q), states(hs))\n",
    "    added = add_constraints!.(S, Ps)\n",
    "    @show added\n",
    "end\n",
    "function iterate!(hs, S, nit)\n",
    "    map(i -> (gc(); @_time set_iteration!(hs, S)), 1:nit)\n",
    "end"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "Mmax = 1\n",
    "nit = 2\n",
    "t = zeros(Mmax, nit)\n",
    "Hs = Vector{HybridSystem}(Mmax)\n",
    "CIS = Vector{Vector{Polyhedron}}(Mmax)\n",
    "for m in 1:Mmax\n",
    "    Hs[m] = cruise_control_example(N, m, vmin = 5., v=(va, vb, vc), U=U, H=H, sym=false, m0=500);\n",
    "    I0 = Hs[m].invariants;\n",
    "    @show nineqs(I0[1])\n",
    "    CIS[m] = deepcopy(I0);\n",
    "    @show m\n",
    "    t[m, :] = iterate!(Hs[m], CIS[m], nit)\n",
    "    @show t[m, :]\n",
    "end\n",
    "t"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "Mmax = size(t, 1)\n",
    "nit = 1\n",
    "previt = size(t, 2)\n",
    "t = [t zeros(Mmax, nit)]\n",
    "totit = size(t, 2)\n",
    "for m in 1:Mmax\n",
    "    t[m, previt+(1:nit)] = iterate!(Hs[m], CIS[m], nit)\n",
    "    @show t[m, :]\n",
    "end\n",
    "t"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import Plots\n",
    "Plots.pyplot()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "D = [1, 2]\n",
    "Plots.plot(project(Hs[1].invariants[1], D))\n",
    "Plots.plot!(project(CIS[1][1], D))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "Plots.savefig(\"dist_trailerspeed.png\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "t"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Julia 0.6.0",
   "language": "julia",
   "name": "julia-0.6"
  },
  "language_info": {
   "file_extension": ".jl",
   "mimetype": "application/julia",
   "name": "julia",
   "version": "0.6.2"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}