### A Pluto.jl notebook ###
# v0.11.14

using Markdown
using InteractiveUtils

# This Pluto notebook uses @bind for interactivity. When running this notebook outside of Pluto, the following 'mock version' of @bind gives bound variables a default value (instead of an error).
macro bind(def, element)
    quote
        local el = $(esc(element))
        global $(esc(def)) = Core.applicable(Base.get, el) ? Base.get(el) : missing
        el
    end
end

# ╔═╡ e96ee5ce-ef51-11ea-18f7-5fffcbe97c21
begin
	using Pkg
	Pkg.activate(".")
	Pkg.add.(["LaTeXStrings","Plots", "PlutoUI"])
	using LaTeXStrings
	using Plots
	using PlutoUI
end

# ╔═╡ b31a8560-f132-11ea-2567-ffd1936707d4
html"<button onclick=present()>Present</button>"

# ╔═╡ e53fab60-ee9e-11ea-0602-456fbb0afe8e
md"
## $3.4-4.$

Use the graphical method to solve the problem:

Minimize:
```math
Z = 3x_1 + 2x_2
```
subject to:
```math
\begin{aligned}
x_1 + 2x_2 &\leq 12 \\
2x_1 + 3x_2 &= 12 \\
2x_1 + x_2 &\geq 8 \\
x_1 \geq 0,\ x_2 &\geq 0 \\
\end{aligned}
```
"

# ╔═╡ 64223ee0-eea2-11ea-278b-ed5d31ebfe83
md"""
!!! hint "Solution 👀"
    ```math
    \begin{aligned}
    x_1 &= 3\\
    x_2 &= 2\\
    Z &= 13
    \end{aligned}
    ```
"""

# ╔═╡ 24606590-ef54-11ea-22ae-939492342184
md"""
### Graphical solution
---

``Z=3x_1+2x_2``: $(@bind obj CheckBox())

``x_1 + 2x_2 \leq 12``: $(@bind c1 CheckBox())

``2x_1 + 3x_2 = 12``: $(@bind c2 CheckBox())

``2x_1 + x_2 \geq 8``: $(@bind c3 CheckBox())

``Z`` = $(@bind Z Slider(0:18, default=0,show_value=true))
"""

# ╔═╡ 1be9eb68-f905-11ea-0097-372fc9887503
begin
	lims = [(0, 12), (0, 10), (0, 13)]
	xlims, ylims, zlims = lims[1], lims[2], lims[3]

	plot(widen=false)

	if c1
		plot!([0, 12], [6, 0], fillrange=0, fillalpha=0.1,label=L"$x_1 + 2x_2 \leq 12$")
	end
	if c2
		plot!([0, 6],[4, 0], label=L"$2x_1 + 3x_2 = 12$", linewidth=2)
	end
	if c3
		plot!([0, 12],[8, -16], fillrange=10, fillalpha=0.1, label=L"$2x_1 + x_2 \geq 8$")
	end
	if obj
		plot!([0,Z/3],[Z/2,0], label=L"$Z=3x_1+2x_2$", color=:red, linestyle=:dash, linewidth=1)
	end

	if obj && Z == 13
		scatter!([3], [2], color="red", label="Optimal solution")
		annotate!((3.3, 2.5, text("Optimal solution", :left)))
	end

	xlims!(xlims)
	ylims!(ylims)
	xlabel!(L"$x_1$")
	ylabel!(L"$x_2$")
end

# ╔═╡ b2dd82b0-f46f-11ea-079b-376595f80407
md"
_Note that the feasible regions for_ $x_1 \geq 0$ _and_ $x_2 \geq 0$ _are not shown/colored._
"

# ╔═╡ f06c1500-ee9e-11ea-3485-5d8ebf946aae
md"
## Exercise 1

Consider the economic dispatch problem for a very small power system with only two units, Unit 1 and Unit 2.

**a)** Solve the economic dispatch problem for one time period. Unit 1 is a coal-fired steam unit with minimum and maximum generation levels of 150 MW and 350 MW and production costs of 600 Euro/MW. Unit 2 is a oil-fired steam unit with minimum and maximum generation levels of 50 MW and 500 MW and production costs of 900 Euro/MW. Demand is 600 MW.

**b)** Is the problem feasible for any data set? Is it bounded for any data set? (Hint: Look at minimum and maximum generation levels).

**c)** Extend the economic dispatch problem to include two time periods. Demand in the first and second time periods is 600 MW and 400 MW, respectively. Solve this problem.

**d)** Adapt the model such that the production cost includes a variable cost of operation and maintenance ``a_i^{O\&M}`` (Euro/MWh) of Unit ``i``, a fuel cost ``a_i^{fuel}`` (Euro/MWh) of Unit ``i`` and an emission costs ``a_i^{emis}`` (Euro/T) for Unit ``i`` for ``i`` = 1; 2. Denote the amount of emissions from burning fuel by ``p^{emis}`` (T/MWh). Extend this formulation to include a fuel efficiency ``\eta_i^{fuel}`` (note that the fuel efficiency has an impact on both fuel and emission costs).

**e)** Introduce a constraint that forces the generation level of Unit 1 to
be constant, e.g., equal to the parameter ``p_1^{const}`` or 300 MW.
"

# ╔═╡ 68ad3570-ef77-11ea-1ae9-cb0c85fffac0
md"""
!!! hint "Solution 👀"
    **a)**
    ```math
    \begin{align}
    \text{ Min } Z = 600p_1 + 900p_2\\
    \text{ s.t. } \quad 150 \leq p_1 \leq 350\\
    50 \leq p_2 \leq 500\\
    p_1 + p_2 = 600 \\
    p_1 \geq 0,\ p_2 \geq 0 \\
    \end{align}
    ```
    And the solution is:
    ```math
    \begin{aligned}
    p_1^* &= 350\ \text{[MWh]}\\
    p_2^* &= 600 - 350 = 250\ \text{[MWh]}\\
    z^* &= 600\cdot350 + 900\cdot250 = 435,000\ \text{[Euro]}
    \end{aligned}
    ```

    **b)** The problem is feasible for:
    ```math
    \begin{aligned}
    50+150 \leq d \leq 500 + 350 \quad \Leftrightarrow \quad 200 \leq d \leq 850
    \end{aligned}
    ```
    Since ``p_1`` and ``p_2`` are bounded, the problem will always be bounded.

    **c)** The problem will not be time dependent, thus, you can solve the problem one period at the time. Solution to the for time period is found in **a)**, the solution to the second time period is:
    ```math
    \begin{aligned}
    p_1^* &= 350\ \text{[MWh]}\\
    p_2^* &= 50\ \text{[MWh]}\\
    z^* &= 255,000\ \text{[Euro]}
    \end{aligned}
    ```
    Hence, the overall solution can be written as:

    $$p_1^* = (350, 350),\ p_2^* = (250, 50),\ z^* = 690,000$$

    **d)** Production costs:

    $$a_i = a_i^{O\&M}+\frac{a_i^{fuel}+a_i^{emis}\cdot p_i^{emis}}{\eta_i^{fuel}},\ 0 \leq \eta_i^{fuel} \leq 1$$

    **e)**

    $$p_1^{const} = 300\ \text{MW};\ p_1^{min} = 300 \leq p_1 \leq 300 = p_1^{max}$$
"""

# ╔═╡ bb61b24a-f911-11ea-27b7-997c31ecf3d1
md"""
### Graphical solution
---

``p_1^{min}``: $(@bind p1_min TextField(default="150"))

``p_1^{max}``: $(@bind p1_max TextField(default="350"))

``p_2^{min}``: $(@bind p2_min TextField(default="50"))

``p_2^{max}``: $(@bind p2_max TextField(default="500"))

``d`` = $(@bind d Slider(0:50:850, default=600,show_value=true))
"""

# ╔═╡ ccd2b7ba-f912-11ea-0b59-15c824b2afdc
begin
	# Assess that we can parse the minimum and maximum
	for option in [p1_min, p1_max, p2_min, p2_max]
		try
			parse(Int, option)
		catch
			throw("You should have entered a numeric value!")
		end
	end

	p1min = parse(Int, p1_min)
	p1max = parse(Int, p1_max)
	p2min = parse(Int, p2_min)
	p2max = parse(Int, p2_max)

	lims2 = [(0, 1000), (0, 1000)]
	xlims2, ylims2 = lims2[1], lims2[2]

	plot(widen=false, legend=:topleft)

	plot!([0,p1min+p2min], [0,0], label=L"p_1^{min}+p_2^{min}")

	plot!([p1min+p2min, p1max+p2min], [0, 600], linetype=:steppre, label=L"p_1^{max}-p_1^{min}")

	plot!([p1max+p2min, p2max+p1max], [600, 900], linetype=:steppre, label=L"p_2^{max}-p_2^{min}")

	plot!([d,d], [0,ylims2[2]], label="Demand", color="red")

	xlims!(xlims2)
	ylims!(ylims2)
	xlabel!("Production [MWh]")
	ylabel!("Cost [Euro]")

end

# ╔═╡ fe2a6cc0-eea1-11ea-0642-8735d6ad8a43
md"
## Exercise 2

In contrast to taking a systems perspective, consider the economic dispatch problem from the perspective of a producer. When selling production in an electricity market, most producers rely on profit maximization rather than cost minimization and are not obliged to meet demand. Reformulate the economic dispatch problem as a profit maximization problem by introducing market prices and ignoring the demand constraints. What does this mean for the structure of the problem in terms of solving it? (Hint: Smaller problems are usually easier to solve).
"

# ╔═╡ 6bea5f10-ef77-11ea-1181-1356c9b3b4e7
md"""
!!! hint "Solution 👀"
    Producer's economic dispatch:

    ```math
    \begin{aligned}
    \text{max}\ \sum_{i=1}^I \sum_{t=1}^T \left( p_i-a_i\right)p_{it}&\qquad\\
    \text{s.t.}\ p_i^{min} \leq p_{it} \leq p_i^{max}&\qquad \forall i \in I,\ \forall t \in T\\
    p_{it} \geq 0&\qquad \forall i \in I,\ \forall t \in T\\
    \end{aligned}
    ```
    where ``p_i`` is the revenue.

    No demand satisfaction ``\rightarrow`` no unit and time coupling constraints ``\Rightarrow`` the problem can now be solved for each unit (one unit at-a-time) and each time period individually.
"""

# ╔═╡ 2b4bcf90-eea3-11ea-0a5b-c5507c28ed4e
md"
## Exercise 5

Consider the economic dispatch model for electricity system operation. Assume that all units are dispatched in two markets; a dayahead market and an intra-day market. You can think of the day-ahead and intra-day markets as two different markets and ignore the time aspect indicated by their names (the physical dispatch of production takes place within the same time horizon for the two markets). At the day-ahead market, **`forecasted`** demand ``d_t^{forecast}`` must be satisfied by production in every time period of the scheduling horizon. However, since the demand forecast is not entirely correct, at the intra-day market, **`realized`** demand ``d_t^{realized}`` must be satisfied by adjusting production in every time period. Production can be adjusted both upwards and downwards in the intraday market. Reformulate the economic dispatch model to include the two markets (Hint: You will need two demand constraints and two sets of variables that takes care of production and adjustments to production, respectively).
"

# ╔═╡ 6ef52e10-ef77-11ea-0e6c-356bdd085208
md"""
!!! hint "Solution 👀"
    ```math
    \begin{aligned}
    \min & \sum_{i=1}^{I} \sum_{t=1}^{T} a_{i}\left(p_{i t}+p_{i t}^{+}-p_{i t}^{-}\right) &\qquad\\
    \text {st } & p_{i}^{\min } \leq p_{i t}+p_{i t}^{+}-p_{i t}^{-} \leq p_{i}^{\max } &\qquad \forall i \in I,\ \forall t \in T\\
    & \sum_{i=1}^{I} p_{i t}=d_{t}^{f o r e c a s t} &\qquad \forall t \in T\\
    & \sum_{i=1}^{I}\left(p_{i t}+p_{i t}^{+}-p_{i t}^{-}\right)=d_{t}^{r e a l i z e d} &\qquad \forall t \in T\\
    & p_{i t}, p_{i t}^{+}, p_{i t}^{-} \geq 0&\qquad \forall i \in I,\ \forall t \in T
    \end{aligned}
    ```

    where ``p_{it}^+`` is up-regulation and ``p_{it}^-`` is down-regulation.
"""

# ╔═╡ f7931ed0-eea5-11ea-038b-75467446c7a9
md"
## Exercise 6

Formulate the economic dispatch problem when demand is flexible, i.e., can be adjusted by the system. For example, the system operator may contact the industry to enforce a decrease of production. Include a benefit (such a benefit is usually referred to the `utility` of consumers) of being able to serve demand, assuming that this benefit is proportional to the amount of demand served. Is demand a parameter or a variable? Is this problem feasible for any data set?
"

# ╔═╡ 727b6e9e-ef77-11ea-2b63-21b8b91abd31
md"""
!!! hint "Solution 👀"
    Social costs minimization:

    ```math
    \begin{aligned}
    \min & \sum_{i=1}^{I} \sum_{t=1}^{T} a_{i}p_{i t} - \sum_{t=1}^T bd_t &\\
    \text {s.t. } & p_{i}^{\min } \leq p_{i t} \leq p_{i}^{\max } &\qquad \forall i \in I,\ \forall t \in T\\
    & \sum_{i=1}^{I} p_{i t} = d_{t}&\qquad \forall t \in T\\
    & p_{i t}, d_t \geq 0&\qquad \forall i \in I,\ \forall t \in T
    \end{aligned}
    ```

    Or equivalently social welfare maximization:
    ```math
    \begin{aligned}
    \max & \sum_{i=1}^{I} \sum_{t=1}^{T} \left(p_t - a_{i}\right)p_{i t} + \sum_{t=1}^T \left(b - p\right)d_t &\\
    \text {s.t. } & p_{i}^{\min } \leq p_{i t} \leq p_{i}^{\max } &\qquad \forall i \in I,\ \forall t \in T\\
    & \sum_{i=1}^{I} p_{i t} = d_{t}&\qquad \forall t \in T\\
    & p_{i t}, d_t \geq 0&\qquad \forall i \in I,\ \forall t \in T
    \end{aligned}
    ```
"""

