"""
```
  lsq_classopath(
    X      :: AbstractMatrix{T},
    y      :: AbstractArray{T};
    Aeq    :: AbstractMatrix = zeros(T, 0, size(X, 2)),
    beq    :: Union{AbstractArray, Number} = zeros(T, size(Aeq, 1)),
    Aineq  :: AbstractMatrix = zeros(T, 0, size(X, 2)),
    bineq  :: Union{AbstractArray, Number} = zeros(T, size(Aineq, 1)),
    ρridge :: Number = zero(T),
    penidx :: Array{Bool} = fill(true, size(X, 2)),
    solver = ECOSSolver(maxit=10e8, verbose=0)
    )
```
Calculate the solution path of the constrained lasso problem that minimizes
    `0.5sumabs2(√obswt .* (y - X * β)) + ρ * sumabs(penwt .* β)`
subject to linear constraints.

### Arguments
- `X`       : predictor matrix.
- `y`       : response vector.

### Optional arguments
- `Aeq`     : equality constraint matrix.
- `beq`     : equality constraint vector.
- `Aineq`   : inequality constraint matrix.
- `bineq`   : inequality constraint vector.
- `ρridge`  : tuning parameter for ridge penalty. Default is 0.
- `penidx`  : a logical vector indicating penalized coefficients.
- `solver`  : a solver Convex.jl supports. Default is ECOS.
              Note that Mosek and Gurobi are more robust than ECOS. Unlike ECOS or
              SCS, both Mosek and Gurobi require a license (free for academic
              use). For details, see <http://convexjl.readthedocs.io/en/latest/solvers.html>.
### Examples
See tutorial examples at <https://hua-zhou.github.io/ConstrainedLasso.jl/latest/demo/path/>.
"""