# ╔═╡ 12145486-f923-11ea-2f8e-036ec37a9f87
md"
## Exercise 12

Reformulate the demand constraint of the economic dispatch problem to account for the integration of run-of river hydro-power $p_t^{hydro}$ , solar $p_t^{solar}$, tidal $p_t^{tidal}$ and wave power $p_t^{wave}$ . Is run-of river hydro-power, solar, tidal and wave power production paramters or decision variables in the problem? Why?
"

# ╔═╡ 64cd5132-f923-11ea-2818-d355cf1c13ab
md"""
!!! hint "Solution 👀"
    $$\sum_{i=1}^I p_{it} = d_t - p_t^{wind} - p_t^{hydro} - p_t^{solar} - p_t^{tidal} - p_t^{wave}$$
    All of them are parameters since production is non-controllable (there are no decisions to be made).
"""

# ╔═╡ d586c278-f923-11ea-33af-1d7dbbd9ae2f
md"
## Exercise 13

Assume that wind power curtailment, i.e., a reduction of the wind power generation, is possible at a cost of, say, $a^{wind}$. Adapt the economic dispatch problem to take this into account.
"

# ╔═╡ f0c92578-f923-11ea-1c50-537ffdf4476e
md"""
!!! hint "Solution 👀"
    ```math
    \begin{aligned}
    \min \sum_{i=1}^{I} &\sum_{t=1}^{T} a_{i} p_{i t}+\sum_{t=1}^{T} a^{\text {wind}} p_{t}^{\text {wind},-} &\qquad\\
    \text {st } p_{i}^{\min } &\leq p_{i t} \leq p_{i}^{\max } &\qquad \forall i \in I,\ \forall t \in T\\
    \sum_{i=1}^{I} p_{i t} &= d_{t}-\left(p_{t}^{w i n d}-p_{t}^{w i n d,-}\right) &\qquad \forall t \in T\\
    p_{t}^{\text {wind},-} &\leq p_{t}^{\text {wind}} &\qquad \forall t \in T\\
    p_{i t}, p_{t}^{w i n d,-} &\geq 0 &\qquad \forall i \in I,\ \forall t \in T
    \end{aligned}
    ```
    where $p_t^{wind, -}$ is the reduction or curtailment of wind power in period t, and $a^{wind}$ is the cost of curtailment.
"""

# ╔═╡ b63864f0-ef76-11ea-0ba7-f331f3f5d767
md"
## Notes:
"

# ╔═╡ a360fdf0-edce-11ea-2f42-07bd9073048a
author = (name = "Jorge Montalvo Arvizu", id = "s192184")

# ╔═╡ 7698a280-edcb-11ea-3048-7f291d85476f
md"""
# Module 3 - The economic dispatch problem ⚡📈💰

_42002 - Modelling and analysis of Sustainable Energy Systems using Operations Research_

Prepared by: **_$(author.name)_** ($(author.id)@student.dtu.dk)

### Exercises 📚
---

Solve the following exercises from 	`Hillier, Lieberman. Introduction to Operations Research, Eight Edition. pp. 91-95`, and from the course notes `Mathematical programming models for energy system analysis: An introduction. pp. 26-30`.

- HL:

  - 3.4 - 4

- Course notes:

  - 1 (excluding f)
  - 2
  - 5
  - 6
  - 12
  - 13
"""

# ╔═╡ Cell order:
# ╟─b31a8560-f132-11ea-2567-ffd1936707d4
# ╟─7698a280-edcb-11ea-3048-7f291d85476f
# ╟─e53fab60-ee9e-11ea-0602-456fbb0afe8e
# ╟─64223ee0-eea2-11ea-278b-ed5d31ebfe83
# ╟─24606590-ef54-11ea-22ae-939492342184
# ╟─1be9eb68-f905-11ea-0097-372fc9887503
# ╟─b2dd82b0-f46f-11ea-079b-376595f80407
# ╟─f06c1500-ee9e-11ea-3485-5d8ebf946aae
# ╟─68ad3570-ef77-11ea-1ae9-cb0c85fffac0
# ╟─bb61b24a-f911-11ea-27b7-997c31ecf3d1
# ╟─ccd2b7ba-f912-11ea-0b59-15c824b2afdc
# ╟─fe2a6cc0-eea1-11ea-0642-8735d6ad8a43
# ╟─6bea5f10-ef77-11ea-1181-1356c9b3b4e7
# ╟─2b4bcf90-eea3-11ea-0a5b-c5507c28ed4e
# ╟─6ef52e10-ef77-11ea-0e6c-356bdd085208
# ╟─f7931ed0-eea5-11ea-038b-75467446c7a9
# ╟─727b6e9e-ef77-11ea-2b63-21b8b91abd31
# ╟─12145486-f923-11ea-2f8e-036ec37a9f87
# ╟─64cd5132-f923-11ea-2818-d355cf1c13ab
# ╟─d586c278-f923-11ea-33af-1d7dbbd9ae2f
# ╟─f0c92578-f923-11ea-1c50-537ffdf4476e
# ╟─b63864f0-ef76-11ea-0ba7-f331f3f5d767
# ╟─a360fdf0-edce-11ea-2f42-07bd9073048a
# ╟─e96ee5ce-ef51-11ea-18f7-5fffcbe97c21


#! /usr/bin/env julia

# Calculate the n^th Fibonacci number.
function fib(n)
  if n < 2
    return n
  else
    return fib(n - 1) + fib(n - 2);
  end
end

res = fib(20);
println(res);


### A Pluto.jl notebook ###
# v0.12.7

using Markdown
using InteractiveUtils

# ╔═╡ 6fd616f6-1ea1-11eb-3814-8bfb4a096c49
using Pkg, DrWatson

# ╔═╡ 6ffe2628-1ea1-11eb-24ea-57f985146a72
begin
	@quickactivate "StatisticsWithJuliaPlutoNotebooks"
	using Distributions, Random, StatsBase, DataFrames, Plots
	Random.seed!(1)
end;

# ╔═╡ ed174bc4-1ea0-11eb-1e2f-a32874cec549
md"## Listing 3.30"

# ╔═╡ 70179b62-1ea1-11eb-13ce-7fb4d54bad4b
begin
	n = 10^6
	data = tan.(rand(n)*pi .- pi/2)
	averages = accumulate(+,data)./collect(1:n)
end

# ╔═╡ 588624fe-21c2-11eb-3d7b-f37b1c1f44ca
plot( 1:n, averages,
	c=:blue, legend=:none,
	xscale=:log10, xlims=(1,n), xlabel="n", ylabel="Running average")

# ╔═╡ 70182cd8-1ea1-11eb-094d-8d0d49cf15f3
md"## End of listing 3.30"

# ╔═╡ Cell order:
# ╟─ed174bc4-1ea0-11eb-1e2f-a32874cec549
# ╠═6fd616f6-1ea1-11eb-3814-8bfb4a096c49
# ╠═6ffe2628-1ea1-11eb-24ea-57f985146a72
# ╠═70179b62-1ea1-11eb-13ce-7fb4d54bad4b
# ╠═588624fe-21c2-11eb-3d7b-f37b1c1f44ca
# ╟─70182cd8-1ea1-11eb-094d-8d0d49cf15f3


module IterativeLinearSolvers

# Included from https://github.com/andreasnoack/IterativeLinearSolvers.jl
# Eventually, use IterativeSolvers.jl

typealias Preconditioner{T} Union{AbstractMatrix{T}, Factorization{T}}

function cg{T<:LinAlg.BlasReal}(A::AbstractMatrix{T},
    x::AbstractVector{T},
    b::AbstractVector{T},
    M::Preconditioner{T},
    max_it::Integer,
    tol::Real)
#  -- Iterative template routine --
#     Univ. of Tennessee and Oak Ridge National Laboratory
#     October 1, 1993
#     Details of this algorithm are described in "Templates for the
#     Solution of Linear Systems: Building Blocks for Iterative
#     Methods", Barrett, Berry, Chan, Demmel, Donato, Dongarra,
#     Eijkhout, Pozo, Romine, and van der Vorst, SIAM Publications,
#     1993. (ftp netlib2.cs.utk.edu; cd linalg; get templates.ps).
#
#  [x, error, iter, flag] = cg(A, x, b, M, max_it, tol)
#
# cg.m solves the symmetric positive definite linear system Ax=b
# using the Conjugate Gradient method with preconditioning.
#
# input   A        REAL symmetric positive definite matrix
#         x        REAL initial guess vector
#         b        REAL right hand side vector
#         M        REAL preconditioner matrix
#         max_it   INTEGER maximum number of iterations
#         tol      REAL error tolerance
#
# output  x        REAL solution vector
#         error    REAL error norm
#         iter     INTEGER number of iterations performed
#         flag     INTEGER: 0 = solution found to tolerance
#                           1 = no convergence given max_it

    n = length(b)
    flag = 0                                 # initialization
    iter = 0

    bnrm2 = norm(b)
    if bnrm2 == 0.0 bnrm2 = one(T) end

    local ρ₁
    z = zeros(T, n)
    q = zeros(T, n)
    p = zeros(T, n)
    # r = copy(b)
    # A_mul_B!(-one(T),A,x,one(T),r)
    r = b - A*x
    error = norm(r)/bnrm2
    if error < tol
        return
    end

    for iter = 1:max_it                       # begin iteration

        z[:] = r
        A_ldiv_B!(M, z)
        # z[:] = M\r
        ρ = dot(r,z)

        if iter > 1                       # direction vector
            β = ρ/ρ₁
            for l = 1:n
                p[l] = z[l] + β*p[l]
            end
        else
            p[:] = z
        end

        # A_mul_B!(one(T),A,p,zero(T),q)
        q[:] = A*p
        α = ρ / dot(p,q)
        for l = 1:n
            x[l] += α*p[l]                    # update approximation vector
            r[l] -= α*q[l]                    # compute residual
        end

        error = norm(r)/bnrm2                     # check convergence
        if error <= tol break end

        ρ₁ = ρ

    end

    if error > tol flag = 1 end                 # no convergence
    return x, error, iter, flag
end

function cgs{T<:Number}(A::AbstractMatrix{T},
    x::AbstractVector{T},
    b::AbstractVector{T},
    M::Preconditioner{T},
    max_it::Integer,
    tol::Real)
#  -- Iterative template routine --
#     Univ. of Tennessee and Oak Ridge National Laboratory
#     October 1, 1993
#     Details of this algorithm are described in "Templates for the
#     Solution of Linear Systems: Building Blocks for Iterative
#     Methods", Barrett, Berry, Chan, Demmel, Donato, Dongarra,
#     Eijkhout, Pozo, Romine, and van der Vorst, SIAM Publications,
#     1993. (ftp netlib2.cs.utk.edu; cd linalg; get templates.ps).
#
#  [x, error, iter, flag] = cgs(A, x, b, M, max_it, tol)
#
# cgs.m solves the linear system Ax=b using the
# Conjugate Gradient Squared Method with preconditioning.
#
# input   A        REAL matrix
#         x        REAL initial guess vector
#         b        REAL right hand side vector
#         M        REAL preconditioner
#         max_it   INTEGER maximum number of iterations
#         tol      REAL error tolerance
#
# output  x        REAL solution vector
#         error    REAL error norm
#         iter     INTEGER number of iterations performed
#         flag     INTEGER: 0 = solution found to tolerance
#                           1 = no convergence given max_it

    iter = 0                               # initialization
    flag = 0

    n = length(b)
    bnrm2 = norm(b)
    if bnrm2 == 0
        bnrm2 = one(bnrm2)
    end

    u = zeros(T, n)
    p = zeros(T, n)
    p̂ = zeros(T, n)
    q = zeros(T, n)
    û = zeros(T, n)
    v̂ = zeros(T, n)
    ρ = zero(T)
    ρ₁ = ρ
    r = copy(b)
    A_mul_B!(-one(T), A, x, one(T), r)
    # r = b - A*x
    error = norm(r)/bnrm2

    if error < tol
        return x, error, iter, flag
    end

    r_tld = copy(r)

    for iter = 1:max_it                      # begin iteration

        ρ = dot(r_tld, r)
        if ρ == 0
            break
        end

        if iter > 1                          # direction vectors
            β = ρ/ρ₁
            for l = 1:n
                u[l] = r[l] + β*q[l]
                p[l] = u[l] + β*(q[l] + β*p[l])
            end
        else
            u[:] = r
            p[:] = u
        end

        p̂[:] = p
        A_ldiv_B!(M, p̂)
        # p̂[:] = M\p
        A_mul_B!(one(T), A, p̂, zero(T), v̂)  # adjusting scalars
        # v̂[:] = A*p̂
        α = ρ/dot(r_tld, v̂)
        for l = 1:n
            q[l] = u[l] - α*v̂[l]
            û[l] = u[l] + q[l]
        end
        A_ldiv_B!(M, û)
        # û[:] = M\û

        for l = 1:n
            x[l] += α*û[l]                  # update approximation
        end

        A_mul_B!(-α, A, û, one(T), r)
        # r[:] -= α*(A*û)
        error = norm(r)/bnrm2               # check convergence
        if error <= tol
            break
        end

        ρ₁ = ρ

    end

    if error <= tol                         # converged
        flag = 0
    elseif ρ == 0                           # breakdown
        flag = -1
    else                                    # no convergence
        flag = 1
    end
    return x, error, iter, flag
end

end


module Varpro
using LinearAlgebra
using SparseArrays

include("VarproTypes.jl")

using NL2sol

export varpro, FitContext, NL2SOL, NL2SNO

"""
  # Description

    varpro solves a separable nonlinear least squares problem.

        varpro(ctx::FitContext)
        Returns: [alpha, c, wresid, wresid_norm, y_est, Regression]

        ctx is constructed by the FitContext() constructor.  See types.jl
        for more info

    Given a set of m observations y[1],...,y[m]
    this program computes a weighted least squares fit using the model

        eta(alpha,c,t) =
            c_1 * phi_1 (alpha,t) + ...  + c_n * phi_n (alpha,t)
        (possibly with an extra term  + phi_{n+1} (alpha,t) ).

    This program determines optimal values of the q nonlinear parameters
    alpha and the n linear parameters c, given observations y at m
    different values of the "time" t and given evaluation of phi and
    (optionally) derivatives of phi.

    Varpro calls either NL2sol, NL2sno (fd Jacobian) which solves a
    non-linear least squares problem.

    What distinguishes varpro from levenberg_marquardt is that, for efficiency and
    reliability, varpro causes the nonlinear solver NL2sol to only iterate on the
    nonlinear parameters.  Given the information in phi and dphi, this
    requires an intricate but inexpensive computation of partial
    derivatives, and this is handled by the varpro function formJacobian.

    nl2sol is in the NL2sol.jl package.

    The original Fortran implementation of the variable projection
    algorithm (ref. 2) was modified in 1977 by John Bolstad
    Computer Science Department, Serra House, Stanford University,
    using ideas of Linda Kaufman (ref. 5) to speed up the
    computation of derivatives.  He also allowed weights on
    the observations, and computed the covariance matrix.

    The orginal MATLAB version of the Varpro program is documented in
    Dianne P. O'Leary and Bert W. Rust,
    "Variable Projection for Nonlinear Least Squares Problems",
    Computational Optimization and Applications (2012)
    doi 10.1007/s10589-012-9492-9.

    US National Inst. of Standards and Technology, 2010.

    Main reference:

        0.  Gene H. Golub and V. Pereyra, 'Separable nonlinear least
            squares: the variable projection method and its applications,'
            Inverse Problems 19, R1-R26 (2003).

    See also these papers, cited in John Bolstad's Fortran code:

    1.  Gene H. Golub and V. Pereyra, 'The differentiation of
        pseudo-inverses and nonlinear least squares problems whose
        variables separate,' SIAM J. Numer. Anal. 10, 413-432
        (1973).
    2.  ------, same title, Stanford C.S. Report 72-261, Feb. 1972.
    3.  Michael R. Osborne, 'Some aspects of non-linear least
        squares calculations,' in Lootsma, Ed., 'Numerical Methods
        for Non-Linear Optimization,' Academic Press, London, 1972.
    4.  Fred Krogh, 'Efficient implementation of a variable projection
        algorithm for nonlinear least squares problems,'
        Comm. ACM 17:3, 167-169 (March, 1974).
    5.  Linda Kaufman, 'A variable projection method for solving
        separable nonlinear least squares problems', B.I.T. 15,
        49-57 (1975).
    6.  C. Lawson and R. Hanson, Solving Least Squares Problems,
        Prentice-Hall, Englewood Cliffs, N. J., 1974.

    These books discuss the statistical background:

    7.  David A. Belsley, Edwin Kuh, and Roy E. Welsch, Regression
        Diagnostics, John Wiley & Sons, New York, 1980, Chap. 2.
    8.  G.A.F. Seber and C.J. Wild, Nonlinear Regression,
        John Wiley & Sons, New York, 1989, Sec. 2.1, 5.1, and 5.2.

    Dianne P. O'Leary, NIST and University of Maryland, February 2011.
    Bert W. Rust,      NIST                             February 2011.
    Comments updated 07-2012.

    MODIFIED: May 2016
    Ported to Julia, extended to handle complex residuals of complex
    parameters (both linear and non-linear), restructured to use context
    struct, style changes and added ability to call nl2sol as the nonlinear
    solver

    Don MacMillen

# Input

    On Input and stored in the FitContext struct ctx are:

    y    m x 1   vector containing the m observations (may be complex)
    w    m x 1   vector of weights used in the least squares
        fit.  We minimize the norm of the weighted residual
        vector r, where, for i=1:m,

        r[i] = w[i] * (y[i] - eta(alpha, c, t[i,:])).

        Therefore, w[i] should be set to 1 divided by
        the standard deviation in the measurement y[i].
        If this number is unknown, set w[i] = 1.
        Only real weights are supported

    alpha q x 1  initial estimates of the parameters alpha.
        If alpha = [], Varpro assumes that the problem
        is linear and returns estimates of the c parameters.
        May be complex
    n            number of linear parameters c
    ada          a function handle, described below.
    gada         a function handle, described below.
    lb    q x 1  lower bounds on the parameters alpha.
        (Optional)   (Omit this argument or use [] if there are
        no lower bounds.)  May be complex but currently unimplmented
    ub    q x 1  upper bounds on the parameters alpha.
        (Optional)   (Omit this argument or use [] if there are
        no upper bounds.)  May be complex but currently unimplemented

# Output

    alpha  q      estimates of the nonlinear parameters.
    c      n      estimates of the linear parameters.
    wresid m      weighted residual vector, with i-th component
        w(i) * (y(i) - eta(alpha, c, t(i,:))).
    wresid_norm   norm of wresid.
    y_est  m      the model estimates = eta(alpha, c, t(i,:)))
    regression    a structure containing diagnostics about the model

                **************************************************
                *                C a u t i o n:                  *
                *   The theory that makes statistical            *
                *   diagnostics useful is derived for            *
                *   linear regression, with no upper- or         *
                *   lower-bounds on variables.                   *
                *   The relevance of these quantities to our     *
                *   nonlinear model is determined by how well    *
                *   the linearized model (Taylor series model)   *
                *         eta(alpha_true, c_true)                *
                *            +  phi * (c  - c_true)              *
                *            + dphi * (alpha - alpha_true)       *
                *   fits the data in the neighborhood of the     *
                *   true values for alpha and c, where phi       *
                *   and dphi contain the partial derivatives     *
                *   of the model with respect to the c and       *
                *   alpha parameters, respectively, and are      *
                *   defined in ada.                              *
                **************************************************

    regression.results:
        This structure includes information on the solution
        process, including the number of iterations,
        termination criterion, and exitflag from lsqnonlin.

    regression.rank is the computed rank of the
        matrix for the linear subproblem.  If this equals
        n, then the linear coefficients are well-determined.
        If it is less than n, then although the model might
        fit the data well, other linear coefficients might
        give just as good a fit.

    regression.sigma:
        The estimate of the standard deviation is the
        weighted residual norm divided by the square root
        of the number of degrees of freedom.
        This is also called the "regression standard error"
        or the square-root of the weighted SSR (sum squared
        residual) divided by the square root of the
        number of degrees of freedom.

    regression.rms:
       The "residual mean square" is equal to sigma^2:
       RMS = wresid_norm^2 / (m-n+q)

    regression.coef_determ:
        The "coefficient of determination" for the fit,
        also called the square of the multiple correlation
        coefficient, is sometimes called R^2.
        It is computed as 1 - wresid_norm^2/CTSS,
        where the "corrected total sum of squares"
        CTSS is the norm-squared of W*(y-y_bar),
        and the entries of y_bar are all equal to
        (the sum of W_i^2 y_i) divided by (the sum of W_i^2).
        A value of .95, for example, indicates that 95 per
        cent of the CTTS is accounted for by the fit.

    regression.covmx: (n+q) x (n+q)
        This is the estimated variance/covariance matrix for
        the parameters.  The linear parameters c are ordered
        first, followed by the nonlinear parameters alpha.
        This is empty if dphi is not computed by ada.

    regression.cormx: (n+q) x (n+q)
        This is the estimated correlation matrix for the
        parameters.  The linear parameters c are ordered
        first, followed by the nonlinear parameters alpha.
        This is empty if dphi is not computed by ada.

    regression.std_param: (n+q) x 1
        This vector contains the estimate of the standard
        deviation for each parameter.
        The k-th element is the square root of the k-th main
        diagonal element of Regression.CovMx
        This is empty if dphi is not computed by ada.

    regression.t_ratio:   (n+q)
        The t-ratio for each parameter is equal to the
        parameter estimate divided by its standard deviation.
        (linear parameters c first, followed by alpha)
        This is empty if dphi is not computed by ada.

    regression.standardized_wresid:
        The k-th component of the "standardized weighted
        residual" is the k-th component of the weighted
        residual divided by its standard deviation.
        This is empty if dphi is not computed by ada.

    Specification of the function ada, which computes information
    related to phi:

    function  phi, dphi, ind = ada(alpha)

    This function computes phi and returns it.  Additionally, it
    computes more information that is needed in formJacobian and
    stores it in the context

    On Input:

        alpha q     contains the current value of the alpha parameters.

        Note:  If more input arguments are needed, use the standard
               Matlab syntax to accomplish this.  For example, if
               the input arguments to ada are t, z, and alpha, then
               before calling varpro, initialize t and z, and in calling
               varpro, replace "@ada" by "@(alpha)ada(t,z,alpha)".

     On Output:

        phi   m x n1   where phi(i,j) = phi_j(alpha,t_i).
                       (n1 = n if there is no extra term;
                        n1 = n+1 if an extra term is used)
        dphi  m x p    where the columns contain partial derivative
                       information for phi and p is the number of
                       columns in Ind
                       (or dphi = [] if derivatives are not available).
        Ind   2 x p    Column k of dphi contains the partial
                       derivative of phi_j with respect to alpha_i,
                       evaluated at the current value of alpha,
                       where j = Ind(1,k) and i = Ind(2,k).
                       Columns of dphi that are always zero, independent
                       of alpha, need not be stored.
        Example:  if  phi_1 is a function of alpha_2 and alpha_3,
                  and phi_2 is a function of alpha_1 and alpha_2, then
                  we can set
                          Ind = [ 1 1 2 2
                                  2 3 1 2 ]
                  In this case, the p=4 columns of dphi contain
                          d phi_1 / d alpha_2,
                          d phi_1 / d alpha_3,
                          d phi_2 / d alpha_1,
                          d phi_2 / d alpha_2,
                  evaluated at each t_i.
                  There are no restrictions on how the columns of
                  dphi are ordered, as long as Ind correctly specifies
                  the ordering.

        If derivatives dphi are not available, then set dphi = Ind = [].

"""
function varpro(ctx)
    y, W, q, m, n1, n =  ctx.y, ctx.w, ctx.q, ctx.m, ctx.n1, ctx.n

    T = eltype(ctx.alpha)
    nl2_msg = ""

    #
    # Solve the least squares problem using NL2sol or, if there
    # are no nonlinear parameters, using the SVD procedure in formJacobian.
    #
    regression = Regression(T, q, n, m)

    if q > 0  # The problem is nonlinear.

        if ctx.iscomplex
            alpha_real = [real(ctx.alpha); imag(ctx.alpha)]
            mreal = 2 * ctx.m
        else
            alpha_real = ctx.alpha
            mreal = ctx.m
        end

        f(a, r) = f_lsq(a, r, ctx)
        g(a, j) = g_lsq(a, j, ctx)
        iv, v = nl2_set_defaults(mreal, length(alpha_real))
        iv[MXFCAL] = ctx.mxfcal
        iv[MXITER] = ctx.mxiter
        iv[PRUNIT] = 0
        if ctx.opto == NL2SOL
            results = nl2sol(f, g, alpha_real, mreal, iv, v)
            ctx.verbose && println("\nNL2sol return code: ", return_code[iv[1]])
        else
            results = nl2sno(f, alpha_real, mreal, iv, v)
            ctx.verbose && println("\nNL2sno return code: ", return_code[iv[1]])
        end

        ctx.verbose && println(nl2_msg)
        ctx.verbose && @show(results)
        alpha_real .= results.minimum
        if ctx.iscomplex
            ctx.alpha .= complex.(alpha_real[1:ctx.q], alpha_real[ctx.q+1:end])  ## cwt barks here
        end
        wresid_norm2 = results.f_minimum
        f_lsq(alpha_real, ctx.wresid_real, ctx)
        g_lsq(alpha_real, ctx.jac_real,  ctx)
        r, Jacobian, phi, dphi, y_est = ctx.wresid, ctx.jac, ctx.phi, ctx.dphi, ctx.y_est

        wresid = r
        wresid_norm = sqrt(wresid_norm2)
        regression.results = results
        regression.rank = ctx.rank

    else       # The problem is linear.

        ctx.verbose && println("VARPRO problem is linear, since length(alpha) = 0")

        Jacobian = formJacobian(ctx)
        c = ctx.c
        wresid = ctx.wresid
        y_est = ctx.y_est
        regression.report.rank = ctx.rank
        wresid_norm = norm(wresid)
        wresid_norm2 = wresid_norm * wresid_norm
    end

    # Calculate sample variance,  the norm-squared of the residual
    #    divided by the number of degrees of freedom.
    sigma2 = wresid_norm2 / float(m - n - q)

    # Compute  Regression.sigma:
    #     square-root of weighted residual norm squared divided
    #     by number of degrees of freedom.
    regression.sigma = sqrt(sigma2)

    # Compute Regression.coef_determ:
    #     The coeficient of determination for the fit, also called the square
    #     of the multiple correlation coefficient, or R^2. It is computed as
    #     1 - wresid_norm^2/CTSS, where the corrected total sum of squares
    #     CTSS is the norm-squared of W*(y-y_bar), and the entries of y_bar
    #     are all equal to (the sum of W_i y_i) divided by (the sum of W_i).

    w = diag(W)
    y_bar = sum(w.*y) / sum(w)
    CTTS = norm(W * (y .- y_bar)) ^ 2
    regression.coef_determ = 1 - wresid_norm^2 / CTTS

    # Compute  regression.RMS = sigma^2:
    #     the weighted residual norm divided by the number of degrees of
    #     freedom. RMS = wresid_norm / sqrt(m-n+q)

    regression.rms = sigma2

    if !isempty(dphi) && !ctx.skip_stats
        # Calculate the covariance matrix CovMx, which is sigma^2 times the
        # inverse of H'*H, where
        #              H = W*[phi,J]
        # contains the partial derivatives of  wresid  with
        # respect to the parameters in alpha and c.

        if ctx.iscomplex
            dphi = dphi[:,1:q]
        end
        xx, pp = size(dphi)
        J = zeros(eltype(ctx.alpha), m, q)
        for kk = 1:pp
            j = ctx.ind[1, kk]
            i = ctx.ind[2, kk]
            if j > n
                J[:, i] = J[:, i] + dphi[:, kk]
            else
                J[:, i] = J[:, i] + ctx.c[j] * dphi[:, kk]
            end
        end
        # Uses compact pivoted QR.
        F = qr(W*[phi[:, 1:n] J], Val(true))
        Qj = F.Q
        Rj = F.R
        Pj = F.p
        T2 = Rj \ diagm(ones((size(Rj, 1))))
        covmx = sigma2 * T2 * T2'
        regression.covmx[Pj, Pj] = covmx  # Undo the pivoting permutation.

        # Compute  regression.CorMx:
        #     estimated correlation matrix (n+q) x (n+q) for the
        #     parameters.  The linear parameters are ordered
        #     first, followed by the nonlinear parameters.
        d = 1 ./ sqrt.(diag(regression.covmx))
        D = spdiagm(0 => d)
        regression.cormx = D * regression.covmx * D

        # Compute  regression.std_param:
        #     The k-th element is the square root of the k-th main
        #     diagonal element of CovMx.
        regression.std_param = sqrt.(diag(regression.covmx))

        # Compute  regression.t_ratio:
        #     parameter estimates divided by their standard deviations.
        regression.t_ratio = [ctx.c; ctx.alpha] .* d

        # Compute  regression.standardized_wresid:
        #     The k-th component is the k-th component of the
        #     weighted residual, divided by its standard deviation.
        #     Let X = W*[phi, J],
        #        h[k] = k-th main diagonal element of covariance
        #               matrix for wresid
        #             = k-th main diagonal element of X*inv(X'*X)*X'
        #             = k-th main diagonal element of Qj*Qj'
        #     Then the standard deviation is estimated by sigma*sqrt(1-h[k])

        temp = zeros(eltype(ctx.alpha), m)
        for k = 1:m
            temp[k] = (Qj[k, :] * Qj[k, :]')[1]
        end
        regression.std_wresid = wresid ./(regression.sigma*sqrt.(1 .- temp))
     end

    if ctx.verbose
        println(" ")
        println("VARPRO Results:")
        println(" Linear Parameters: ", ctx.c)

        println(" Nonlinear Parameters: ", ctx.alpha)

        println(" Norm-squared of weighted residual  = ", wresid_norm2)
        println(" Norm-squared of data vector        = ", norm(w.*y)^2)
        println(" Norm         of weighted residual  = ", wresid_norm)
        println(" Norm         of data vector        = ", norm(w.*y))
        println(" Expected error of observations     = ", regression.sigma)
        println(" Coefficient of determination       = ", regression.coef_determ)
    end

    return ctx.alpha, ctx.c, wresid, wresid_norm, y_est, regression
end

function update_alpha!(alpha_trial, ctx)
    if ctx.iscomplex
        # We do it this way since alpha_trial can be a NL2Array and we haven't
        # implemented slicing for that type.
        for i = 1:ctx.q
            ctx.alpha_real[i] = alpha_trial[i]
            ctx.alpha_real[i+ctx.q] = alpha_trial[i+ctx.q]
        end
        ctx.alpha = complex.(ctx.alpha_real[1:ctx.q], ctx.alpha_real[ctx.q+1:end])
    else
        # again, cannot use a colon assignment here
        for i = 1:ctx.q
            ctx.alpha[i] = alpha_trial[i]
        end
    end
    return nothing
end

"""
# Description
    This function is used by a nonlinear least squares solver NL2sol
    to compute wresid, the current estimate of the weighted residual
    (but it is wrapped in a closure first so it is only a function
     of alpha_trial and wresid).

# Input
    alpha_trial  trial vector of non-linear parameters
    ctx: a varpro context
    function f_lsq(alpha_trial, ctx)

# Output
    wresid.  Also calculates current estimates of c, phi, y_est,
        and myrank and stores in the context struct

# NB
    It uses the user-supplied function ada

    An added complication is that both alpha_trial and r must be
    real for the optimizer.  However both of these values may be
    complex.  In that case, we check ctx to see and then recombine
    into the complex values for ada and then blast back to real
    before returning
"""
function f_lsq(alpha_trial, r, ctx)

    # Convert back to complex if necessary
    update_alpha!(alpha_trial, ctx)
    W = ctx.w

    # We determine the optimal linear parameters c for
    # the given values of alpha, the resulting residual and the y estimate
    #
    # We use the singular value decomposition to solve the linear least
    # squares problem
    #
    #    min_{c} || W resid ||.
    #       resid =  y - phi * c.
    #
    # If W*phi has any singular value less than m * eps() its largest singular value,
    # these singular values are set to zero.
    # The following values on stored on the FitContext:
    #      c        n x 1 the optimal linear parameters for this choice of alpha.
    #      wresid   m x 1 the weighted residual = W(y - phi * c)
    #      y_est    m x 1 the model estimates = phi * c)
    #      rank     1 x 1 the rank of the matrix W*phi.

    ctx.phi = ctx.ada(ctx.alpha, ctx)
    U, S, V = svd(W * ctx.phi[:, 1:ctx.n], full=true)
    ctx.U = U
    ctx.s = S
    ctx.V = V

    if ctx.n > 0
        s = S
    else    #  n = 0

        ctx.c = ctx.iscomplex ? Base.Complex{Float64}[] : Float64[]
        ctx.y_est .= ctx.phi
        ctx.wresid .= W * (y - ctx.y_est)
        r .= ctx.wresid
        ctx.rank = 1
        return ctx.wresid
    end

    # Time to "regularize"
    tol = ctx.m * eps()
    ctx.rank = sum(s .> tol*s[1] ) # number of singular values > tol*norm(W*phi)
    s = s[1:ctx.rank]

    if ctx.rank < ctx.n && ctx.verbose
        println("Warning from VARPRO:")
        println("   The linear parameters are currently not well-determined.")
        println("   The rank of the matrix in the subproblem is $(ctx.rank)")
        println("   which is less than the $(ctx.n) linear parameters.")
    end

    yuse = copy(ctx.y)
    if ctx.n < ctx.n1
        yuse  = ctx.y - ctx.phi[:, n1]  # extra function phi(:,n+1)
    end
    temp  = U[:, 1:ctx.rank]' * (W * yuse)
    ctx.c = V[:, 1:ctx.rank] * (temp ./ s)
    ctx.y_est = ctx.phi[:, 1:ctx.n] * ctx.c
    ctx.wresid = W * (yuse - ctx.y_est)

    # This is most certainly wrong now, but we don't use the case n < n1
    if ctx.n < ctx.n1
        ctx.y_est += ctx.phi[:, ctx.n1]
    end
    if ctx.iscomplex
        ctx.wresid_real[1:ctx.m] = real(ctx.wresid)
        ctx.wresid_real[ctx.m+1:end] = imag(ctx.wresid)
    else
        ctx.wresid_real .= ctx.wresid
    end

    # Set r[:] for NL2sol. This is unrolled because r might be an NL2Array
    for i in 1:length(ctx.wresid_real)
        r[i] = ctx.wresid_real[i]
    end

    return ctx.wresid_real
end

function g_lsq(alpha_trial, jnl2, ctx)
    # function g_lsq(alpha_trial, ctx)
    #
    # Returns: jacobian, but also calculates dphi and stores it in ctx
    #
    # This is an 'extra' function that does not appear in the MATLAB version
    # of Varpro.  It is needed by levenberg_marquardt solver to compute the
    # the Jacobian matrix for the nonlinear parameters
    #
    # It also computes dphi the partial derivatives of phi (if available).
    # It uses the user-supplied function gada and the Varpro function formJacobian.
    #
    # We depend on f_lsq to update ctx.alpha and ctx.alpha_real.  In effect,
    # we assume that we never ask for a gradient unless we have first asked for
    # for the residual.
    f_lsq(alpha_trial, ctx.wresid_real, ctx)

    ctx.gada(ctx.alpha, ctx)
    formJacobian(ctx)

    if ctx.iscomplex
        # This is true because of Cauchy-Riemann Equations
        ctx.jac_real = [real(ctx.jac)  -imag(ctx.jac);
                        imag(ctx.jac)   real(ctx.jac)]
    else
        ctx.jac_real = ctx.jac
    end

    # Now must set the memory that nl2sol uses.
    m, n = size(ctx.jac_real)
    for j = 1:n
        for i in 1:m
            jnl2[i, j] = ctx.jac_real[i, j]
        end
    end

    return ctx.jac_real
end

"""
    function formJacobian(ctx)

    Returns: Jacobian

    This function computes the Jacobian and dphi
    It is used by the functions Varpro and f_lsq.

    Notation: there are m data observations
                        n1 basis functions (columns of phi)
                        n linear parameters c
                            (n = n1, or n = n1 - 1)
                        q nonlinear parameters alpha
                        p nonzero columns of partial derivatives of phi

    Input: (on ctx)
        alpha  q x 1   the nonlinear parameters,
        phi    m x n1  the basis functions phi(alpha),
        dphi   m x p   the partial derivatives of phi

    The variables W, y, q, m, n1, and n are also used.

    Output:

        Jacobian  m x p the Jacobian matrix, with J[i, k] = partial
                    derivative of W resid[i] with respect to alpha[k].
"""
function formJacobian(ctx)
    if ctx.q == 0 || isempty(ctx.dphi)
        ctx.jac = []
        return
    end

    # Compute the Jacobian.
    # There are two pieces, which we call Jac1 and Jac2,
    # with Jacobian = - (Jac1 + Jac2).
    #
    # The formula for Jac1 is (P D(W*phi) pinv(W*phi) y,
    #             and Jac2 is ((W*phi)^+})^T (P D(W*phi))^T y.
    # where  P           is the projection onto the orthogonal complement
    #                       of the range of W*phi,
    #        D(W*phi)    is the m x n x q tensor of derivatives of W*phi,
    #        pinv(W*phi) is the pseudo-inverse of W*phi.
    #  (See Golub&Pereyra (1973) equation (5.4).  We use their notational
    #   conventions for multiplications by tensors.)
    #
    # Golub & Pereyra (2003), p. R5 break these formulas down by columns:
    #     The j-th column of Jac1 is P D_j pinv(W*phi) y
    #                             =  P D_j c
    # and the j-th column of Jac2 is (P D_j pinv(W*phi))^T y,
    #                             =  (pinv(W*phi))^T D_j^T P^T y
    #                             =  (pinv(W*phi))^T D_j^T wresid.
    # where D_j is the m x n matrix of partial derivatives of W*phi
    #     with respect to alpha(j).
    #
    # We begin the computation by precomputing
    #       Wdphi, which contains the derivatives of W*phi, and
    #       Wdphi_r, which contains Wdphi' * wresid.

    Wdphi = ctx.w * ctx.dphi
    Wdphi_r = Wdphi' * ctx.wresid
    T2 = zeros(eltype(ctx.alpha), ctx.n1, ctx.q)
    ctemp = ctx.c
    if ctx.n1 > ctx.n
        ctemp = [ctemp; 1]
    end

    #   Now we work column-by-column, for j=1:q.
    #
    #   We form Jac1 = D(W*phi) ctemp.
    #   After the loop, this matrix is multiplied by
    #        P = U[:, ctx.rank+1:m] * (U[:, ctx.rank+1:m]' * Jac1)
    #   to complete the computation.
    #
    #   We also form  T2 = (D_j(W*phi))^T wresid  by unpacking
    #   the information in Wdphi_r, using ind.
    #   After the loop, T2 is multiplied by the pseudoinverse
    #       (pinv(W*phi))^T = U[:,1:ctx.rank] * diagm(1./s) * (V[:, 1:ctx.rank]'
    #   to complete the computation of Jac2.
    #   Note: if n1 > n, last row of T2 is not needed.
    for j = 1:ctx.q                            # for each nonlinear parameter alpha[j]
        range = findall(ctx.ind[2, :] .== j)   # columns of Wdphi relevant to alpha[j]
        indrows = vec(ctx.ind[1, range])       # relevant rows of ctemp, need as vector
        ctx.jac1[:, j] = Wdphi[:, range] * ctemp[indrows]
        T2[indrows, j] = Wdphi_r[range]
    end

    ctx.jac1 .= ctx.U[:, ctx.rank+1:ctx.m] * (ctx.U[:, ctx.rank+1:ctx.m]' * ctx.jac1)
    T2 = diagm(0 => 1 ./ ctx.s[1:ctx.rank]) * (ctx.V[:, 1:ctx.rank]' * T2[1:ctx.n, :])
    ctx.jac2 .= ctx.U[:, 1:ctx.rank] * T2

    ctx.jac .= -(ctx.jac1 + ctx.jac2)

    if ctx.debug
        if ctx.neglect
            ctx.verbose && println("VARPRO norm(neglected Jacobian)/norm(Jacobian) = ",
                                   norm(ctx.jac2,"fro")/norm(ctx.jac,"fro"))
            ctx.verbose && println("neglecting Jac2")
            ctx.jac .= -ctx.jac1
        end
    end
    return ctx.jac
end

end # module Varpro


"""
function to find MPS approximation of exp(A)v using the Arnoldi method with
numv Krylov space vectors
"""

function Arnoldi_expm(A, v, numv)

    rhov = Array{Array{Array{Complex{Float64}, 3}, 1}, 1}(numv)
    vprod = Array{Array{Array{Complex{Float64}, 3}, 1}, 1}(numv)
    N = complex(zeros(numv, numv))
    H = complex(zeros(numv, numv))

    rhov[1] = v
    vprod[1] = applyMPOtoMPS(rhov[1], A)
    H[1] = scal_prod(rhov[1], vprod[1])
    N[1] = scal_prod(rhov[1], rhov[1])

    coef = complex(zeros(numv))

    for jj = 2:numv

        coef[1:jj - 1] = [-H[ii, jj - 1]/N[ii, ii] for ii in 1:jj - 1]
        rhov[jj] = compress_sum_var(v, [1.0, coef[1:jj - 1]...],
                                                  [vprod[jj - 1], rhov[1:jj - 1]...], 4)
        vprod[jj] = applyMPOtoMPS(rhov[jj], A)
        H[jj, jj] = scal_prod(rhov[jj], vprod[jj])
        N[jj, jj] = scal_prod(rhov[jj], rhov[jj])

        for ll = 1:jj - 1

            H[jj, ll] = scal_prod(rhov[jj], vprod[ll])
            H[ll, jj] = scal_prod(rhov[ll], vprod[jj])
            N[jj, ll] = scal_prod(rhov[jj], rhov[ll])
            N[ll, jj] = conj(N[jj, ll])

        end

    end
    println(H)
    println(diag(N))
    coef = expm(N\H)[:, 1]
    println(coef)
    return compress_sum_var(v, coef, rhov, 4)

end

function Arnoldi_expm_norm(A, v, numv)

    rhov = Array{Array{Array{Complex{Float64}, 3}, 1}, 1}(numv)
    vprod = Array{Array{Array{Complex{Float64}, 3}, 1}, 1}(numv)
    N = complex(eye(numv))
    H = complex(zeros(numv, numv))

    rhov[1] = deepcopy(v)
    v_norm = normalize_lo!(rhov[1])
    vprod[1] = applyMPOtoMPS(rhov[1], A)
    H[1] = scal_prod(rhov[1], vprod[1])

    coef = complex(zeros(numv))

    for jj = 2:numv

        coef[1:jj - 1] = [-H[ii, jj - 1]/N[ii, ii] for ii in 1:jj - 1]
        rhov[jj] = compress_sum_var(v, [1.0, coef[1:jj - 1]...],
                                                  [vprod[jj - 1], rhov[1:jj - 1]...], 1)
        no = normalize_lo!(rhov[jj])
        println(no)
        vprod[jj] = applyMPOtoMPS(rhov[jj], A)
        H[jj, jj] = scal_prod(rhov[jj], vprod[jj])

        for ll = 1:jj - 1

            H[jj, ll] = scal_prod(rhov[jj], vprod[ll])
            H[ll, jj] = scal_prod(rhov[ll], vprod[jj])
            N[jj, ll] = scal_prod(rhov[jj], rhov[ll])
            N[ll, jj] = conj(N[jj, ll])

        end

    end
    coef = sqrt(v_norm)*expm(N\H)[:, 1]
    println(coef)
    return compress_sum_var(v, coef, rhov, 1)

end

function Arnoldi_expm!(A, rhov, numv, env, envop)

    N = complex(eye(numv))
    H = complex(zeros(numv, numv))

    H[1, 1] = scal_op_prod(rhov[1], A, rhov[1])

    for jj = 2:numv

        compress_sum_var_apply_H!(rhov[jj], envop,
                                    [- H[1:(jj - 1), jj - 1]..., 1.0],
                                        [rhov[1:(jj - 1)]..., rhov[jj - 1]], A, 1)
        normalize_lo!(rhov[jj])
        H[jj, jj] = scal_op_prod(rhov[jj], A, rhov[jj])

        for ll = 1:jj - 1

            H[jj, ll] = scal_op_prod(rhov[jj], A, rhov[ll])
            H[ll, jj] = scal_op_prod(rhov[ll], A, rhov[jj])
            N[jj, ll] = scal_prod(rhov[jj], rhov[ll])
            N[ll, jj] = conj(N[jj, ll])

        end

    end
    coef = expm(N\H)[:, 1]
    println(abs.(coef))
    rhov[1] = compress_sum_var(rhov[1], coef, rhov, 1)
    normalize_lo!(rhov[1])
    nothing

end
"""
function to evolve MPS using the 4th order Runga Kutta method
"""

function RK4stp(dt,H,psi,k0)

k1 = compress_var(k0,applyMPOtoMPS(psi,H[1]),1)
k2 = compress_var(k1,applyMPOtoMPS(
    compress_sum_var(psi,[1,dt/2],[psi,k1],1),H[2]),1)
k3 = compress_var(k2,applyMPOtoMPS(
    compress_sum_var(psi,[1,dt/2],[psi,k2],1),H[2]),1)
k4 = compress_var(k3,applyMPOtoMPS(
    compress_sum_var(psi,[1,dt],[psi,k3],1),H[3]),1)

psi = compress_sum_var(psi,[1,dt/6,dt/3,dt/3,dt/6],[psi,k1,k2,k3,k4],1)
return psi,k4

end

"""
function to evolve MPS using the 4th order Runga Kutta method
"""

function RK4stp_alt(dt,H,psi)

psi1 = compress_sum_var(psi,[1,dt/2],[psi,applyMPOtoMPS(psi,H[1])],1)
psi2 = compress_sum_var(psi1,[1,dt/2],[psi,applyMPOtoMPS(psi1,H[2])],1)
psi3 = compress_sum_var(psi2,[1,dt],[psi,applyMPOtoMPS(psi2,H[2])],1)
psi = compress_sum_var(psi3,[-1/3,1/3,2/3,1/3,dt/6],[psi,psi1,psi2,psi3,
                       applyMPOtoMPS(psi3,H[3])],1)

end

"""
function to evolve MPS using the 4th order Runga Kutta method
"""

function RK4stp_apply_H(dt,H,psi)

psi1 = compress_var_apply_H(psi,psi,H[1],1)
psi2 = compress_sum_var_apply_H(psi1,[1,dt/2],[psi,psi1],H[2],1)
psi3 = compress_sum_var_apply_H(psi2,[1,dt],[psi,psi2],H[2],1)
psi = compress_sum_var_apply_H(psi3,[-1/3,1/3,2/3,1/3],[psi,psi1,psi2,
                               psi3],H[3],1)

end

function RK4stp_apply_H_half(dt, H, psi)

psi1 = compress_var_apply_H_left(psi, psi, H[1])
psi2 = compress_sum_var_apply_H_right(psi1, [1, dt/2], [psi, psi1], H[2])
psi3 = compress_sum_var_apply_H_left(psi2, [1, dt], [psi, psi2], H[2])
compress_sum_var_apply_H_right(psi3, [-1/3, 1/3, 2/3, 1/3],
                                           [psi, psi1, psi2, psi3], H[3])

end

function RK4stp_apply_H_half!(dt, H, psi, psi1, psi2, psi3, env1, env2, env3, envop)

compress_var_apply_H_left!(psi1, envop, psi, psi, H[1])
compress_sum_var_apply_H_right!(psi2, [env1], envop, psi1, [1, dt/2], [psi, psi1], H[2])
compress_sum_var_apply_H_left!(psi3, [env1], envop, psi2, [1, dt], [psi, psi2], H[2])
compress_sum_var_apply_H_right!(psi, [env1, env2, env3], envop, psi3, [-1/3, 1/3, 2/3, 1/3],
                                [psi, psi1, psi2, psi3], H[3])

end

function RK4stp_apply_H_half!(dt, H, psi0, psi, psi1, psi2, psi3, env1, env2, env3, envop)

compress_var_apply_H_left!(psi1, envop, psi, psi, H[1])
compress_sum_var_apply_H_right!(psi2, [env1], envop, psi1, [1, dt/2], [psi, psi1], H[2])
compress_sum_var_apply_H_left!(psi3, [env1], envop, psi2, [1, dt], [psi, psi2], H[2])
compress_sum_var_apply_H_right!(psi0, [env1, env2, env3], envop, psi3, [-1/3, 1/3, 2/3, 1/3],
                                [psi, psi1, psi2, psi3], H[3])

end

function RK4stp_apply_H_half_two_site(dt,H,psi,svd_tol,dims,dlim)

psi1 = compress_var_apply_H_two_site_left(psi,psi,H[1],1,svd_tol,dims,dlim)
psi2 = compress_sum_var_apply_H_right(psi1,[1,dt/2],[psi,psi1],H[2],1)
psi3 = compress_sum_var_apply_H_left(psi2,[1,dt],[psi,psi2],H[2],1)
psi = compress_sum_var_apply_H_right(psi3,[-1/3,1/3,2/3,1/3],[psi,psi1,psi2,
                               psi3],H[3],1)

end

function RK4stp_apply_H_half_full_two_site(dt,H,psi,svd_tol,dims,dlim)

psi1 = compress_var_apply_H_two_site_left(psi,psi,H[1],1,svd_tol,dims,dlim)
psi2 = compress_sum_var_apply_H_two_site_right(psi1,[1,dt/2],[psi,psi1],H[2],1,svd_tol,dims,dlim)
psi3 = compress_sum_var_apply_H_two_site_left(psi2,[1,dt],[psi,psi2],H[2],1,svd_tol,dims,dlim)
psi = compress_sum_var_apply_H_two_site_right(psi3,[-1/3,1/3,2/3,1/3],[psi,psi1,psi2,
                               psi3],H[3],1,svd_tol,dims,dlim)

end

function RK4stp_apply_H_half_two_site_2(dt,H,psi,svd_tol,dims,dlim)

psi1 = compress_var_apply_H_left(psi,psi,H[1],1)
psi2 = compress_sum_var_apply_H_right(psi1,[1,dt/2],[psi,psi1],H[2],1)
psi3 = compress_sum_var_apply_H_left(psi2,[1,dt],[psi,psi2],H[2],1)
psi = compress_sum_var_apply_H_two_site_right(psi3,[-1/3,1/3,2/3,1/3],[psi,psi1,psi2,
                               psi3],H[3],1,svd_tol,dims,dlim)

end

"""
function to evolve MPS A (must be in left canonical form)
in time acording to the trapezoidal method
"""

function Evolve_trap(A,LF,LB,maxsweeps,nor)

	na = length(A)
	At = deepcopy(A)
	FL = Array{Array}(na)
	FR = Array{Array}(na)
	SL = Array{Array}(na)
	SR = Array{Array}(na)
	XY = applyMPOtoMPO(LF,Conj_MPO(LB))
	XX = applyMPOtoMPO(LB,Conj_MPO(LB))

	FL[1] = ones(1,1,1)
	SL[1] = ones(1,1,1)
	for jj = 1:na-1
		FL[jj+1] = update_le(At[jj],XY[jj],A[jj],FL[jj])
		SL[jj+1] = update_le(At[jj],XX[jj],At[jj],SL[jj])
	end
	FR[na] = ones(1,1,1)
	SR[na] = ones(1,1,1)

	for sweep = 1:maxsweeps

		for jj = na:-1:2
		    HH = reshape(O_prod_LR(SL[jj],XX[jj],SR[jj]),
		        size(SL[jj],1)*size(XX[jj],2)*size(SR[jj],1),
		        size(SL[jj],1)*size(XX[jj],2)*size(SR[jj],1))
		    HE = reshape(A_prod_LOR(FL[jj],A[jj],XY[jj],FR[jj]),
		        size(SL[jj],1)*size(XX[jj],2)*size(SR[jj],1),1)
		    At[jj] = reshape(HH\HE,size(SL[jj],1),size(XX[jj],2),size(SR[jj],1))
		    Q,R = qr(reshape(At[jj],size(At[jj],1),size(At[jj],3)*size(At[jj],2))')
		    At[jj] = reshape(Q',size(Q,2),size(At[jj],2),size(At[jj],3))
		    FR[jj-1] = update_re(At[jj],XY[jj],A[jj],FR[jj])
		    SR[jj-1] = update_re(At[jj],XX[jj],At[jj],SR[jj])
		end
	    HH = reshape(O_prod_LR(SL[1],XX[1],SR[1]),
	        size(SL[1],1)*size(XX[1],2)*size(SR[1],1),
	        size(SL[1],1)*size(XX[1],2)*size(SR[1],1))
	    HE = reshape(A_prod_LOR(FL[1],A[1],XY[1],FR[1]),
	        size(SL[1],1)*size(XX[1],2)*size(SR[1],1),1)
	    At[1] = reshape(HH\HE,size(SL[1],1),size(XX[1],2),size(SR[1],1));

		for jj = 1:na-1;
		    HH = reshape(O_prod_LR(SL[jj],XX[jj],SR[jj]),
		        size(SL[jj],1)*size(XX[jj],2)*size(SR[jj],1),
		        size(SL[jj],1)*size(XX[jj],2)*size(SR[jj],1))
		    HE = reshape(A_prod_LOR(FL[jj],A[jj],XY[jj],FR[jj]),
		        size(SL[jj],1)*size(XX[jj],2)*size(SR[jj],1),1)
		    At[jj] = reshape(HH\HE,size(SL[jj],1),size(XX[jj],2),size(SR[jj],1));
		    Q,R = qr(reshape(At[jj],size(At[jj],1)*size(At[jj],2),size(At[jj],3)));
		    At[jj] = reshape(Q,size(At[jj],1),size(At[jj],2),size(Q,2))
		    FL[jj+1] = update_le(At[jj],XY[jj],A[jj],FL[jj]);
		    SL[jj+1] = update_le(At[jj],XX[jj],At[jj],SL[jj]);
		end
	    HH = reshape(O_prod_LR(SL[na],XX[na],SR[na]),
	        size(SL[na],1)*size(XX[na],2)*size(SR[na],1),
	        size(SL[na],1)*size(XX[na],2)*size(SR[na],1))
	    HE = reshape(A_prod_LOR(FL[na],A[na],XY[na],FR[na]),
	        size(SL[na],1)*size(XX[na],2)*size(SR[na],1),1)
	    At[na] = reshape(HH\HE,size(SL[na],1),size(XX[na],2),size(SR[na],1))

	end

	no = sum(abs2(At[na][:]))

	if nor
	    At[na] = At[na]./sqrt(no)
	end

	return At, no

end


"""
    AbstractVectorTransportMethod

Abstract type for methods for transporting vectors. Such vector transports are not
necessarily linear.

# See also

[`AbstractLinearVectorTransportMethod`](@ref)
"""
abstract type AbstractVectorTransportMethod end

"""
    AbstractLinearVectorTransportMethod <: AbstractVectorTransportMethod

Abstract type for linear methods for transporting vectors, that is transport of a linear
combination of vectors is a linear combination of transported vectors.
"""
abstract type AbstractLinearVectorTransportMethod <: AbstractVectorTransportMethod end

@doc raw"""
    DifferentiatedRetractionVectorTransport{R<:AbstractRetractionMethod} <:
        AbstractVectorTransportMethod

A type to specify a vector transport that is given by differentiating a retraction.
This can be introduced in two ways. Let ``\mathcal M`` be a Riemannian manifold,
``p∈\mathcal M`` a point, and ``X,Y∈ T_p\mathcal M`` denote two tangent vectors at ``p``.

Given a retraction (cf. [`AbstractRetractionMethod`](@ref)) ``\operatorname{retr}``,
the vector transport of `X` in direction `Y` (cf. [`vector_transport_direction`](@ref))
by differentiation this retraction, is given by

```math
\mathcal T^{\operatorname{retr}}_{p,Y}X
= D_Y\operatorname{retr}_p(Y)[X]
= \frac{\mathrm{d}}{\mathrm{d}t}\operatorname{retr}_p(Y+tX)\Bigr|_{t=0}.
```
see [^AbsilMahonySepulchre2008], Section 8.1.2 for more details.

This can be phrased similarly as a [`vector_transport_to`](@ref) by introducing
``q=\operatorname{retr}_pX`` and defining

```math
\mathcal T^{\operatorname{retr}}_{q \gets p}X = \mathcal T^{\operatorname{retr}}_{p,Y}X
```

which in practice usually requires the [`inverse_retract`](@ref) to exists in order to
compute ``Y = \operatorname{retr}_p^{-1}q``.

# Constructor

    DifferentiatedRetractionVectorTransport(m::AbstractRetractionMethod)
"""
struct DifferentiatedRetractionVectorTransport{R<:AbstractRetractionMethod} <:
       AbstractLinearVectorTransportMethod
    retraction::R
end

@doc raw"""
    ParallelTransport <: AbstractVectorTransportMethod

Compute the vector transport by parallel transport, see
[`parallel_transport_to`](@ref)
"""
struct ParallelTransport <: AbstractLinearVectorTransportMethod end

"""
    ProjectionTransport <: AbstractVectorTransportMethod

Specify to use projection onto tangent space as vector transport method within
[`vector_transport_to`](@ref), [`vector_transport_direction`](@ref), or
[`vector_transport_along`](@ref). See [`project`](@ref) for details.
"""
struct ProjectionTransport <: AbstractLinearVectorTransportMethod end

@doc raw"""
    PoleLadderTransport <: AbstractVectorTransportMethod

Specify to use [`pole_ladder`](@ref) as vector transport method within
[`vector_transport_to`](@ref), [`vector_transport_direction`](@ref), or
[`vector_transport_along`](@ref), i.e.

Let $X∈ T_p\mathcal M$ be a tangent vector at $p∈\mathcal M$ and $q∈\mathcal M$ the
point to transport to. Then $x = \exp_pX$ is used to call
`y = `[`pole_ladder`](@ref)`(M, p, x, q)` and the resulting vector is obtained by computing
$Y = -\log_qy$.

The [`PoleLadderTransport`](@ref) posesses two advantages compared to
[`SchildsLadderTransport`](@ref):
* it is cheaper to evaluate, if you want to transport several vectors, since the
  mid point $c$ then stays unchanged.
* while both methods are exact if the curvature is zero, pole ladder is even exact in
  symmetric Riemannian manifolds[^Pennec2018]

The pole ladder was was proposed in [^LorenziPennec2014]. Its name stems from the fact that
it resembles a pole ladder when applied to a sequence of points usccessively.

# Constructor
````julia
PoleLadderTransport(
    retraction = ExponentialRetraction(),
    inverse_retraction = LogarithmicInverseRetraction(),
)
````
Construct the classical pole ladder that employs exp and log, i.e. as proposed
in[^LorenziPennec2014]. For an even cheaper transport the inner operations can be
changed to an [`AbstractRetractionMethod`](@ref) `retraction` and an
[`AbstractInverseRetractionMethod`](@ref) `inverse_retraction`, respectively.

[^LorenziPennec2014]:
    > Lorenzi, M. and Pennec, X: Efficient parallel transport of deformations in time
    > series of images: From Schild’s to pole ladder.
    > Journal of Mathematical Imaging and Vision (2014), 50(1), pp. 5–17
    > doi [10.1007/s10851-013-0470-3](https://doi.org/10.1007/s10851-013-0470-3),
    > hal: [hal-00870489](https://hal.inria.fr/hal-00870489)
[^Pennec2018]:
    > Pennec, X: Parallel Transport with Pole Ladder: a Third Order Scheme in Affine
    > Connection Spaces which is Exact in Affine Symmetric Spaces.
    > arXiv: [1805.11436](https://arxiv.org/abs/1805.11436)

"""
struct PoleLadderTransport{
    RT<:AbstractRetractionMethod,
    IRT<:AbstractInverseRetractionMethod,
} <: AbstractLinearVectorTransportMethod
    retraction::RT
    inverse_retraction::IRT
    function PoleLadderTransport(
        retraction = ExponentialRetraction(),
        inverse_retraction = LogarithmicInverseRetraction(),
    )
        return new{typeof(retraction),typeof(inverse_retraction)}(
            retraction,
            inverse_retraction,
        )
    end
end

@doc raw"""
    ScaledVectorTransport{T} <: AbstractVectorTransportMethod

Introduce a scaled variant of any [`AbstractVectorTransportMethod`](@ref) `T`,
as introduced in [^SatoIwai2013] for some ``X∈ T_p\mathcal M`` as

```math
    \mathcal T^{\mathrm{S}}(X) = \frac{\lVert X\rVert_p}{\lVert \mathcal T(X)\rVert_q}\mathcal T(X).
```

Note that the resulting point `q` has to be known, i.e. for [`vector_transport_direction`](@ref)
the curve or more precisely its end point has to be known (via an exponential map or a
retraction). Therefore a default implementation is only provided for the [`vector_transport_to`](@ref)

# Constructor

    ScaledVectorTransport(m::AbstractVectorTransportMethod)

[^SatoIwai2013]:
    > Sato, H., Iwai, T.: _A new, globally convergent Riemannian conjugate gradient method_,
    > Optimization, 2013, Volume 64(4), pp. 1011–1031.
    > doi: [10.1080/02331934.2013.836650](https://doi.org/10.1080/02331934.2013.836650),
    > arXiv: [1302.0125](https://arxiv.org/abs/1302.0125).
"""
struct ScaledVectorTransport{T<:AbstractVectorTransportMethod} <:
       AbstractVectorTransportMethod
    method::T
end

@doc raw"""
    SchildsLadderTransport <: AbstractVectorTransportMethod

Specify to use [`schilds_ladder`](@ref) as vector transport method within
[`vector_transport_to`](@ref), [`vector_transport_direction`](@ref), or
[`vector_transport_along`](@ref), i.e.

Let $X∈ T_p\mathcal M$ be a tangent vector at $p∈\mathcal M$ and $q∈\mathcal M$ the
point to transport to. Then

````math
P^{\mathrm{S}}_{q\gets p}(X) =
    \log_q\bigl( \operatorname{retr}_p ( 2\operatorname{retr}_p^{-1}c ) \bigr),
````

where $c$ is the mid point between $q$ and $d=\exp_pX$.

This method employs the internal function [`schilds_ladder`](@ref)`(M, p, d, q)` that avoids
leaving the manifold.

The name stems from the image of this paralleltogram in a repeated application yielding the
image of a ladder. The approximation was proposed in [^EhlersPiraniSchild1972].

# Constructor
````julia
SchildsLadderTransport(
    retraction = ExponentialRetraction(),
    inverse_retraction = LogarithmicInverseRetraction(),
)
````
Construct the classical Schilds ladder that employs exp and log, i.e. as proposed
in[^EhlersPiraniSchild1972]. For an even cheaper transport these inner operations can be
changed to an [`AbstractRetractionMethod`](@ref) `retraction` and an
[`AbstractInverseRetractionMethod`](@ref) `inverse_retraction`, respectively.

[^EhlersPiraniSchild1972]:
    > Ehlers, J., Pirani, F.A.E., Schild, A.: The geometry of free fall and light
    > propagation. In: O’Raifeartaigh, L. (ed.) General Relativity: Papers in Honour of
    > J. L. Synge, pp. 63–84. Clarendon Press, Oxford (1972).
    > reprint doi: [10.1007/s10714-012-1353-4](https://doi.org/10.1007/s10714-012-1353-4)

"""
struct SchildsLadderTransport{
    RT<:AbstractRetractionMethod,
    IRT<:AbstractInverseRetractionMethod,
} <: AbstractLinearVectorTransportMethod
    retraction::RT
    inverse_retraction::IRT
    function SchildsLadderTransport(
        retraction = ExponentialRetraction(),
        inverse_retraction = LogarithmicInverseRetraction(),
    )
        return new{typeof(retraction),typeof(inverse_retraction)}(
            retraction,
            inverse_retraction,
        )
    end
end

@doc raw"""
    VectorTransportDirection{VM<:AbstractVectorTransportMethod,RM<:AbstractRetractionMethod}
        <: AbstractVectorTransportMethod

Specify a [`vector_transport_direction`](@ref) using a [`AbstractVectorTransportMethod`](@ref)
with explicitly using the [`AbstractRetractionMethod`](@ref) to determine the point in
the specified direction where to transsport to.
Note that you only need this for the non-default (non-implicit) second retraction method
associated to a vector transport, i.e. when a first implementation assumed
an implicit associated retraction.
"""
struct VectorTransportDirection{
    VM<:AbstractVectorTransportMethod,
    RM<:AbstractRetractionMethod,
} <: AbstractVectorTransportMethod
    retraction::RM
    vector_transport::VM
    function VectorTransportDirection(
        vector_transport = ParallelTransport(),
        retraction = ExponentialRetraction(),
    )
        return new{typeof(vector_transport),typeof(retraction)}(
            retraction,
            vector_transport,
        )
    end
end

@doc raw"""
    VectorTransportTo{VM<:AbstractVectorTransportMethod,RM<:AbstractRetractionMethod}
        <: AbstractVectorTransportMethod

Specify a [`vector_transport_to`](@ref) using a [`AbstractVectorTransportMethod`](@ref)
with explicitly using the [`AbstractInverseRetractionMethod`](@ref) to determine the direction
that transports from  in `p`to `q`.
Note that you only need this for the non-default (non-implicit) second retraction method
associated to a vector transport, i.e. when a first implementation assumed
an implicit associated retraction.
"""
struct VectorTransportTo{
    VM<:AbstractVectorTransportMethod,
    IM<:AbstractInverseRetractionMethod,
} <: AbstractVectorTransportMethod
    inverse_retraction::IM
    vector_transport::VM
    function VectorTransportTo(
        vector_transport = ParallelTransport(),
        inverse_retraction = LogarithmicInverseRetraction(),
    )
        return new{typeof(vector_transport),typeof(inverse_retraction)}(
            inverse_retraction,
            vector_transport,
        )
    end
end

"""
    default_vector_transport_method(M::AbstractManifold)

The [`AbstractVectorTransportMethod`](@ref) that is used when calling
[`vector_transport_along`](@ref), [`vector_transport_to`](@ref), or
[`vector_transport_direction`](@ref) without specifying the vector transport method.
By default, this is [`ParallelTransport`](@ref).
"""
function default_vector_transport_method(::AbstractManifold)
    return ParallelTransport()
end

@doc raw"""
    pole_ladder(
        M,
        p,
        d,
        q,
        c = mid_point(M, p, q);
        retraction=default_retraction_method(M),
        inverse_retraction=default_inverse_retraction_method(M)
    )

Compute an inner step of the pole ladder, that can be used as a [`vector_transport_to`](@ref).
Let $c = \gamma_{p,q}(\frac{1}{2})$ mid point between `p` and `q`, then the pole ladder is
given by

````math
    \operatorname{Pl}(p,d,q) = \operatorname{retr}_d (2\operatorname{retr}_d^{-1}c)
````

Where the classical pole ladder employs $\operatorname{retr}_d=\exp_d$
and $\operatorname{retr}_d^{-1}=\log_d$ but for an even cheaper transport these can be set
to different [`AbstractRetractionMethod`](@ref) and [`AbstractInverseRetractionMethod`](@ref).

When you have $X=log_pd$ and $Y = -\log_q \operatorname{Pl}(p,d,q)$,
you will obtain the [`PoleLadderTransport`](@ref). When performing multiple steps, this
method avoids the switching to the tangent space. Keep in mind that after $n$ successive
steps the tangent vector reads $Y_n = (-1)^n\log_q \operatorname{Pl}(p_{n-1},d_{n-1},p_n)$.

It is cheaper to evaluate than [`schilds_ladder`](@ref), sinc if you want to form multiple
ladder steps between `p` and `q`, but with different `d`, there is just one evaluation of a geodesic
each., since the center `c` can be reused.
"""
function pole_ladder(
    M,
    p,
    d,
    q,
    c = mid_point(M, p, q);
    retraction = default_retraction_method(M),
    inverse_retraction = default_inverse_retraction_method(M),
)
    return retract(M, d, 2 * inverse_retract(M, d, c, inverse_retraction), retraction)
end
@doc raw"""
    pole_ladder(
        M,
        pl,
        p,
        d,
        q,
        c = mid_point(M, p, q),
        X = allocate_result_type(M, log, d, c);
        retraction = default_retraction_method(M),
        inverse_retraction = default_inverse_retraction_method(M),
    )

Compute the [`pole_ladder`](@ref), i.e. the result is saved in `pl`.
`X` is used for storing intermediate inverse retraction.
"""
function pole_ladder!(
    M,
    pl,
    p,
    d,
    q,
    c = mid_point(M, p, q),
    X = allocate_result(M, log, d, c);
    retraction = default_retraction_method(M),
    inverse_retraction = default_inverse_retraction_method(M),
)
    inverse_retract!(M, X, d, c, inverse_retraction)
    X *= 2
    return retract!(M, pl, d, X, retraction)
end

@doc raw"""
    schilds_ladder(
        M,
        p,
        d,
        q,
        c = mid_point(M, q, d);
        retraction = default_retraction_method(M),
        inverse_retraction = default_inverse_retraction_method(M),
    )

Perform an inner step of schilds ladder, which can be used as a
[`vector_transport_to`](@ref), see [`SchildsLadderTransport`](@ref).
Let $c = \gamma_{q,d}(\frac{1}{2})$ denote the mid point
on the shortest geodesic connecting $q$ and the point $d$. Then Schild's ladder reads as

````math
\operatorname{Sl}(p,d,q) = \operatorname{retr}_x( 2\operatorname{retr}_p^{-1} c)
````

Where the classical Schilds ladder employs $\operatorname{retr}_d=\exp_d$
and $\operatorname{retr}_d^{-1}=\log_d$ but for an even cheaper transport these can be set
to different [`AbstractRetractionMethod`](@ref) and [`AbstractInverseRetractionMethod`](@ref).

In consistency with [`pole_ladder`](@ref) you can change the way the mid point is computed
using the optional parameter `c`, but note that here it's the mid point between `q` and `d`.

When you have $X=log_pd$ and $Y = \log_q \operatorname{Sl}(p,d,q)$,
you will obtain the [`PoleLadderTransport`](@ref).
Then the approximation to the transported vector is given by $\log_q\operatorname{Sl}(p,d,q)$.

When performing multiple steps, this method avoidsd the switching to the tangent space.
Hence after $n$ successive steps the tangent vector reads
$Y_n = \log_q \operatorname{Pl}(p_{n-1},d_{n-1},p_n)$.
"""
function schilds_ladder(
    M,
    p,
    d,
    q,
    c = mid_point(M, q, d);
    retraction = default_retraction_method(M),
    inverse_retraction = default_inverse_retraction_method(M),
)
    return retract(M, p, 2 * inverse_retract(M, p, c, inverse_retraction), retraction)
end
@doc raw"""
    schilds_ladder!(
        M,
        sl
        p,
        d,
        q,
        c = mid_point(M, q, d),
        X = allocate_result_type(M, log, d, c);
        retraction = default_retraction_method(M),
        inverse_retraction = default_inverse_retraction_method(M),
    )

Compute [`schilds_ladder`](@ref) and return the value in the parameter `sl`.
If the required mid point `c` was computed before, it can be passed using `c`,
and the allocation of new memory can be avoided providing a tangent vector `X`
for the interims result.
"""
function schilds_ladder!(
    M,
    sl,
    p,
    d,
    q,
    c = mid_point(M, q, d),
    X = allocate_result(M, log, d, c);
    retraction = default_retraction_method(M),
    inverse_retraction = default_inverse_retraction_method(M),
)
    inverse_retract!(M, X, p, c, inverse_retraction)
    X *= 2
    return retract!(M, sl, p, X, retraction)
end

function show(io::IO, ::ParallelTransport)
    return print(io, "ParallelTransport()")
end
function show(io::IO, m::ScaledVectorTransport)
    return print(io, "ScaledVectorTransport($(m.method))")
end

"""
    vector_transport_along(M::AbstractManifold, p, X, c)
    vector_transport_along(M::AbstractManifold, p, X, c, m::AbstractVectorTransportMethod)

Transport a vector `X` from the tangent space at a point `p` on the [`AbstractManifold`](@ref) `M`
along the curve represented by `c` using the `method`, which defaults to
[`default_vector_transport_method`](@ref)`(M)`.
"""
function vector_transport_along(
    M::AbstractManifold,
    p,
    X,
    c,
    m::AbstractVectorTransportMethod = default_vector_transport_method(M),
)
    return _vector_transport_along(M, p, X, c, m)
end
function _vector_transport_along(M::AbstractManifold, p, X, c, ::ParallelTransport)
    return parallel_transport_along(M, p, X, c)
end
function _vector_transport_along(
    M::AbstractManifold,
    p,
    X,
    c,
    m::DifferentiatedRetractionVectorTransport,
)
    return vector_transport_along_diff(M, p, X, c, m.retraction)
end
@doc raw"""
    vector_transport_along_diff(M::AbstractManifold, p, X, c, m::AbstractRetractionMethod)

Compute the vector transport of `X` from ``T_p\mathcal M`` along the curve `c`
using the differential of the [`AbstractRetractionMethod`](@ref) `m`.
"""
function vector_transport_along_diff(
    M::AbstractManifold,
    p,
    X,
    c,
    m::AbstractRetractionMethod,
)
    Y = allocate_result(M, vector_transport_along, X, p)
    return vector_transport_along_diff!(M, Y, p, X, c, m)
end
@doc raw"""
    vector_transport_along_diff!(M::AbstractManifold, Y, p, X, c, m::AbstractRetractionMethod)

Compute the vector transport of `X` from ``T_p\mathcal M`` along the curve `c`
using the differential of the [`AbstractRetractionMethod`](@ref) `m` in place of `Y`.
"""
vector_transport_along_diff!(M::AbstractManifold, Y, p, X, c, m)

function vector_transport_along_diff! end

function _vector_transport_along(M::AbstractManifold, p, X, c, ::ProjectionTransport)
    return vector_transport_along_project(M, p, X, c)
end
@doc raw"""
    vector_transport_along_project(M::AbstractManifold, p, X, c::AbstractVector)

Compute the vector transport of `X` from ``T_p\mathcal M`` along the curve `c`
using a projection.
"""
function vector_transport_along_project(M::AbstractManifold, p, X, c::AbstractVector)
    Y = allocate_result(M, vector_transport_along, X, p)
    return vector_transport_along_project!(M, Y, p, X, c)
end
@doc raw"""
    vector_transport_along_project!(M::AbstractManifold, Y, p, X, c::AbstractVector)

Compute the vector transport of `X` from ``T_p\mathcal M`` along the curve `c`
using a projection. The result is computed in place of `Y`.
"""
vector_transport_along_project!(M::AbstractManifold, Y, p, X, c::AbstractVector)

function vector_transport_along_project! end

function _vector_transport_along(
    M::AbstractManifold,
    p,
    X,
    c::AbstractVector,
    m::PoleLadderTransport,
)
    Y = allocate_result(M, vector_transport_along, X, p)
    return _vector_transport_along!(M, Y, p, X, c, m)
end
function _vector_transport_along(
    M::AbstractManifold,
    p,
    X,
    c::AbstractVector,
    m::SchildsLadderTransport,
)
    Y = allocate_result(M, vector_transport_along, X, p)
    return _vector_transport_along!(M, Y, p, X, c, m)
end

"""
    vector_transport_along!(M::AbstractManifold, Y, p, X, c::AbstractVector)
    vector_transport_along!(M::AbstractManifold, Y, p, X, c::AbstractVector, m::AbstractVectorTransportMethod)

Transport a vector `X` from the tangent space at a point `p` on the [`AbstractManifold`](@ref) `M`
along the curve represented by `c` using the `method`, which defaults to
[`default_vector_transport_method`](@ref)`(M)`. The result is saved to `Y`.
"""
function vector_transport_along!(
    M::AbstractManifold,
    Y,
    p,
    X,
    c::AbstractVector,
    m::AbstractVectorTransportMethod = default_vector_transport_method(M),
)
    return _vector_transport_along!(M, Y, p, X, c, m)
end

function parallel_transport_along!(M::AbstractManifold, Y, p, X, c::AbstractVector)
    n = length(c)
    if n == 0
        copyto!(Y, X)
    else
        parallel_transport_to!(M, Y, p, X, c[1])
        for i in 1:(length(c) - 1)
            parallel_transport_to!(M, Y, c[i], Y, c[i + 1])
        end
    end
    return Y
end

function _vector_transport_along!(
    M::AbstractManifold,
    Y,
    p,
    X,
    c::AbstractVector,
    ::ParallelTransport,
)
    return parallel_transport_along!(M, Y, p, X, c)
end
function _vector_transport_along!(
    M::AbstractManifold,
    Y,
    p,
    X,
    c::AbstractVector,
    m::DifferentiatedRetractionVectorTransport,
)
    return vector_transport_along_diff!(M, Y, p, X, c, m.retraction)
end
function _vector_transport_along!(
    M::AbstractManifold,
    Y,
    p,
    X,
    c::AbstractVector,
    ::ProjectionTransport,
)
    return vector_transport_along_project!(M, Y, p, X, c)
end

@doc raw"""
    function vector_transport_along(
        M::AbstractManifold,
        p,
        X,
        c::AbstractVector,
        m::PoleLadderTransport
    )

Compute the vector transport along a discretized curve using
[`PoleLadderTransport`](@ref) succesively along the sampled curve.
This method is avoiding additional allocations as well as inner exp/log by performing all
ladder steps on the manifold and only computing one tangent vector in the end.
"""
vector_transport_along(
    M::AbstractManifold,
    Y,
    p,
    X,
    c::AbstractVector,
    m::PoleLadderTransport,
)

function _vector_transport_along!(
    M::AbstractManifold,
    Y,
    p,
    X,
    c::AbstractVector,
    m::PoleLadderTransport,
)
    clen = length(c)
    if clen == 0
        copyto!(Y, X)
    else
        d = retract(M, p, X, m.retraction)
        mp = mid_point(M, p, c[1])
        pole_ladder!(
            M,
            d,
            p,
            d,
            c[1],
            mp,
            Y;
            retraction = m.retraction,
            inverse_retraction = m.inverse_retraction,
        )
        for i in 1:(clen - 1)
            # precompute mid point inplace
            ci = c[i]
            cip1 = c[i + 1]
            mid_point!(M, mp, ci, cip1)
            # compute new ladder point
            pole_ladder!(
                M,
                d,
                ci,
                d,
                cip1,
                mp,
                Y;
                retraction = m.retraction,
                inverse_retraction = m.inverse_retraction,
            )
        end
        inverse_retract!(M, Y, c[clen], d, m.inverse_retraction)
        Y *= (-1)^clen
    end
    return Y
end

@doc raw"""
    vector_transport_along(
        M::AbstractManifold,
        p,
        X,
        c::AbstractVector,
        m::SchildsLadderTransport
    )

Compute the vector transport along a discretized curve using
[`SchildsLadderTransport`](@ref) succesively along the sampled curve.
This method is avoiding additional allocations as well as inner exp/log by performing all
ladder steps on the manifold and only computing one tangent vector in the end.
"""
vector_transport_along(
    M::AbstractManifold,
    p,
    X,
    c::AbstractVector,
    m::SchildsLadderTransport,
)
function _vector_transport_along!(
    M::AbstractManifold,
    Y,
    p,
    X,
    c::AbstractVector,
    m::SchildsLadderTransport,
)
    clen = length(c)
    if clen == 0
        copyto!(Y, X)
    else
        d = retract(M, p, X, m.retraction)
        mp = mid_point(M, c[1], d)
        schilds_ladder!(
            M,
            d,
            p,
            d,
            c[1],
            mp,
            Y;
            retraction = m.retraction,
            inverse_retraction = m.inverse_retraction,
        )
        for i in 1:(clen - 1)
            ci = c[i]
            cip1 = c[i + 1]
            # precompute mid point inplace
            mid_point!(M, mp, cip1, d)
            # compute new ladder point
            schilds_ladder!(
                M,
                d,
                ci,
                d,
                cip1,
                mp,
                Y;
                retraction = m.retraction,
                inverse_retraction = m.inverse_retraction,
            )
        end
        inverse_retract!(M, Y, c[clen], d, m.inverse_retraction)
    end
    return Y
end

@doc raw"""
    vector_transport_direction(M::AbstractManifold, p, X, d)
    vector_transport_direction(M::AbstractManifold, p, X, d, m::AbstractVectorTransportMethod)

Given an [`AbstractManifold`](@ref) ``\mathcal M`` the vector transport is a generalization of the
[`parallel_transport_direction`](@ref) that identifies vectors from different tangent spaces.

More precisely using [^AbsilMahonySepulchre2008], Def. 8.1.1, a vector transport
``T_{p,d}: T_p\mathcal M \to T_q\mathcal M``, ``p∈ \mathcal M``, ``Y∈ T_p\mathcal M`` is a smooth mapping
associated to a retraction ``\operatorname{retr}_p(Y) = q`` such that

1. (associated retraction) ``\mathcal T_{p,d}X ∈ T_q\mathcal M`` if and only if ``q = \operatorname{retr}_p(d)``.
2. (consistency) ``\mathcal T_{p,0_p}X = X`` for all ``X∈T_p\mathcal M``
3. (linearity) ``\mathcal T_{p,d}(αX+βY) = α\mathcal T_{p,d}X + β\mathcal T_{p,d}Y``

For the [`AbstractVectorTransportMethod`](@ref) we might even omit the third point.
The [`AbstractLinearVectorTransportMethod`](@ref)s are linear.

# Input Parameters
* `M` a manifold
* `p` indicating the tangent space of
* `X` the tangent vector to be transported
* `d` indicating a transport direction (and distance through its length)
* `m` an [`AbstractVectorTransportMethod`](@ref), by default [`default_vector_transport_method`](@ref), so usually [`ParallelTransport`](@ref)

Usually this method requires a [`AbstractRetractionMethod`](@ref) as well.
By default this is assumed to be the [`default_retraction_method`](@ref) or
implicitly given (and documented) for a vector transport.
To explicitly distinguish different retractions for a vector transport,
see [`VectorTransportDirection`](@ref).

Instead of spcifying a start direction `d` one can equivalently also specify a target tanget space
``T_q\mathcal M``, see [`vector_transport_to`](@ref).
By default [`vector_transport_direction`](@ref) falls back to using [`vector_transport_to`](@ref),
using the [`default_retraction_method`](@ref) on `M`.

[^AbsilMahonySepulchre2008]:
    > Absil, P.-A., Mahony, R. and Sepulchre R.,
    > _Optimization Algorithms on Matrix Manifolds_
    > Princeton University Press, 2008,
    > doi: [10.1515/9781400830244](https://doi.org/10.1515/9781400830244)
    > [open access](http://press.princeton.edu/chapters/absil/)
"""
function vector_transport_direction(
    M::AbstractManifold,
    p,
    X,
    d,
    m::AbstractVectorTransportMethod = default_vector_transport_method(M),
)
    return _vector_transport_direction(M, p, X, d, m)
end
function _vector_transport_direction(
    M::AbstractManifold,
    p,
    X,
    d,
    m::AbstractVectorTransportMethod = default_vector_transport_method(M),
)
    r = default_retraction_method(M)
    return vector_transport_to(M, p, X, retract(M, p, d, r), m)
end
function _vector_transport_direction(
    M::AbstractManifold,
    p,
    X,
    d,
    m::VectorTransportDirection,
)
    return vector_transport_to(M, p, X, retract(M, p, d, m.retraction), m.vector_transport)
end
function _vector_transport_direction(
    M::AbstractManifold,
    p,
    X,
    d,
    m::DifferentiatedRetractionVectorTransport{R},
) where {R<:AbstractRetractionMethod}
    return vector_transport_direction_diff(M, p, X, d, m.retraction)
end
@doc raw"""
    vector_transport_direction_diff(M::AbstractManifold, p, X, d, m::AbstractRetractionMethod)

Compute the vector transport of `X` from ``T_p\mathcal M`` into the direction `d`
using the differential of the [`AbstractRetractionMethod`](@ref) `m`.
"""
function vector_transport_direction_diff(
    M::AbstractManifold,
    p,
    X,
    d,
    r::AbstractRetractionMethod,
)
    Y = allocate_result(M, vector_transport_direction, p, X, d)
    return vector_transport_direction_diff!(M, Y, p, X, d, r)
end
function _vector_transport_direction(M::AbstractManifold, p, X, d, ::ParallelTransport)
    return parallel_transport_direction(M, p, X, d)
end

"""
    vector_transport_direction!(M::AbstractManifold, Y, p, X, d)
    vector_transport_direction!(M::AbstractManifold, Y, p, X, d, m::AbstractVectorTransportMethod)

Transport a vector `X` from the tangent space at a point `p` on the [`AbstractManifold`](@ref) `M`
in the direction indicated by the tangent vector `d` at `p`. By default, [`retract`](@ref) and
[`vector_transport_to!`](@ref) are used with the `m` and `r`, which default
to [`default_vector_transport_method`](@ref)`(M)` and [`default_retraction_method`](@ref)`(M)`, respectively.
The result is saved to `Y`.

See [`vector_transport_direction`](@ref) for more details.
"""
function vector_transport_direction!(
    M::AbstractManifold,
    Y,
    p,
    X,
    d,
    m::AbstractVectorTransportMethod = default_vector_transport_method(M),
)
    return _vector_transport_direction!(M, Y, p, X, d, m)
end
function _vector_transport_direction!(
    M::AbstractManifold,
    Y,
    p,
    X,
    d,
    m::AbstractVectorTransportMethod = default_vector_transport_method(M),
)
    r = default_retraction_method(M)
    return vector_transport_to!(M, Y, p, X, retract(M, p, d, r), m)
end
function _vector_transport_direction!(
    M::AbstractManifold,
    Y,
    p,
    X,
    d,
    m::VectorTransportDirection,
)
    return vector_transport_to!(
        M,
        Y,
        p,
        X,
        retract(M, p, d, m.retraction),
        m.vector_transport,
    )
end
function _vector_transport_direction!(
    M::AbstractManifold,
    Y,
    p,
    X,
    d,
    m::DifferentiatedRetractionVectorTransport,
)
    return vector_transport_direction_diff!(M, Y, p, X, d, m.retraction)
end
@doc raw"""
    vector_transport_direction_diff!(M::AbstractManifold, Y, p, X, d, m::AbstractRetractionMethod)

Compute the vector transport of `X` from ``T_p\mathcal M`` into the direction `d`
using the differential of the [`AbstractRetractionMethod`](@ref) `m` in place of `Y`.
"""
vector_transport_direction_diff!(M, Y, p, X, d, m)

function vector_transport_direction_diff! end

function _vector_transport_direction!(M::AbstractManifold, Y, p, X, d, ::ParallelTransport)
    return parallel_transport_direction!(M, Y, p, X, d)
end

@doc raw"""
    vector_transport_to(M::AbstractManifold, p, X, q)
    vector_transport_to(M::AbstractManifold, p, X, q, m::AbstractVectorTransportMethod)
    vector_transport_to(M::AbstractManifold, p, X, q, m::AbstractVectorTransportMethod)

Transport a vector `X` from the tangent space at a point `p` on the [`AbstractManifold`](@ref) `M`
along a curve implicitly given by an [`AbstractRetractionMethod`](@ref) associated to `m`.
By default `m` is the [`default_vector_transport_method`](@ref)`(M)`.
To explicitly specify a (different) retraction to the implicitly assumeed retraction, see [`VectorTransportTo`](@ref).
Note that some vector transport methods might also carry their own retraction they are associated to,
like the  [`DifferentiatedRetractionVectorTransport`](@ref) and some are even independent of the retraction, for example the [`ProjectionTransport`](@ref).

This method is equivalent to using ``d = \operatorname{retr}^{-1}_p(q)`` in [`vector_transport_direction`](@ref)`(M, p, X, q, m, r)`,
where you can find the formal definition. This is the fallback for [`VectorTransportTo`](@ref).
"""
function vector_transport_to(
    M::AbstractManifold,
    p,
    X,
    q,
    m::AbstractVectorTransportMethod = default_vector_transport_method(M),
)
    return _vector_transport_to(M, p, X, q, m)
end
function _vector_transport_to(M::AbstractManifold, p, X, q, m::VectorTransportTo)
    d = inverse_retract(M, p, q, m.inverse_retraction)
    return vector_transport_direction(M, p, X, d, m.vector_transport)
end
function _vector_transport_to(M::AbstractManifold, p, X, q, ::ParallelTransport)
    return parallel_transport_to(M, p, X, q)
end
function _vector_transport_to(
    M::AbstractManifold,
    p,
    X,
    q,
    m::DifferentiatedRetractionVectorTransport,
)
    return vector_transport_to_diff(M, p, X, q, m.retraction)
end
@doc raw"""
    vector_transport_to_diff(M::AbstractManifold, p, X, q, r)

Compute a vector transport by using a [`DifferentiatedRetractionVectorTransport`](@ref) `r`.
"""
function vector_transport_to_diff(M::AbstractManifold, p, X, q, r)
    Y = allocate_result(M, vector_transport_to, X, p)
    return vector_transport_to_diff!(M, Y, p, X, q, r)
end
@doc raw"""
    vector_transport_to_diff(M::AbstractManifold, p, X, q, r)

Compute a vector transport by using a [`DifferentiatedRetractionVectorTransport`](@ref) `r` in place of `Y`.
"""
vector_transport_to_diff!(M::AbstractManifold, Y, p, X, q, r)

function vector_transport_to_diff! end

function _vector_transport_to(M::AbstractManifold, p, X, q, ::ProjectionTransport)
    return vector_transport_to_project(M, p, X, q)
end
@doc raw"""
    vector_transport_to_project(M::AbstractManifold, p, X, q)

Compute a vector transport by projecting ``X\in T_p\mathcal M`` onto the tangent
space ``T_q\mathcal M`` at ``q``.
"""
function vector_transport_to_project(M::AbstractManifold, p, X, q)
    Y = allocate_result(M, vector_transport_to, X, p)
    return vector_transport_to_project!(M, Y, p, X, q)
end
@doc raw"""
    vector_transport_to_project!(M::AbstractManifold, Y, p, X, q)

Compute a vector transport by projecting ``X\in T_p\mathcal M`` onto the tangent
space ``T_q\mathcal M`` at ``q`` in place of `Y`.
"""
function vector_transport_to_project!(M::AbstractManifold, Y, p, X, q)
    # Note that we have to use embed (not embed!) since we do not have memory to store this embedded value in
    return project!(M, Y, q, embed(M, p, X))
end

"""
    vector_transport_to!(M::AbstractManifold, Y, p, X, q)
    vector_transport_to!(M::AbstractManifold, Y, p, X, q, m::AbstractVectorTransportMethod)

Transport a vector `X` from the tangent space at a point `p` on the [`AbstractManifold`](@ref) `M`
to `q` using the [`AbstractVectorTransportMethod`](@ref) `m` and the [`AbstractRetractionMethod`](@ref) `r`.

The result is computed in `Y`.
See [`vector_transport_to`](@ref) for more details.
"""
function vector_transport_to!(
    M::AbstractManifold,
    Y,
    p,
    X,
    q,
    m::AbstractVectorTransportMethod = default_vector_transport_method(M),
)
    return _vector_transport_to!(M, Y, p, X, q, m)
end
function _vector_transport_to!(M::AbstractManifold, Y, p, X, q, m::VectorTransportTo)
    d = inverse_retract(M, p, q, m.inverse_retraction)
    return vector_transport_direction!(M, Y, p, X, d, m.vector_transport)
end
function _vector_transport_to!(M::AbstractManifold, Y, p, X, q, ::ParallelTransport)
    return parallel_transport_to!(M, Y, p, X, q)
end
function _vector_transport_to!(
    M::AbstractManifold,
    Y,
    p,
    X,
    q,
    m::DifferentiatedRetractionVectorTransport,
)
    return vector_transport_to_diff!(M, Y, p, X, q, m.retraction)
end

function _vector_transport_to!(M::AbstractManifold, Y, p, X, q, ::ProjectionTransport)
    return vector_transport_to_project!(M, Y, p, X, q)
end

function _vector_transport_to(M::AbstractManifold, p, X, q, m::PoleLadderTransport)
    Y = allocate_result(M, vector_transport_to, X, p)
    return _vector_transport_to!(M, Y, p, X, q, m)
end
function _vector_transport_to!(M::AbstractManifold, Y, p, X, q, m::PoleLadderTransport)
    inverse_retract!(
        M,
        Y,
        q,
        pole_ladder(
            M,
            p,
            retract(M, p, X, m.retraction),
            q;
            retraction = m.retraction,
            inverse_retraction = m.inverse_retraction,
        ),
        m.inverse_retraction,
    )
    copyto!(Y, -Y)
    return Y
end

function _vector_transport_to(M::AbstractManifold, p, X, c, m::ScaledVectorTransport)
    Y = allocate_result(M, vector_transport_to, X, p)
    return _vector_transport_to!(M, Y, p, X, c, m)
end
function _vector_transport_to!(M::AbstractManifold, Y, p, X, q, m::ScaledVectorTransport)
    vector_transport_to!(M, Y, p, X, q, m.method)
    Y .*= norm(M, p, X) / norm(M, q, Y)
    return Y
end
function _vector_transport_to(M::AbstractManifold, p, X, c, m::SchildsLadderTransport)
    Y = allocate_result(M, vector_transport_to, X, p)
    return _vector_transport_to!(M, Y, p, X, c, m)
end
function _vector_transport_to!(M::AbstractManifold, Y, p, X, q, m::SchildsLadderTransport)
    return inverse_retract!(
        M,
        Y,
        q,
        schilds_ladder(
            M,
            p,
            retract(M, p, X, m.retraction),
            q;
            retraction = m.retraction,
            inverse_retraction = m.inverse_retraction,
        ),
        m.inverse_retraction,
    )
end


####################################################################
# In this example we demonstrate using the Trimming solver.
#
# We test the l2 penalty (with trimming) here. As such, we use
# an example problem where the outlier noise is limited to a
# few specific sensors (this corresponds to mode=2 in the genDMD
# routine that creates the synthetic data)
#
# We also use a prox operator that forces the real part of the
# eigenvalues alpha to be less than or equal
# to one. It is generally advisable to put some bound
# in order to keep e^(Re(alpha)*tmax) from being too large,
# e.g. overflow may occur without such a choice

using RobustDMD

T = Float32

#--------------------------------------------------------------------
# Generate DMD Synthetic Data
#--------------------------------------------------------------------
println("generate synthetic data...")
# dimensions
m = 100;    # temporal dimension
n = 100;    # spatial dimension
h = 80;     # trimming level
k = 3;      # number of modes
# generate data
sigma = T(1e-4); # size of background noise
mu = T(1); # size of spikes
p = T(0.1); # frequency of spikes (as mode=2 this determines the percentage
# of columns that are corrupted)
X, Xclean, t, alphat, betat = genDMD(m, n, k, sigma, mu; seed=123456, p=p, mode=2);

#--------------------------------------------------------------------
# Initial Guess
#--------------------------------------------------------------------
println("initialize alpha and B...")
# create initial guess using trap. rule with exact dmd
alpha0, B0 = dmdexactestimate(m,n,k,X,t);

#--------------------------------------------------------------------
# Trim l2
#--------------------------------------------------------------------
println("Trim l2 loss experiment...")
# loss functions
lossf = (z) -> l2_func(z);
lossg = (z) -> l2_grad!(z);
l2Params = DMDParams(k, X, t, lossf, lossg);
l2DMD = DMDVariables(alpha0, B0, l2Params);
# wrapper: sets up inner solve specialized for l2
l2Svars = DMDVPSolverVariablesLSQ(l2Params);
# prox operator
function prox_mr(αr)
    k = length(αr) >> 1;
    for i = 1:k
        αr[i<<1-1] = min(T(1.0), αr[i<<1-1]);
    end
end

# apply solver
options = Trim_options(h,300,T(1e-5),T(1.2),true,true,10,prox=prox_mr);
Trim_solve_DMD!(l2DMD, l2Params, l2Svars, options);

#--------------------------------------------------------------------
# Compare Result
#--------------------------------------------------------------------
println("compare result...")
err0 = besterrperm(alpha0, alphat);
err1 = besterrperm(l2DMD.alpha, alphat);
println("err exact DMD ..... ", err0);
println("err w/ trimming ... ", err1);