function lsq_classopath(
    X::AbstractMatrix{T},
    y::AbstractArray{T};
    Aeq::AbstractMatrix   = zeros(T, 0, size(X, 2)),
    beq::Union{AbstractArray, Number} = zeros(T, size(Aeq, 1)),
    Aineq::AbstractMatrix = zeros(T, 0, size(X, 2)),
    bineq::Union{AbstractArray, Number} = zeros(T, size(Aineq, 1)),
    ρridge::Number        = zero(T),
    penidx::Array{Bool}   = fill(true, size(X, 2)),
    solver = ECOSSolver(maxit=10e8, verbose=0)
    ) where T

    # T = promote_type(eltype(X), eltype(y))
    n, p = size(X)

    if n < p
        warn("Adding a small ridge penalty (default is 1e-4) since n < p")
        if ρridge <= 0
            warn("ρridge must be positive, switching to default value (1e-4)")
            ρridge = 1e-4
        end
        # create augmented data
        y = [y; zeros(p)]
        X = [X; √ρridge * eye(p)]
        # record original number of observations
        # n_orig = n
    else
        # make sure X is full column rank
        R = qrfact(X)[:R]
        rankX = sum(abs.(diag(R)) .> abs(R[1,1]) * max(n,p) * eps(eps(eltype(R))))

        if rankX != p
            warn("Adding a small ridge penalty (default is 1e-4) since X is rank deficient")
            if ρridge <= 0
                warn("ρridge must be positive, switching to default value (1e-4)")
                ρridge = 1e-4
            end
            # create augmented data
            y = [y; zeros(p)]
            X = [X; √ρridge * eye(p)]
        end
    end

    # allocate variables along path
    neq, nineq = size(Aeq, 1), size(Aineq, 1)
    maxiters = 5(p + nineq)               # max number of path segments to consider
    βpath = zeros(p, maxiters)
    λpatheq = zeros(neq, maxiters)      # dual variables for equality
    μpathineq = zeros(nineq, maxiters)  # dual variables for inequality
    ρpath = zeros(maxiters)             # tuning parameter
    dfpath = fill(Inf, maxiters)        # degree of freedom
    objvalpath = zeros(maxiters)        # objective value
    violationspath = fill(Inf, maxiters)

    ### initialization
    # use LP to find ρmax
    H = At_mul_B(X, X)
    #sense = [repmat(['='], neq); repmat(['<'], nineq)]
    #β, _, problem = lsq_constrsparsereg(X, y, Inf;      # why necessary?
    #          [Aeq; Aineq], sense, [beq; bineq], lb, ub)#

    # find the maximum ρ (starting value)
    ρpath[1], idx, = find_ρmax(X, y; Aeq = Aeq, beq = beq, Aineq = Aineq,
              bineq = bineq, solver = solver)

    # calculate at ρmax
    βpath[:, 1], objvalpath[1], problem = lsq_constrsparsereg(X, y, ρpath[1];
              Aeq = Aeq, beq = beq, Aineq = Aineq, bineq = bineq,
              penwt = penidx, solver = solver);

    for i in 1:min(2, length(problem.constraints))
      if eval((problem.constraints[i]).head) == ==
        λpatheq[:, 1] = problem.constraints[i].dual
      elseif eval((problem.constraints[i]).head) == <=
        μpathineq[:, 1] = problem.constraints[i].dual
      end
    end

    μpathineq[μpathineq .< 0] = 0
    setActive = (abs.(βpath[:, 1]) .> 1e-4) .| (.~penidx)
    βpath[find(.!setActive), 1] = 0

    residIneq = Aineq * βpath[:, 1] - bineq
    setIneqBorder = residIneq .== 0
    nIneqBorder = countnz(setIneqBorder)

    # initialize subgradient vector
    resid = y - X * βpath[:, 1]
    subgrad = X' * resid - Aeq' * λpatheq[:, 1] - Aineq' * μpathineq[:, 1]

    subgrad[setActive] = sign.(βpath[find(setActive), 1])
    subgrad[.!setActive] = subgrad[.!setActive] / ρpath[1]
    setActive[idx] = true
    nActive = countnz(setActive)

    # calculate degrees of freedom
    rankAeq = rank(Aeq) ### Brian's comment: need to make it more efficient
    dfpath[1] = nActive - rankAeq - nIneqBorder

    # set initial violations counter to 0
    violationspath[1] = 0

    # sign for path direction (originally went both ways, but increasing was retired)
    dirsgn = -1

    ####################################
    ### main loop for path following ###
    ####################################

    k = 0

    for k in 2:maxiters

      # threshold near-zero rhos to zero and stop algorithm
      if ρpath[k-1] <= 1e-4
        ρpath[k-1] = 0
        break
      end

      # calculate derivative for coefficients and multipliers
      # construct matrix

      activeCoeffs = find(setActive)
      inactiveCoeffs = find(.!setActive)
      idxIneqBorder = find(setIneqBorder)

      M = hcat(H[activeCoeffs, activeCoeffs], Aeq[:, activeCoeffs]',
        Aineq[find(setIneqBorder), activeCoeffs]')
      M = vcat(M, zeros(neq + nIneqBorder, size(M, 2)))
      M[(end - neq - nIneqBorder + 1):end, 1:nActive] = [Aeq[:, activeCoeffs];
                Aineq[idxIneqBorder, activeCoeffs]]

      # calculate derivative
      dir = try
        dirsgn * (M \ [subgrad[setActive]; zeros(neq + nIneqBorder)])
      catch
        dir = -(pinv(M) * [subgrad[setActive]; zeros(neq + nIneqBorder)])
      end

      if any(isnan.(dir))
        dir = -(pinv(M) * [subgrad[setActive]; zeros(neq + nIneqBorder)])
      end

      # calculate the derivative for rho * subgradient
      dirSubgrad = - hcat(H[inactiveCoeffs, activeCoeffs], Aeq[:, inactiveCoeffs]',
                Aineq[idxIneqBorder, inactiveCoeffs]') * dir

      ### check additional events related to potential subgraient violations

      ## inactive coefficients moving too slowly
      # negative subgradient
      inactSlowNegIdx = find(((1*dirsgn - 1e-8) .<= subgrad[.~setActive]) .&
        (subgrad[.~setActive] .<= (1*dirsgn + 1e-8)) .&
        (1*dirsgn .< dirSubgrad))

      # positive subgradient
      inactSlowPosIdx = find(((-1*dirsgn - 1e-8) .<= subgrad[.~setActive]) .&
          (subgrad[.~setActive] .<= (-1*dirsgn + 1e-8)) .&
          (dirSubgrad .< -1*dirsgn))

      ## "active" coeficients estimated as 0 with potential sign mismatch #%
      # positive subgrad but negative derivative
      signMismatchPosIdx = find(((0 - 1e-8) .<= subgrad[setActive]) .&
          (subgrad[setActive] .<= (1 + 1e-8)) .&
          (dirsgn * dir[1:nActive] .<= (0 - 1e-8)) .&
          (βpath[activeCoeffs, k-1] .== 0))

      # Negative subgradient but positive derivative
      signMismatchNegIdx = find(((-1 - 1e-8) .<= subgrad[setActive]) .&
          (subgrad[setActive] .<= (0 + 1e-8)) .&
          ((0 + 1e-8) .<= dirsgn * dir[1:nActive]) .&
          (βpath[activeCoeffs, k-1] .== 0))

      # reset violation counter (to avoid infinite loops)
      violateCounter = 0

      # outer while loop for checking all conditions together
      while ( ~isempty(inactSlowNegIdx) || ~isempty(inactSlowPosIdx) ||
            ~isempty(signMismatchPosIdx) || ~isempty(signMismatchNegIdx) )

          # monitor and fix condition 1 violations
          while !isempty(inactSlowNegIdx)
            ## Identify and move problem coefficient
            # indices corresponding to inactive coefficients
            inactiveCoeffs = find(.!setActive)
            # identify problem coefficient
            viol_coeff = inactiveCoeffs[inactSlowNegIdx]
            setActive[viol_coeff] = true

            # determine new number of active coefficients
            nActive = countnz(setActive)
            # determine number of active/binding inequality constraints
            nIneqBorder = countnz(setIneqBorder)

            activeCoeffs = find(setActive)
            inactiveCoeffs = find(.!setActive)
            idxIneqBorder = find(setIneqBorder)
            M = hcat(H[activeCoeffs, activeCoeffs], Aeq[:, activeCoeffs]',
              Aineq[find(setIneqBorder), activeCoeffs]')
            M = vcat(M, zeros(neq + nIneqBorder, size(M, 2)))
            M[(end - neq - nIneqBorder + 1):end, 1:nActive] = [Aeq[:, activeCoeffs];
                      Aineq[idxIneqBorder, activeCoeffs]]

            # calculate derivative
            dir = try
              dirsgn * (M \ [subgrad[setActive]; zeros(neq + nIneqBorder)])
            catch
              dir = -(pinv(M) * [subgrad[setActive]; zeros(neq + nIneqBorder)])
            end

            ## calculate derivative for rho*subgradient #%
            dirSubgrad = - hcat(H[inactiveCoeffs, activeCoeffs], Aeq[:, inactiveCoeffs]',
                                    Aineq[idxIneqBorder, inactiveCoeffs]') * dir

            ## Misc. housekeeping #%
            # check for violations again

            ## inactive coefficients moving too slowly
            # negative subgradient
            inactSlowNegIdx = find(((1*dirsgn - 1e-8) .<= subgrad[.~setActive]) .&
              (subgrad[.~setActive] .<= (1*dirsgn + 1e-8)) .&
              (1*dirsgn .< dirSubgrad))

            # positive subgradient
            inactSlowPosIdx = find(((-1*dirsgn - 1e-8) .<= subgrad[.~setActive]) .&
                (subgrad[.~setActive] .<= (-1*dirsgn + 1e-8)) .&
                (dirSubgrad .< -1*dirsgn))

            ## "active" coeficients estimated as 0 with potential sign mismatch #%
            # positive subgrad but negative derivative
            signMismatchPosIdx = find(((0 - 1e-8) .<= subgrad[setActive]) .&
                (subgrad[setActive] .<= (1 + 1e-8)) .&
                (dirsgn * dir[1:nActive] .<= (0 - 1e-8)) .&
                (βpath[activeCoeffs, k-1] .== 0))

            # Negative subgradient but positive derivative
            signMismatchNegIdx = find(((-1 - 1e-8) .<= subgrad[setActive]) .&
                (subgrad[setActive] .<= (0 + 1e-8)) .&
                ((0 + 1e-8) .<= dirsgn * dir[1:nActive]) .&
                (βpath[activeCoeffs, k-1] .== 0))

            # update violation counter
            violateCounter = violateCounter + 1;
            # break loop if needed
            if violateCounter >= maxiters
                break
            end

          end

          # Monitor & fix subgradient condition 2 violations
          while ~isempty(inactSlowPosIdx)
              # Identify & move problem coefficient #%
              # indices corresponding to inactive coefficients
              inactiveCoeffs = find(.!setActive)
              # identify problem coefficient
              viol_coeff = inactiveCoeffs[inactSlowPosIdx]
              # put problem coefficient back into active set;
              setActive[viol_coeff] = true
              # determine new number of active coefficients
              nActive = countnz(setActive)
              # determine number of active/binding inequality constraints
              nIneqBorder = countnz(setIneqBorder)

              # Recalculate derivative for coefficients & multiplier #%
              # construct matrix

              activeCoeffs = find(setActive)
              inactiveCoeffs = find(.!setActive)
              idxIneqBorder = find(setIneqBorder)
              M = hcat(H[activeCoeffs, activeCoeffs], Aeq[:, activeCoeffs]',
                Aineq[find(setIneqBorder), activeCoeffs]')
              M = vcat(M, zeros(neq + nIneqBorder, size(M, 2)))
              M[(end - neq - nIneqBorder + 1):end, 1:nActive] = [Aeq[:, activeCoeffs];
                        Aineq[idxIneqBorder, activeCoeffs]]

              # calculate derivative
              dir = try
                dirsgn * (M \ [subgrad[setActive]; zeros(neq + nIneqBorder)])
              catch
                dir = -(pinv(M) * [subgrad[setActive]; zeros(neq + nIneqBorder)])
              end

              # calculate derivative for rho*subgradient #
              dirSubgrad = - hcat(H[inactiveCoeffs, activeCoeffs], Aeq[:, inactiveCoeffs]',
                                        Aineq[idxIneqBorder, inactiveCoeffs]') * dir

              # Misc. housekeeping #%
              # check for violations again
              inactSlowPosIdx = find(((-1*dirsgn - 1e-8) .<= subgrad[.!setActive]) .&
                  (subgrad[.!setActive] .<= (-1*dirsgn + 1e-8)) .&
                  (dirSubgrad .< -1*dirsgn))

              # "Active" coeficients est'd as 0 with potential sign mismatch #%
              # Positive subgrad but negative derivative
              signMismatchPosIdx = find(((0 - 1e-8) .<= subgrad[setActive]) .&
                  (subgrad[setActive] .<= (1 + 1e-8)) .&
                  (dirsgn * dir[1:nActive] .<= (0 - 1e-8)) .&
                  (βpath[activeCoeffs, k-1] .== 0))

              # Negative subgradient but positive derivative
              signMismatchNegIdx = find(((-1 - 1e-8) .<= subgrad[setActive]) .&
                  (subgrad[setActive] .<= (0 + 1e-8)) .&
                  ((0 + 1e-8) .<= dirsgn * dir[1:nActive]) .&
                  (βpath[activeCoeffs, k-1] .== 0))

              # update violation counter
              violateCounter = violateCounter + 1;
              # break loop if needed
              if violateCounter >= maxiters
                  break
              end
          end

          # Monitor & fix condition 3 violations
          while ~isempty(signMismatchPosIdx)
              ## Identify & move problem coefficient #%
              # indices corresponding to active coefficients
              activeCoeffs = find(setActive)
              # identify prblem coefficient
              viol_coeff = activeCoeffs[signMismatchPosIdx]
              # put problem coefficient back into inactive set;
              setActive[viol_coeff] = false
              # determine new number of active coefficients
              nActive = countnz(setActive)
              # determine number of active/binding inequality constraints
              nIneqBorder = countnz(setIneqBorder)

              ## Recalculate derivative for coefficients & multipliers #%
              # construct matrix
              activeCoeffs = find(setActive)
              inactiveCoeffs = find(.!setActive)
              idxIneqBorder = find(setIneqBorder)
              M = hcat(H[activeCoeffs, activeCoeffs], Aeq[:, activeCoeffs]',
                Aineq[idxIneqBorder, activeCoeffs]')
              M = vcat(M, zeros(neq + nIneqBorder, size(M, 2)))
              M[(end - neq - nIneqBorder + 1):end, 1:nActive] = [Aeq[:, activeCoeffs];
                        Aineq[idxIneqBorder, activeCoeffs]]

              # calculate derivative
              dir = try
                dirsgn * (M \ [subgrad[setActive]; zeros(neq + nIneqBorder)])
              catch
                dir = -(pinv(M) * [subgrad[setActive]; zeros(neq + nIneqBorder)])
              end

              # calculate derivative for rho*subgradient (Eq. 10) #%
              dirSubgrad = - hcat(H[inactiveCoeffs, activeCoeffs], Aeq[:, inactiveCoeffs]',
                                        Aineq[idxIneqBorder, inactiveCoeffs]') * dir

              ## Misc. housekeeping #%
              # check for violations again
              signMismatchPosIdx = find(((0 - 1e-8) .<= subgrad[setActive]) .&
                      (subgrad[setActive] .<= (1 + 1e-8)) .&
                      (dirsgn * dir[1:nActive] .<= (0 - 1e-8)) .&
                      (βpath[activeCoeffs, k-1] .== 0))

              # Negative subgradient but positive derivative
              signMismatchNegIdx = find(((-1 - 1e-8) .<= subgrad[setActive]) .&
                      (subgrad[setActive] .<= (0 + 1e-8)) .&
                      ((0 + 1e-8) .<= dirsgn * dir[1:nActive]) .&
                      (βpath[activeCoeffs, k-1] .== 0))

              # update violation counter
              violateCounter = violateCounter + 1
              # break loop if needed
              if violateCounter >= maxiters
                  break
              end
          end

          # Monitor & fix condition 4 violations
          while ~isempty(signMismatchNegIdx)
              ## Identify & move problem coefficient #%
              # indices corresponding to active coefficients
              activeCoeffs = find(setActive)
              # identify prblem coefficient
              viol_coeff = activeCoeffs[signMismatchNegIdx]
              # put problem coefficient back into inactive set;
              setActive[viol_coeff] = false
              # determine new number of active coefficients
              nActive = countnz(setActive)
              # determine number of active/binding inequality constraints
              nIneqBorder = countnz(setIneqBorder)

              ## Recalculate derivative for coefficients & multipliers #%
              # construct matrix
              activeCoeffs = find(setActive)
              inactiveCoeffs = find(.!setActive)
              idxIneqBorder = find(setIneqBorder)
              M = hcat(H[activeCoeffs, activeCoeffs], Aeq[:, activeCoeffs]',
                Aineq[idxIneqBorder, activeCoeffs]')
              M = vcat(M, zeros(neq + nIneqBorder, size(M, 2)))
              M[(end - neq - nIneqBorder + 1):end, 1:nActive] = [Aeq[:, activeCoeffs];
                        Aineq[idxIneqBorder, activeCoeffs]]

              # calculate derivative
              dir = try
                dirsgn * (M \ [subgrad[setActive]; zeros(neq + nIneqBorder)])
              catch
                dir = -(pinv(M) * [subgrad[setActive]; zeros(neq + nIneqBorder)])
              end

              # calculate derivative for rho*subgradient #%
              dirSubgrad = - hcat(H[inactiveCoeffs, activeCoeffs], Aeq[:, inactiveCoeffs]',
                                        Aineq[idxIneqBorder, inactiveCoeffs]') * dir

              # Recheck for violations #%
              signMismatchNegIdx = find(((-1 - 1e-8) .<= subgrad[setActive]) .&
                        (subgrad[setActive] .<= (0 + 1e-8)) .&
                        ((0 + 1e-8) .<= dirsgn * dir[1:nActive]) .&
                        (βpath[activeCoeffs, k-1] .== 0))
              # update violation counter
              violateCounter = violateCounter + 1
              # break loop if needed
              if violateCounter >= maxiters
                  break
              end

          end

          ## update violation trackers to see if any issues persist ##%

          activeCoeffs = find(setActive)
          inactiveCoeffs = find(.!setActive)
          idxIneqBorder = find(setIneqBorder)

          # Inactive coefficients moving too slowly #%
          # Negative subgradient
          inactSlowNegIdx =
              find(((1 * dirsgn - 1e-8) .<= subgrad[.!setActive]) .&
              (subgrad[.~setActive] .<= (1 * dirsgn + 1e-8)) .&
              (1 * dirsgn .< dirSubgrad))
%         # Positive subgradient
          inactSlowPosIdx = find(((-1*dirsgn - 1e-8) .<= subgrad[.!setActive]) .&
              (subgrad[.!setActive] .<= (-1 * dirsgn + 1e-8)) .&
              (dirSubgrad .< -1 * dirsgn))

          # "Active" coefficients estimated as 0 with potential sign mismatch #%
          # Positive subgrad but negative derivative
          signMismatchPosIdx = find(((0 - 1e-8) .<= subgrad[setActive]) .&
              (subgrad[setActive] .<= (1 + 1e-8)) .&
              (dirsgn * dir[1:nActive] .<= (0 - 1e-8)) .&
              (βpath[activeCoeffs, k-1] .== 0))

          # Negative subgradient but positive derivative
          signMismatchNegIdx = find(((-1 - 1e-8) .<= subgrad[setActive]) .&
              (subgrad[setActive] .<= (0 + 1e-8)) .&
              ((0 + 1e-8) .<= dirsgn * dir[1:nActive]) .&
              (βpath[activeCoeffs, k-1] .== 0))

          # break loop if needed
          if violateCounter >= maxiters
              break
          end
      end # end of outer while loop

      # store number of violations
      violationspath[k] = violateCounter;

      # calculate derivative for residual inequality
      activeCoeffs = find(setActive)
      inactiveCoeffs = find(.!setActive)
      idxIneqBorder = find(setIneqBorder)

      dirResidIneq = Aineq[find(.!setIneqBorder), activeCoeffs] * dir[1:nActive]

      ### Determine rho for next event (via delta rho) ###%
      ## Events based on coefficients changing activation status ##%

      # clear previous values for delta rho
      nextρβ = fill(Inf, p, 1)

      # Active coefficient going inactive #%
      nextρβ[setActive] = -dirsgn * βpath[activeCoeffs, k-1] ./ dir[1:nActive]

      # Inactive coefficient becoming positive #%
      t1 = dirsgn * ρpath[k-1] * (1 - subgrad[inactiveCoeffs]) ./ (dirSubgrad - 1)
      # threshold values hitting ceiling
      t1[t1 .<= 1e-8] = Inf;

      # Inactive coefficient becoming negative #%
      t2 = -dirsgn * ρpath[k-1] * (1 + subgrad[.!setActive]) ./ (dirSubgrad + 1)
      # threshold values hitting ceiling
      t2[t2 .<= 1e-8] = Inf

      # choose smaller delta rho out of t1 and t2
      nextρβ[.!setActive] = min.(t1, t2)
      # ignore delta rhos numerically equal to zero
      nextρβ[(nextρβ .<= 1e-8) .| (.!penidx)] = Inf

      ## Events based inequality constraints ##%
	    # clear previous values
      nextρIneq = fill(Inf, nineq, 1)

      # Inactive inequality constraint becoming active #%
      nextρIneq[.!setIneqBorder] = reshape(-dirsgn * residIneq[.!setIneqBorder],
                countnz(.!setIneqBorder), 1) ./
                reshape(dirResidIneq, countnz(.~setIneqBorder), 1)

      # Active inequality constraint becoming deactive #%
      if !isempty(μpathineq)
      nextρIneq[setIneqBorder] =
      - dirsgn * μpathineq[idxIneqBorder, k-1] ./
                reshape(dir[nActive+neq+1:end], nIneqBorder, 1)
      end

      # ignore delta rhos equal to zero
      nextρIneq[nextρIneq .<= 1e-8] = Inf

      ## determine next rho ##
      # find smallest rho
      chgρ = findmin([nextρβ; nextρIneq])[1]
      # find all indices corresponding to this chgρ
      idx = find(([nextρβ; nextρIneq] - chgρ) .<= 1e-8)

      # terminate path following if no new event found
      if chgρ == Inf
         chgρ = ρpath[k-1]
      end

      ## Update values at new rho ##%
      # move to next rho #%
      # make sure next rho isn't negative
      if ρpath[k-1] + dirsgn * chgρ < 0
          chgρ = ρpath[k-1]
      end
      # calculate new value of rho
      ρpath[k] = ρpath[k-1] + dirsgn * chgρ

      ## Update parameter and subgradient values #%
      # new coefficient estimates
      activeCoeffs = find(setActive)

      βpath[activeCoeffs, k] = βpath[activeCoeffs, k-1] +
            dirsgn * chgρ * dir[1:nActive]
      # force near-zero coefficients to be zero (helps with numerical issues)
      βpath[abs.(βpath[:, k]) .< 1e-12, k] = 0

      # new subgradient estimates
      subgrad[.!setActive] = (ρpath[k-1] * subgrad[.!setActive] +
            dirsgn * chgρ * dirSubgrad) / ρpath[k]

      # Update dual variables #%
      # update lambda (lagrange multipliers for equality constraints)
      if !isempty(λpatheq)
        λpatheq[:, k] = λpatheq[:, k-1] +
            dirsgn * chgρ * reshape(dir[nActive + 1:nActive + neq], neq, 1)
      end
      # update mu (lagrange multipliers for inequality constraints)
      if !isempty(μpathineq)
        μpathineq[idxIneqBorder, k] = μpathineq[idxIneqBorder, k-1] +
            dirsgn * chgρ * reshape(dir[nActive + neq + 1:end], nIneqBorder, 1)
      end
      # update residual inequality
      residIneq = Aineq * βpath[:, k] - bineq

      ## update sets ##%
      for j = 1:length(idx)
          curidx = idx[j]
          if curidx <= p && setActive[curidx]
              # an active coefficient hits 0, or
              setActive[curidx] = false
          elseif curidx <= p && ~setActive[curidx]
              # a zero coefficient becomes nonzero
              setActive[curidx] = true
          elseif curidx > p
              # an ineq on boundary becomes strict, or
              # a strict ineq hits boundary
              setIneqBorder[curidx - p] = !setIneqBorder[curidx - p]
          end
      end

      # determine new number of active coefficients
      nActive = countnz(setActive)
      # determine number of active/binding inequality constraints
      nIneqBorder = countnz(setIneqBorder)

      ## Calcuate and store values of interest along the path #%
      # calculate value of objective function
      objvalpath[k] = norm(y - X * βpath[:, k])^2/2 +
            ρpath[k] * sum(abs.(βpath[:, k]))

      # calculate degrees of freedom
      dfpath[k] = nActive - rankAeq - nIneqBorder
      # break algorithm when df are exhausted
      if dfpath[k] >= n
          break
      end

    end # end of big for loop

       βpath = βpath[:, 1:k-1]
       deleteat!(ρpath, k:length(ρpath))
       deleteat!(objvalpath, k:length(objvalpath))
       deleteat!(dfpath, k:length(dfpath))
       dfpath[dfpath .< 0] = 0;

    return βpath, ρpath, objvalpath, λpatheq, μpathineq, dfpath, violationspath

end # end of the function

"""
```
  find_ρmax(
    X      :: AbstractMatrix,
    y      :: AbstractArray;
    Aeq    :: AbstractMatrix = zeros(eltype(X), 0, size(X, 2)),
    beq    :: Union{AbstractArray, Number} = zeros(eltype(X), size(Aeq, 1)),
    Aineq  :: AbstractMatrix = zeros(eltype(X), 0, size(X, 2)),
    bineq  :: Union{AbstractArray, Number} = zeros(eltype(X), size(Aineq, 1)),
    penidx :: Array{Bool} = fill(true, size(X, 2)),
    solver = ECOSSolver(maxit=10e8, verbose=0)
    )
```
Find the maximum tuning parameter value `ρmax` to kick-start the solution path.
"""
function find_ρmax(
    X::AbstractMatrix,
    y::AbstractArray;
    Aeq::AbstractMatrix = zeros(eltype(X), 0, size(X, 2)),
    beq::Union{AbstractArray, Number} = zeros(eltype(X), size(Aeq, 1)),
    Aineq::AbstractMatrix = zeros(eltype(X), 0, size(X, 2)),
    bineq::Union{AbstractArray, Number} = zeros(eltype(X), size(Aineq, 1)),
    penidx::Array{Bool} = fill(true, size(X, 2)),
    solver = ECOSSolver(maxit=10e8, verbose=0)
    )

    p = size(X, 2)

    x = Variable(p)
    problem = minimize(dot(ones(p), abs(x)), Aeq * x == beq,
              Aineq * x <= bineq)

    problem = minimize(dot(ones(eltype(X), size(X, 2)), abs(x)))
    if !isempty(Aeq)
      problem.constraints += Aeq * x == beq
    end
    if !isempty(Aineq)
      problem.constraints += Aineq * x <= bineq
    end

    # TT = STDOUT # save original STDOUT stream
    # redirect_stdout()
    solve!(problem, solver)
    β = x.value
    # redirect_stdout(TT) # restore STDOUT

    λeq = zeros(eltype(X), 0, 1)
    μineq = zeros(eltype(X), 0, 1)

    for i in 1:min(2, length(problem.constraints))
      if eval((problem.constraints[i]).head) == ==
        λeq = problem.constraints[i].dual
      elseif eval((problem.constraints[i]).head) == <=
        μineq = problem.constraints[i].dual
      end
    end

    if !isempty(μineq)
      μineq[μineq .< 0] = 0
    end
    setActive = (abs.(β) .> 1e-4) .| (.~penidx)
    β[.!setActive] = 0

    resid = y - X * β
    subgrad = X' * resid - Aeq' * λeq - Aineq' * μineq
    ρmax, indρmax = findmax(abs.(subgrad))

    return ρmax, indρmax, problem.optval, λeq, μineq
end


struct FTL
    L :: Array{Float64, 1};

    function FTL(K)
        new(zeros(K));
    end
end

function act(h::FTL)
    u = h.L .== minimum(h.L);
    u ./ sum(u);
end

function incur!(h::FTL, l)
    h.L .+= l;
end

mutable struct AdaHedge
    L :: Array{Float64, 1};
    Δ :: Float64;

    function AdaHedge(K)
        new(zeros(K), 0.01);
    end
end

function act(h::AdaHedge)
    η = log(length(h.L))/h.Δ;
    u = exp.(-η.*(h.L .- minimum(h.L)));
    u ./ sum(u);
end

function incur!(h::AdaHedge, l)
    u = act(h);
    η = log(length(h.L))/h.Δ;
    h.L .+= l;
    m = minimum(l) - 1/η*log(u'exp.(-η.*(l .- minimum(l))));
    # println(η, " ", u, " ", u'l, " ", -1/η*log(u'exp.(-η.*l)));
    @assert isfinite(m) && u'l ≥ m - 1e-7 "hedge loss $(u'l), Δ=$(h.Δ), η=$η, mix loss $m, u=$u, L=$(h.L)";
    h.Δ  += u'l - m;
end

# Menard calls this "Lazy Mirror Descent" As perhaps the main feature
# here is the mixing step, we call it "Fixed Share".  This is the
# version with slowly decreasing O(1/sqrt(t)) learnign and switching
# rates.  The switching is used to force exploration
mutable struct FixedShare
    w :: Array{Float64, 1};
    t; # time
    S; # loss range: S ≥ \max_k l_k - \min_k l_k
       # TODO: adapt to it (like AdaHedge?)

    function FixedShare(K; S=1)
        new(fill(1/K,K), 0, S);
    end
end

function act(h::FixedShare)
    h.w
end

function incur!(h::FixedShare, l)
    K = length(l);
    h.t += 1;

    # Exponential Weights update
    η = sqrt(log(K)/h.t)/h.S;   # decaying learning rate
    h.w .*= exp.(-η*(l .- minimum(l)));
    h.w ./= sum(h.w);

    # Fixed Share (to unifor prior)
    γ = 1/(4*sqrt(h.t)); # decaying switching rate
    h.w .= (1-γ).*h.w .+ γ/K;
end


"""
Non-negative CANDECOMP tensor decomposition.
"""
function nncp(tnsr::StridedArray,
              r::Integer;
              tol::Float64=1e-4,
              maxiter::Integer=100,
              compute_error::Bool=false,
              verbose::Bool=true)

    minimum(tnsr) >= 0 || error("Input tensor must be nonnegative.")
    num_modes = _check_tensor(tnsr, r)
    T_norm = vecnorm(tnsr)

    factors = [abs(F) * (T_norm ^ (1/num_modes) / vecnorm(F)) for F::Matrix{Float64} in _random_factors(size(tnsr), r)]
    factors_old = deepcopy(factors)
    factors_exp = deepcopy(factors)
    gram = Matrix{Float64}[F'F for F in factors]

    niters = 0
    converged = false
    obj_old = 0.0
    t = 1.0
    t_old = 1.0

    LB = ones(num_modes)
    LB_old = ones(num_modes)

    U = Array(Float64, num_modes, num_modes)
    M = Array(Float64, r, r)
    while !converged && niters < maxiter
        LB_old = copy(LB)
        for i in 1:num_modes
            idx = [num_modes:-1:i+1; i-1:-1:1]
            U = reduce(.*, gram[idx])
            LB[i] = vecnorm(U)
            M = _row_unfold(tnsr, i) * reduce(khatrirao, factors[idx])
            factors[i] = max(0, factors_exp[i] - (factors_exp[i] * U - M) * (1 / LB[i]))
            At_mul_B!(gram[i], factors[i], factors[i])
        end

        obj = sum(gram[num_modes] .* U) - 4 * sum(factors[num_modes] .* M)

        if obj > obj_old
            factors_exp = factors_old
        else
            t = 1 + sqrt(1 + 4 * t_old^2)
            weights = min((t_old - 2) / t, sqrt( LB_old ./ LB ))
            factors_exp = map((F, F_old, w) -> F + w * (F - F_old), factors, factors_old, weights)
            factors_old = deepcopy(factors)
            converged = obj_old - obj < tol * abs(obj_old)
            t_old = t
            obj_old = obj
        end
        niters += 1

    end

    verbose && _iter_status(converged, niters, maxiter)

    res = CANDECOMP((factors...), ones(r))
    if compute_error
      _set_rel_residue(res, tnsr)
    end
    return res
end


# This file is a part of JuliaFEM.
# License is MIT: see https://github.com/JuliaFEM/FEMBasis.jl/blob/master/LICENSE

using FEMBasis: vandermonde_matrix
using Test

polynomial = :(1 + u + v + u*v)
coordinates = ((-1.0,-1.0), (1.0,-1.0), (1.0,1.0), (-1.0,1.0))
V = vandermonde_matrix(polynomial, coordinates)
@debug "create Vandermonde matrix" polynomial coordinates V
V_expected = [
    1.0 -1.0 -1.0  1.0
    1.0  1.0 -1.0 -1.0
    1.0  1.0  1.0  1.0
    1.0 -1.0  1.0 -1.0]
@test isapprox(V, V_expected)

polynomial = :(1 + u + v)
coordinates = ((0.0, 0.0), (1.0, 0.0), (0.0, 1.0))
V = vandermonde_matrix(polynomial, coordinates)
V_expected = [
    1.0 0.0 0.0
    1.0 1.0 0.0
    1.0 0.0 1.0]
@test isapprox(V, V_expected)


#=
# Tests vector unknown capability with a simple Poisson-like problem.
=#
if !@isdefined(Femshop)
    include("../Femshop.jl");
    using .Femshop
end
init_femshop("vector");

# Try making an optional log
useLog("vectorlog")

# Set up the configuration
domain(2)
functionSpace(order = 3)

# Specify the problem
mesh(QUADMESH, elsperdim=20)

u = variable("u", VECTOR)
p = variable("p")

testSymbol("v", VECTOR)
testSymbol("w")

boundary(u, 1, DIRICHLET, [0, 0])
boundary(p, 1, DIRICHLET, 0)

# Write the weak form
coefficient("f", ["-25*pi*pi*sin(pi*x)*sin(2*pi*y)", "-125*pi*pi*sin(3*pi*x)*sin(4*pi*y)"], VECTOR)
coefficient("g", "-2*pi*pi*sin(pi*x)*sin(pi*y)")
coefficient("a", 5)

weakForm([u,p], ["-a*inner(grad(u), grad(v)) - dot(f,v)", "-dot(grad(p), grad(w)) - g*w"])

solve([u,p]);

# # exact solution is [sin(pi*x)*sin(2*pi*y), sin(3*pi*x)*sin(4*pi*y)]
# # check error
erroru = zeros(size(u.values));
maxerru = 0
maxerrp = 0
exactu1(x,y) = sin(pi*x)*sin(2*pi*y);
exactu2(x,y) = sin(3*pi*x)*sin(4*pi*y);
exactp(x,y) = sin(pi*x)*sin(pi*y);

for i=1:size(Femshop.grid_data.allnodes,2)
    x = Femshop.grid_data.allnodes[1,i];
    y = Femshop.grid_data.allnodes[2,i];
    exac = [exactu1(x,y), exactu2(x,y)];
    for j=1:size(Femshop.grid_data.allnodes,1)
        erroru[j,i] = u.values[j,i] - exac[j];
        global maxerru;
        maxerru = max(abs(erroru[j,i]),maxerru);
    end
    global maxerrp;
    maxerrp = max(abs(p.values[i] - exactp(x,y)),maxerrp);
end
println("u max error = "*string(maxerru));
println("p max error = "*string(maxerrp));

# using Plots
# pyplot();
# display(plot(Femshop.grid_data.allnodes[1,:], Femshop.grid_data.allnodes[2,:], u.values[1,:], st=:surface))

# check
# log_dump_config();
# log_dump_prob();

output_values([u,p], "vector2d", format="vtk");

finalize_femshop();


abstract type FrequencyEstimator <: InverterComponent end

@def freq_estimation_ports begin
    state_input = [:vd_cap,:vq_cap, :δθ_vsm]
    #TODO: Move PLL to PCC, i.e. move v_cap (D'Arco v_o), to inner inputs
    inner_input = [Vdo_var,Vqo_var,δdqRI_var,ω_freq_estimator_var]
end

"""
Parameters of a Phase-Locked Loop (PLL) for VSM
# Conmutable structor
```julia
PLL(ω_lp, kp_pll, ki_pll)
```

# Arguments
*  `ω_lp`::Float64 : PLL low-pass filter frequency (rad/sec)
*  `kp_pll`::Float64 : PLL proportional gain
*  `ki_pll`::Float64 : PLL integral gain
"""
mutable struct PLL <: FrequencyEstimator
    ω_lp::Float64
    kp_pll::Float64
    ki_pll::Float64
    n_states::Int64
    states::Vector{Symbol}
    ports::Ports

        function PLL(ω_lp::Float64,
                     kp_pll::Float64,
                     ki_pll::Float64)

            @freq_estimation_ports

            new(ω_lp,
                kp_pll,
                ki_pll,
                4,
                [:vpll_d, :vpll_q, :ε_pll, :δθ_pll],
                Ports(state_input, inner_input))

        end

end


function get_cigar_funcs(dimensions)
    fs = Dict()

    function cigar()
        f(x) = x[1]^2 + 10^6 * sum(x[2:end].^2)
        return f
    end

    for d in dimensions
        fs[d] = cigar()
    end

    return fs
end

function get_linear_funcs(dimensions)
    fs = Dict()

    function linear()
        f(x) = sum(x)
        return f
    end

    for d in dimensions
        fs[d] = linear()
    end

    return fs
end

function get_box_funcs(dimensions, alpha=1/2)
    fs = Dict()

    function box()
        f(x) = sum(abs.(x)) <= alpha
        return f
    end

    for d in dimensions
        fs[d] = box()
    end

    return fs
end

function get_step_funcs(dimensions)
    fs = Dict()

    function step()
        f(x) = begin
            v = sum(x)
            if v < 0
                return 0
            elseif v < 2
                return 2
            elseif v < 4
                return 4
            else
                return 5
            end
        end

        return f
    end

    for d in dimensions
        fs[d] = step()
    end

    return fs
end

function get_sinc_funcs(dimensions)
    fs = Dict()

    function msinc()
        f(x) = *(sinc.(x)...)
        return f
    end

    for d in dimensions
        fs[d] = msinc()
    end

    return fs
end

function get_sphere_funcs(dimensions)
    fs = Dict()

    function sphere()
        f(x) = sum(x.^2)
        return f
    end

    for d in dimensions
        fs[d] = sphere()
    end

    return fs
end

function get_rastrigin_funcs(dimensions, A=10.0)
    fs = Dict()

    function rastrigrin(n)
        f(x) = A*n + sum(x.^2 - A * cos.(2π * x))
        return f
    end

    for d in dimensions
        fs[d] = rastrigrin(d)
    end

    return fs
end

function get_rosenbrock_funcs(dimensions)
    fs = Dict()

    function rosenbrock(n)
        f(x) = sum([100 * (x[i+1] - x[i]^2) + (1 - x[i])^2 for i in 1:(n-1)])
        return f
    end

    for d in dimensions
        fs[d] = rosenbrock(d)
    end

    return fs
end

function get_styblinskitang_funcs(dimensions)
    fs = Dict()

    function styblinskitang()
        f(x) = sum(x.^4 - 16x.^2 + 5x) / 2
        return f
    end

    for d in dimensions
        fs[d] = styblinskitang()
    end

    return fs
end


using Distributions: Uniform, Normal, MultivariateNormal
using LazySets: DefaultUniform

for N in [Float64]
    P1 = BallInf([0.6, 0.1, -1.3, -0.4, 0.2], 0.6)
    A = [1.0  0.0;
         0.0  1.0;
        -1.0  0.0;
         0.0 -1.0]
    b = [1.0, 2.0, 3.0, 4.0]
    P2 = HPolyhedron(A, b)
    P3 = HPolyhedron([0.0 0.1], [3.0])
    P4 = Ball2([0.2, -0.3, -1.1, 0.6, -0.7], 0.4)

    # Test rand samples are contained in the set
    p1 = LazySets.sample(P1)
    @test p1 ∈ P1
    p1_samples = LazySets.sample(P1, 100)
    @test sum(p1_samples .∈ Ref(P1)) == length(p1_samples)

    # default distribution
    @test LazySets.RejectionSampler(P2).distribution == [DefaultUniform(-3.0,1.0), DefaultUniform(-4.0,2.0)]

    # polytope sampler
    p1_samples = LazySets.sample(P1, 100, sampler=LazySets.RandomWalkSampler())
    @test all(v ∈ P1 for v in p1_samples)

    # specifying a distribution from Distributions.jl
    @test LazySets.RejectionSampler(P2, Uniform).distribution == [Uniform(-3.0, 1.0), Uniform(-4.0,2.0)]
    @test LazySets.RejectionSampler(P2, Normal).distribution == [Normal(-3.0, 1.0), Normal(-4.0, 2.0)]

    # test univariate distributions
    X = Interval(1, 2.)
    v = sample(X, 5)
    @test all(vi ∈ X for vi in v)
    d = Normal(1.5, 0.5)

    v = sample(X, 5, sampler=LazySets.RejectionSampler(d))
    @test all(vi ∈ X for vi in v)

    v = sample(X, 5, sampler=LazySets.RejectionSampler([d]))
    @test all(vi ∈ X for vi in v)

    v = sample(X, 5, sampler=LazySets.RejectionSampler([DefaultUniform(1, 2)]))
    @test all(vi ∈ X for vi in v)

    v = sample(X, 5, sampler=LazySets.RejectionSampler(DefaultUniform(1, 2)))
    @test all(vi ∈ X for vi in v)

    # test multivariate distribution
    H = rand(Hyperrectangle, dim=2)
    sampler = LazySets.RejectionSampler(MultivariateNormal(N[1 0; 0 1.]), tight=true)
    v = sample(H, 5, sampler=sampler)

    # including vertices
    for k in 0:4
        p1 = sample(P1, 10; include_vertices=k)
        @test length(p1) == 10 + k
    end
    @test length(sample(P1, 10; include_vertices=false)) == 10
    @test length(sample(P1, 10; include_vertices=true)) == 42
end


module QFT

export gen_qft, gen_iqft
using ..GateOps

"""
    gen_qft(qubit_indices::Vector{Int})

Generates an OpenQASM representation of the quantum Fourier transform
for the given number of qubits.

"""
function gen_qft(qubit_indices::Vector{Int})
    circuit = Vector{GateOps.GateCall}()

    angle(k) = 2*pi / 2^k;
    for (iidx,ival) = Iterators.reverse(enumerate(qubit_indices))
        push!(circuit, GateOps.hadamard(qubit_indices[iidx]))
        for (jidx,jval) = Iterators.reverse(enumerate(qubit_indices[1:iidx-1]))
            theta = angle(iidx - jidx);
            push!(circuit, GateOps.c_r_phase(ival, jval, theta))
        end
    end
    for idx = 1:div(length(qubit_indices),2)
        push!(circuit, GateOps.swap(qubit_indices[idx], qubit_indices[length(qubit_indices)-idx+1]))
    end
    return circuit
end

"""
    gen_iqft(qubit_indices::Vector{Int})

Generates an OpenQASM representation of the inverse quantum Fourier transform
for the given number of qubits.

"""
function gen_iqft(qubit_indices::Vector{Int})
    circuit = Vector{GateOps.GateCall}()

    angle(k) = -2*pi / 2^k;
    for (iidx,ival) = enumerate(qubit_indices)
        for (jidx,jval) = enumerate(qubit_indices[1:iidx-1])
            theta = angle(iidx - jidx);
            push!(circuit, GateOps.c_r_phase(ival, jval, theta))
        end
        circuit *= GateOps.apply_gate_h(q_reg, qubit_indices[iidx])

    end
    for idx = 1:div(length(qubit_indices),2)
        push!(circuit, GateOps.swap(qubit_indices[idx], qubit_indices[length(qubit_indices)-idx+1]))
    end
    return circuit
end

end


using MultipleViewGeometry, Test, LinearAlgebra
using MultipleViewGeometry.ModuleTypes

# Construct two camera matrices and parametrise two planar surfaces.
𝐊₁ = Matrix{Float64}(I, 3, 3)
𝐑₁ = Matrix{Float64}(I, 3, 3)
𝐭₁ = [0.0, 0.0, 0.0]
𝐊₂ = Matrix{Float64}(I, 3, 3)
𝐑₂ = Matrix{Float64}(I, 3, 3)
𝐭₂ = [2.0, 2.0, 2.0]
𝐧₁ = [1.0, 0.0, 0.0]
d₁ = 10
𝐧₂ = [0.5, 0.5, 0.0]
d₂ = 15

𝐏₁ = construct(ProjectionMatrix(),𝐊₁,𝐑₁,𝐭₁)
𝐏₂ = construct(ProjectionMatrix(),𝐊₂,𝐑₂,𝐭₂)

# We will construct a pair of homography matrices and then construct a pair of
# projection matrices from the homographies.
𝐇₁ = construct(HomographyMatrix(),𝐊₁,𝐑₁,𝐭₁,𝐊₂,𝐑₂,𝐭₂,𝐧₁,d₁)
𝐇₂ = construct(HomographyMatrix(),𝐊₁,𝐑₁,𝐭₁,𝐊₂,𝐑₂,𝐭₂,𝐧₂,d₂)

# 1. Construct a Fundamental matrix from Camera matrices.
# 2. Construct Projection matrices from the Homography matrices
# 3. Construct a Fundamental matrix from the projection matrices.
# 4. The Fundamental matrices in step 1 and 3 should be equivalent up to sign
#    and scale.
𝐅 = construct(FundamentalMatrix(),𝐊₁,𝐑₁,𝐭₁,𝐊₂,𝐑₂,𝐭₂)

context = ProjectionMatrices(HomographyMatrices(), Chojnacki(), TwoViews())
𝐎₁, 𝐎₂ = construct(context, (𝐇₁,𝐇₂))
𝐅₂ = construct(FundamentalMatrix(), 𝐎₁, 𝐎₂)

# Ensure the matrices have the same scale and sign before comparing them.
𝐅 = 𝐅 / norm(𝐅)
𝐅 = 𝐅 / sign(𝐅[1,2])

𝐅₂ = 𝐅₂ / norm(𝐅₂)
𝐅₂ = 𝐅₂ / sign(𝐅₂[1,2])

@test 𝐅 ≈ 𝐅₂


module Bracketing

abstract type BracketingMethod end

struct Bracket
    left::Real
    right::Real

    function Bracket(a, b)
        if a <= b
            new(a, b)
        else
            new(b, a)
        end
    end
end

function search(params::BracketingMethod, f, x_0 = 0)::Real
    bracket = find_bracket(f, x_0)
    search_bracket(params, f, bracket).right
end

function find_bracket(f, x; step = 1e-2, factor = 2.0, max_iter = 1_000_000)::Bracket
    """
    Algorithm 3.1

    `max_iter` is a very high number since upon reaching this limit the algorithm
    will crash with error. This is because a failure to find a bracket means there's
    something wrong with either the algorithm, or the objective function.
    """
    a, ya = x, f(x)
    b, yb = a + step, f(a + step)
    if yb > ya
        a, b = b, a
        ya, yb = yb, ya
        step = step > 0 ? -step : step
    end
    for _ = 1:max_iter
        c, yc = b + step, f(b + step)
        if yc > yb
            return Bracket(a, c)
        end
        a, ya, b, yb = b, yb, c, yc
        step *= factor
    end
    error("Bracketing.find_bracket: max number of iterations reached")
end

struct GoldenSection <: BracketingMethod
    ϵ
    max_iter
    GoldenSection(; ϵ = eps(), max_iter = 100) = new(ϵ, max_iter)
end

function search_bracket(params::GoldenSection, f, bracket::Bracket)::Bracket
    """
    Golden section search
    Algorithm 3.3
    """
    a, b = bracket.left, bracket.right
    max_eval = (b - a) / (params.ϵ * log(Base.MathConstants.golden))
    n = params.max_iter > max_eval ? max_eval : params.max_iter

    ρ = Base.MathConstants.golden - 1
    d = ρ * b + (1 - ρ) * a
    yd = f(d)
    for _ = 1:n - 1
        if abs(a - b) < params.ϵ
            return Bracket(a, b)
        end
        c = ρ * a + (1 - ρ) * b
        yc = f(c)
        if yc < yd
            b, d, yd = d, c, yc
        else
            a, b = b, c
        end
    end
    Bracket(a, b)
end

end # module


abstract AbstractMDP

# -----------------------------------------------------
# Methods that should work for all types of AbstractMDP
# -----------------------------------------------------

reward(mdp::AbstractMDP, state, action) = reward(mdp.reward, state, action)
probability(mdp::AbstractMDP, s, t, a) = probability(mdp.transition, s, t, a)
num_states(mdp::AbstractMDP) = mdp.n_states
num_actions(mdp::AbstractMDP) = mdp.n_actions

# ------------
# The MDP type
# ------------

immutable MDP <: AbstractMDP
    transition::AbstractTransitionProbability
    reward::AbstractReward
    n_states::Int
    n_actions::Int

    function MDP(transition::AbstractTransitionProbability, reward::AbstractReward)
        ismdp(transition, reward) || error("Not a valid MDP.")
        new(transition, reward, num_states(transition), num_actions(transition))
    end
end

MDP(transition, reward) = MDP(TransitionProbability(transition), Reward(reward))

# Methods
# -------

@doc """
# Value iteration algorithm from Bellman 1957 "A Markovian decision process."

## Arguments

* `Q::AbstractQFunction`: The Q-function
* `mdp::MDP`: The Markov decision process of type MDP.
* `δ`: The discount factor, 0 < discount ≤ 1.
* `ϵ`: The epsilon-optimal stopping value. Default: 0.01
* `max_iter`: The maximum number of iterations. Default: 1000.

## Returns

* `Q::AbstractQFunction`: The Q-function (modified in-place).

""" ->
function value_iteration!(Q::AbstractQFunction, mdp::MDP, δ; ϵ=0.01, max_iter=1000)
    @assert 0 < δ <= 1 "ERROR: δ not in interval (0, 1]"
    @assert ϵ > 0 "ERROR: ϵ not greater than 0"
    @assert max_iter > 0 "ERROR: max_iter not greater than 0"
    # Calculate the stopping threshold
    threshold = δ < 1.0 ? ϵ*(1.0 - δ)/δ : ϵ
    val = Array(valuetype(Q), num_states(mdp))
    value!(val, Q)  # initial value
    # Find the ϵ-optimal value function
    itr = 0
    while true
        itr += 1
        bellman!(Q, val, mdp.transition, mdp.reward, δ)
        val_new = value(Q)
        if span(val_new - val) < threshold
            break
        elseif itr == max_iter
            warn("Reached maximum number of iterations, ϵ-optimal policy not found.")
            break
        end
        copy!(val, val_new)
    end
    return Q
end

function value_iteration(
    mdp::MDP,
    δ;
    ϵ=0.01,
    max_iter=1000,
    initial_value::Nullable{Vector}=Nullable{Vector}(),
)
    S = num_states(mdp)
    val = isnull(initial_value) ? zeros(S) : get(initial_value)
    return value_iteration!(QFunction(val, zeros(Int, S)), mdp, δ, ϵ=ϵ, max_iter=max_iter)
end


#=
	author: Jonathan Payne
=#

using Roots

function rk4step(fdash, t, x, h)
    # Calculates the the nth step in the classical fourth order Runga-Kutta
    # procedure for solving a differential equation of the form:
    #   x'(t) = fdash(x, t),
    # Inputs:
    #   * fdash: user defined function
    #   * t: previous value of independent variable
    #   * x: previous value of dependent variable
    #   * h: increment
    # Outputs:
    #   * xnew: next step
    #   * tnew: new time
    k1 = h*fdash(x, t)
    k2 = h*fdash(x + k1/2, t + h/2)
    k3 = h*fdash(x + k2/2, t + h/2)
    k4 = h*fdash(x + k3, t + h)
    xnew = x + (k1 + 2*k2 + 2k3 + k4) / 6
    tnew = t + h
    return xnew, tnew
end

function rk4uniform(fdash, x0, tspan, N=100)
    # Approximate the solution of the initial value problem using the
    # classical fourth order Runge-Kutta method with uniform step sizes:
    #   * x'(t) = fdash(x, t),
    #   * x(t0) = x0
    # Inputs:
    #   * fdash: user defined function
    #   * t0: initial value of the independent variable
    #   * x0: initial value of the dependent variable(s) (column vector)
    #   * tf: final value of the independent variable
    #   * N: number of universally sized time steps to be taken to advance the
    #        the solution from t = t0 to t = tf
    # Outputs:
    #   * wi: vector / matrix containing the values of the approximate solution
    #         to the ode
    #   * ti: number of uniformly sized time steps to be taken to advance t
    t0, tf = tspan
    neqn = length(x0)
    ti = linspace(t0, tf, N+1)
    wi = zeros(neqn, N+1)
    wi[1:neqn, 1] = x0
    h = (tf - t0) / N
    for i = 1:N
        x0, t0 = rk4step(fdash, t0, x0, h)
        wi[1:neqn, i+1] = x0
    end
    return wi, ti
end

# ------------------------------
# Usage example 1:
# ------------------------------
t0, x0, tf, N = 0, [0,-4], 10, 100
A = [[1 2], [3 2]]
fdash(x, t) = A*x
xtrue(t) = [ 8/5*exp(-t) - 8/5*exp(4*t), -8/5*exp(-t) - 12/5*exp(4*t) ]
tspan = [t0, tf]
wi2, ti2 = rk4uniform(fdash, x0, tspan, N)

using ODE
t0, x0, tf, N = 0, [0,-4], 10, 100
A = [[1 2], [3 2]]
fdash(t, x) = A*x
xtrue(t) = [ 8/5*exp(-t) - 8/5*exp(4*t), -8/5*exp(-t) - 12/5*exp(4*t) ]
tspan = [t0, tf]
span = linspace(t0, tf, N)
ode23s(fdash, x0, span)

# ------------------------------
# odelay (from the pycse package)
# ------------------------------
# Solve an ODE with events.
#
# Inputs:
# func        = is callable, with signature func(Y, x)
# y0          = are the initial conditions
# xspan       = what you want to integrate over
# events      = is a list of callable functions with signature event(Y, x).
# 			    These functions return zero when an event has happened.
# [value, isterminal, direction] = event(Y, x)
# value       = the value of the event function. When value = 0, an event
# 			    is triggered
# isterminal  = True if the integration is to terminate at a zero of
# 			    this event function, otherwise, False.
# direction   = 0 if all zeros are to be located (the default),
#			  = +1 if only zeros where the event function is increasing, and
#             = -1 if only zeros where the event function is decreasing.
# TOLERANCE   = what is used to identify when an event has occurred.
# fsolve_args = a dictionary of options for fsolve
# kwargs  	  = additional options you want to send to odeint.
#
# Returns [x, y, te, ye, ie]
# x           = the independent variable array
# y           = the solution
# te          = an array of independent variable values where events occurred
# ye          = an array of the solution at the points where events occurred
# ie          = an array of indices indicating which event function occurred.

function odelayold(func, y0, xspan,
				 events, TOLERANCE = 1e-6)#, fsolve_args=[], args...)
	#if fsolve_args == None
    #    fsolve_args = []
    #end
    N = length(xspan)
    x0 = xspan[1] # initial point

    sol = [float64(y0)]
    TE, YE, IE = [], [], [] # to store where events occur

    # initial value of events
    e = zeros(length(events), N)
    for (i, event) in enumerate(events)
        e[i, 1], isterminal, direction = event(y0, x0)
    end

    # now we step through the integration
	for (i, x1) in enumerate(xspan[1:end-1])
		println("i", i)
        x2 = xspan[i + 1]
        f1 = sol[i]

        f2, wz = rk4uniform(func, f1', [x1, x2], 1)#, args...)

        #X[i+1] = x2
        #println(X[i+1])
        X = push!(X, x2)
        sol = push!(sol, f2[:,end][1])

        # check event functions. At each step we compute the event
        # functions, and check if they have changed sign since the
        # last step. If they changed sign, it implies a zero was
        # crossed.
        for (j, event) in enumerate(events)
     		e[j, i + 1], isterminal, direction = event(sol[i + 1], X[i + 1])

            if ((e[j, i + 1] * e[j, i] < 0) || abs(e[j, i + 1]) < TOLERANCE || abs(e[j, i]) < TOLERANCE)
                xLt = X[end]       # Last point
                fLt = sol[end]
                eLt = e[j, i+1]

	            function objective(x)
	            	# evaluate ode from xLT to x
	             	txspan = [xLt, x]
	             	tempsol, wyz = rk4uniform(func, fLt, txspan, length(txspan))#, args...)
	             	tsol = tempsol[:, end]
	             	val, isterminal, direction = event(tsol, x)
	             	return val[1]
	            end
	            xZ = fzero(objective, xLt)#, **fsolve_args)  # this should be the
                                              # value of x that makes
                                              # the event zero
                # now evaluate solution at this point, so we can
                # record the function values here.
                txspan = [xLt, xZ]
                tempsol, wyz = rk4uniform(func, fLt, txspan)#, args...)
                fZ = tempsol[:,end]

                vZ, isterminal, direction = event(fZ, xZ)

                COLLECTEVENT = false
                if direction == 0
                    COLLECTEVENT = true
                elseif (e[j, i + 1] > e[j, i] ) && direction == 1
                    COLLECTEVENT = true
                elseif (e[j, i + 1] < e[j, i] ) && direction == -1
                    COLLECTEVENT = true
                end

                if COLLECTEVENT
                    push!(TE, xZ)
                    push!(YE, fZ)
                    push!(IE, j)

                    if isterminal
                        X[end] = xZ[1]
                        sol[end] = fZ[1]
                        return X, sol, TE, YE, IE
                    end
                end

            end
        end
    end
    # at the end, return what we have
    return X, sol, TE, YE, IE

end

function odelay(func, y0, xspan,
                 events, TOLERANCE = 1e-6)#, fsolve_args=[], args...)
    #if fsolve_args == None
    #    fsolve_args = []
    #end
    N = length(xspan)
    x0 = xspan[1] # initial point

    X = [x0]
    Nf = length(y0)
    sol = zeros(N, Nf)
    sol[1,:] = float64(y0)
    TE, YE, IE = [], [], [] # to store where events occur

    # initial value of events
    e = zeros(length(events), N)
    for (i, event) in enumerate(events)
        e[i, 1], isterminal, direction = event(y0, x0)
    end

    # now we step through the integration
    for (i, x1) in enumerate(xspan[1:end-1])
        x2 = xspan[i + 1]
        f1 = sol[i, :]

        f2, wz = rk4uniform(func, f1', [x1, x2], 1)#, args...)

        #X[i+1] = x2
        #println(X[i+1])
        X = push!(X, x2)
        sol[i+1,:] = f2[:, end]

        # check event functions. At each step we compute the event
        # functions, and check if they have changed sign since the
        # last step. If they changed sign, it implies a zero was
        # crossed.
        for (j, event) in enumerate(events)
            e[j, i + 1], isterminal, direction = event(sol[i + 1], X[i + 1])

            if ((e[j, i + 1] * e[j, i] < 0) || abs(e[j, i + 1]) < TOLERANCE || abs(e[j, i]) < TOLERANCE)
                xLt = X[end]       # Last point
                fLt = sol[i+1,:]
                eLt = e[j, i+1]

                function objective(x)
                    # evaluate ode from xLT to x
                    txspan = [xLt, x]
                    tempsol, wyz = rk4uniform(func, fLt', txspan, length(txspan))#, args...)
                    tsol = tempsol[:, end]
                    val, isterminal, direction = event(tsol, x)
                    return val[1]
                end
                xZ = fzero(objective, xLt)#, **fsolve_args)  # this should be the
                                              # value of x that makes
                                              # the event zero
                # now evaluate solution at this point, so we can
                # record the function values here.
                txspan = [xLt, xZ]
                tempsol, wyz = rk4uniform(func, fLt', txspan, length(txspan))#, args...)
                fZ = tempsol[:,end]

                vZ, isterminal, direction = event(fZ, xZ)

                COLLECTEVENT = false
                if direction == 0
                    COLLECTEVENT = true
                elseif (e[j, i + 1] > e[j, i] ) && direction == 1
                    COLLECTEVENT = true
                elseif (e[j, i + 1] < e[j, i] ) && direction == -1
                    COLLECTEVENT = true
                end

                if COLLECTEVENT
                    push!(TE, xZ)
                    push!(YE, fZ)
                    push!(IE, j)

                    if isterminal
                        X[end] = xZ[1]
                        sol[i+1,:] = fZ[1]
                        return X, sol[1:i+1,:], TE, YE, IE
                    end
                end

            end
        end
    end
    # at the end, return what we have
    return X, sol, TE, YE, IE

end

# ------------------------------
# Example usage : 1 dimension
# ------------------------------

# myode(f, x) = 3*x^2 + 12*x -4

# function event1(f, x)
#     # An event is when f = 0 and event is decreasing'
#     isterminal = true
#     direction = -1
#     return f, isterminal, direction
# end

# function event2(f, x)
#     # An event is when f = 0 and increasing'
#     isterminal = false
#     direction = 1
#     return f, isterminal, direction
# end

# f0 = -120

# xspan = linspace(-8, 4, 50)
# events=[event1, event2]
# func = myode; y0 = f0;
# X, F, TE, YE, IE = odelay(myode, f0, xspan, events)

# ------------------------------
# Example usage : 2 dimensions
# ------------------------------

# function myode(z, t)
#     # PROJ: ODE for projectile motion with linear air resistance
#     # z = [z0, z1, z2, z3]
#     g = 9.81 # Specify approximate graviatational constant
#     b = 0.28 # Representative value
#     return [z[3], z[4], -g-b*z[3], -b*z[4]]
# end

# function linevent(z, t)
#     # LINEVENT: Contains the event we are looking for
#     # In this event, z(1) = 0 (hitting the ground)
#     lookfor = z[1] # Sets this to 0
#     isterminal = true # Stop when event is located
#     direction = -1 # specify downward direction
#     return lookfor, isterminal, direction
# end

# f0 = [0.18, 0, 5, 10]
# xspan = linspace(0, 10, 50)
# events = [linevent]
# func = myode
# y0 = f0
# Xm, Fm, TEm, YEm, IEm = odelay(myode, f0, xspan, events)


# Test for infinite Bi-Lanczos

using NonlinearEigenproblemsTest
using NonlinearEigenproblems
using Test
using LinearAlgebra

@bench @testset "Infbilanczos σ=0" begin
    nep=nep_gallery("qdep0");
    nept=SPMF_NEP([copy(nep.A[1]'), copy(nep.A[2]'), copy(nep.A[3]')], nep.fi)
    n=size(nep,1);

    m=40;
    λ,V,T = infbilanczos(Float64,nep,nept,maxit=m,neigs=3,σ=0,logger=displaylevel,
                         v=ones(Float64,n),u=ones(Float64,n),check_error_every=3,
                         tol=1e-7, errmeasure=ResidualErrmeasure(nep));

    # Produced with a different implementation
    Tstar=[  -1.665117675679600   5.780562035399026                   0                   0
             5.780562035399026  11.562308485001218 -18.839546184493731                   0
             0  18.839546184493734 -15.213756300995186   9.788512505128466
             0                   0   9.788512505128464  -0.120825360586847                                ]
    n0=min(size(Tstar,1),size(λ,1));
    @test opnorm(Tstar[1:n0,1:n0]-T[1:n0,1:n0])<1e-10

    thiserr=ones(m)*NaN
    for i=1:length(λ)
        thiserr[i]=norm(compute_Mlincomb(nep,λ[i],V[:,i]));
    end
    @test length(findall(thiserr .< 1e-7)) == 3

    @testset "Compute as many eigenpairs as possible (neigs=Inf)" begin
        m=30;
        λ,V,T = infbilanczos(Float64,nep,nept,maxit=m,neigs=Inf,σ=0,logger=displaylevel,
                             v=ones(Float64,n),u=ones(Float64,n),check_error_every=3,
                             tol=1e-7, errmeasure=ResidualErrmeasure(nep));
        verify_lambdas(3, nep, λ, V, 1e-6)
    end

    @testset "Errors thrown" begin
        @test_throws NEPCore.NoConvergenceException λ,V,T = infbilanczos(Float64,nep,nept,maxit=9,neigs=8,σ=0,logger=displaylevel,
                             v=ones(Float64,n),u=ones(Float64,n),check_error_every=3,
                             tol=1e-7, errmeasure=ResidualErrmeasure(nep));
    end
end

# Disabled to improve unit test performance
#@testset "Infbilanczos σ=$x" for x in (1.0, 1.0+0.1im)
#    m=30;
#    λ,V,T = infbilanczos(nep,nept,maxit=m,neigs=2,σ=x,tol=1e-7,
#                         v=ones(n),u=ones(n));
#    thiserr=ones(m)*NaN
#    for i=1:length(λ)
#        thiserr[i]=norm(compute_Mlincomb(nep,λ[i],V[:,i]));
#    end
#    @test length(find(thiserr .< 1e-7)) >= 2
#end

##for i=1:m
## semilogy(1:m, err[1:m,i], color="red", linewidth=2.0, linestyle="--")
##end
##
#errormeasure=default_errmeasure(nep);
#for i=1:length(λ)
# @info "Eigenvalue = $(λ[i]), residual = $(errormeasure(λ[i],Q[:,i]))"
#end
#


# Translating
# https://proplot.readthedocs.io/en/latest/colorbars_legends.html#Axes-colorbars-and-legends
# to Julia

# Import proplot using PyCall
using PyCall
proplot = pyimport("proplot")

f, axs = proplot.subplots(nrows=2, share=0, axwidth="55mm", panelpad="1em")
[ax.format(suptitle="Stacked colorbars demo") for ax in axs]
N = 10
# Repeat for both axes
for (j, ax) in enumerate(axs)
    ax.format(
        xlabel="data", xlocator=range(0, 0.8, length=5),
        title="Subplot #$(j)"
    )
    for (i, (x0, y0, x1, y1, cmap, scale)) in enumerate((
         (0, 0.5, 1, 1, "grays", 0.5),
         (0, 0, 0.5, 0.5, "reds", 1),
         (0.5, 0, 1, 0.5, "blues", 2)
    ))
        j == 2 && i == 1 && continue
        data = rand(N, N) * scale
        x, y = range(x0, x1, length=N+1), range(y0, y1, length=N+1)
        m = ax.pcolormesh(
            x, y, data, cmap=cmap,
            levels=range(0, scale, length=11)
        )
        ax.colorbar(m, loc="l", label="dataset #$(i)")
    end
end

f.savefig("figs/Axes-colorbars-and-legends-2.svg", transparent=false)


# %% Saltzman iceage models
using DrWatson
@quickactivate "NonlinearDynamicsTextbook"
include(srcdir("style.jl"))
using DynamicalSystems, PyPlot

function sm91(state, param, t)
    u, ν, p, r, s, q = param
    x, y, z = state
    dx = -x -y -u*z # - ν*astronomical(t/10) # ensure time is in kyr for interpolation
    dy = -p*z +r*y -s*y^2 - y^3
    dz = -q*(x+z)
    return SVector(dx, dy, dz)
end

# Astronomical forcing:
using DelimitedFiles, Statistics
X = readdlm(projectdir("exercise_data", "16.csv"))
tsol = X[:, 1] # time in kyr
Isol = X[:, 5]
# Transform simulation to last 500 kyr and reverse time axis
# L = length(Isol)
# Isol = Isol[1:L÷2]
# tsol = tsol[1:L÷2]
Isol = standardize(Isol)
tsol .+= abs(tsol[1])
using Interpolations
astronomical = LinearInterpolation(tsol, Isol)
quasiperiodic(t) = cospi(2t/22.1) - cospi(2t/44.1)
# Default parameter values
p = 1.0
q = 2.5
r = 1.3
s = 0.6
u = 0.2
ν = 0.0
t₀ = 10.0 # timescale in kyr
params = [u, ν, p, r, s, q]
u₀ = [-0.2, 1.0, 0.0]

ds = ContinuousDynamicalSystem(sm91, u₀, params)

fig, axs = subplots(1,3)

# Unforced solution
T = 50.0
Δt = 0.01
tr = trajectory(ds, T; Δt)
t = (0:Δt:T) .* t₀
for i in 1:3;
    x = tr[:, i]
    axs[1].plot(t, x, ls = LINESTYLES[i], color = "C$(i-1)")
    imax = argmax(x)
    label = "\$"*("x", "y", "z")[i]*"\$"
    axs[1].text(t[imax], x[imax], label; color = "C$(i-1)")
end
axs[1].set_xlabel("\$t\$ (kyr, arbtr.)")
axs[1].set_ylim(-1.5, 1.5)
fig.tight_layout(pad=0.3)

# # Forced solution
# set_parameter!(ds, 2, 0.5)
# tr = trajectory(ds, 120; Δt)
# axs[3].plot(tr[:, 2])

# transform tsol into same time units as above
# axs[2].plot(tsol, Isol; c = "C3")
# axs[2].set_xlabel("\$t\$ (kyr)")
# axs[2].text(200, 410, "\$F(t)\$", color = "C3")

# Van der poll version
function vdp(u, p, t)
    x, y = u
    α, β, γ, τ, A = p
    dx = -y - β + γ*astronomical_forcing(A, t)
    # dx = -y - β + γ*sinpi(2t/41)
    dy = -α*(y^3/3 - y - x)
    return SVector(dx/τ, dy/τ)
end

include("12.7_astronomical_forcing.jl")

α = 11.11
β = 0.25
γ = 0.75
τ = 40.09
p = (α, β, γ, τ, A)

u₀ = [0.1, -0.2]
T = 600.0
Δt = 0.1
t0 = 500.0
t = (0:Δt:T) .- t0

F = astronomical_forcing.(Ref(A), t)
axs[2].plot(t, F; color = "C3")
axs[2].text(-400, -5, "\$F(t)\$", color = "C3")
# %%
axs[3].clear()

ds = ContinuousDynamicalSystem(vdp, u₀, p; t0 = -t0)
tr = trajectory(ds, T; Δt)
axs[3].plot(t, standardize(tr[:, 1]))

# Plot vostok
V = readdlm(projectdir("exercise_data", "17.csv"))
Vt = V[:, 1] ./ 1000 # in ky
VT = V[:, 2]
Vt = -reverse(Vt)
VT = reverse(VT)
# Interpolate Vostok data for better plot clarity
using Dierckx
# interpolation object:
spl = Spline1D(Vt, VT; k=3, bc="extrapolate")
Vti = Vt[1]:(10Δt):Vt[end]
VTi = spl(Vti)

axs[3].plot(Vti, standardize(VTi);
    color = "C2", zorder = 99, alpha = 0.8, ls = "-.", lw = 2,
    # marker = "o", mew = 1, mfc = "none", alpha = 0.2, ms = 5,
)

axs[3].text(38.6, 1.61, "\$x\$"; color = "C0")
axs[3].text(-120, 2.6, "Vostok"; color = "C2")

axs[2].set_xlabel("\$t\$ (kyr)")
axs[3].set_xlabel("\$t\$ (kyr)")

add_identifiers!(fig)
wsave(plotsdir("12", "iceages"), fig)