"""
    InfExtendedReal{T} <: Real

The type `T` extended with positive and negative infinity.
"""
struct InfExtendedReal{T<:Real} <: Real
  flag :: InfFlag
  finitevalue :: T

  InfExtendedReal{T}(x::T) where {T<:Real} = new(FINITE, x)
  InfExtendedReal{T}(x::Infinite) where {T<:Real} = new(x==PosInf ? POSINF : NEGINF)

  # For internal testing use only
  function Base.reinterpret(::Type{InfExtendedReal{T}}, t::Tuple{InfFlag, T}) where T<:Real
      new{T}(t[1], t[2])
  end
end

# Since InfExtendedReal is a subtype of Real, and Infinite is also a subtype of real,
# we can just use `x.val` to get either the finite value, or the infinite value. This will
# make arithmetic much simpler.
function Base.getproperty(x::InfExtendedReal, s::Symbol)
    if s === :val
        return x.flag != FINITE ? (x.flag == NEGINF ? -∞ : ∞) : x.finitevalue
    else
        return getfield(x, s)
    end
end

InfExtendedReal{T}(x::Real) where {T<:Real} = InfExtendedReal{T}(isinf(x) ? convert(Infinite, x) : convert(T, x))
InfExtendedReal{T}(x::InfExtendedReal) where {T<:Real} = InfExtendedReal{T}(x.val)

"""
    InfExtendedReal(T)

The union of `T` and `Infinite`: either `T` if infinity can be represented as a `T`, or else `InfExtendedReal{T}`.
"""
@generated InfExtendedReal(::Type{T}) where {T<:Real} = hasinf(T) ? T : InfExtendedReal{T}

"""
    InfExtendedReal(x)

Converts `x` to a `InfExtendedReal(typeof(x))`.
"""
@generated InfExtendedReal(x::T) where {T<:Real} = hasinf(T) ? :x : :($(InfExtendedReal(T))(x))

Utils.isposinf(x::InfExtendedReal) = x.flag == POSINF || x.flag == FINITE && isposinf(x.finitevalue)
Utils.isneginf(x::InfExtendedReal) = x.flag == NEGINF || x.flag == FINITE && isneginf(x.finitevalue)


using Plots, Random, Measures

function path(rng, alpha, n=5000)
    x, y = 0.0, 0.0
    xDat, yDat = [], []
    for _ in 1:n
        flip = rand(rng,1:4)
        if flip == 1
            x += 1
        elseif flip == 2
            y += 1
        elseif flip == 3
            x -= (2+alpha)*rand(rng)
        elseif flip == 4
            y -= (2+alpha)*rand(rng)
        end
        push!(xDat,x)
        push!(yDat,y)
    end
    return xDat, yDat
end

alphaRange = [0.2, 0.21, 0.22]

default(xlabel = "x", ylabel = "y", xlims=(-150,50), ylims=(-250,50))
p1 = plot(path(MersenneTwister(27), alphaRange[1]), c=:blue)
p1 = plot!(path(MersenneTwister(27), alphaRange[2]), c=:red)
p1 = plot!(path(MersenneTwister(27), alphaRange[3]), c=:green)

rng = MersenneTwister(27)
p2 = plot(path(rng, alphaRange[1]), c=:blue)
p2 = plot!(path(rng, alphaRange[2]), c=:red)
p2 = plot!(path(rng, alphaRange[3]), c=:green)

plot(p1, p2, size=(800, 400), legend=:none, margin=5mm)


# ---
# title: 91. Decode Ways
# id: problem91
# author: Indig
# date: 2021-06-02
# difficulty: Medium
# categories: String, Dynamic Programming
# link: <https://leetcode.com/problems/decode-ways/description/>
# hidden: true
# ---
#
# A message containing letters from `A-Z` is being encoded to numbers using the
# following mapping:
#
#
#
#     'A' -> 1
#     'B' -> 2
#     ...
#     'Z' -> 26
#
#
# Given a **non-empty** string containing only digits, determine the total
# number of ways to decode it.
#
# The answer is guaranteed to fit in a **32-bit** integer.
#
#
#
# **Example 1:**
#
#
#
#     Input: s = "12"
#     Output: 2
#     Explanation: It could be decoded as "AB" (1 2) or "L" (12).
#
#
# **Example 2:**
#
#
#
#     Input: s = "226"
#     Output: 3
#     Explanation: It could be decoded as "BZ" (2 26), "VF" (22 6), or "BBF" (2 2 6).
#
#
# **Example 3:**
#
#
#
#     Input: s = "0"
#     Output: 0
#     Explanation: There is no character that is mapped to a number starting with '0'. We cannot ignore a zero when we face it while decoding. So, each '0' should be part of "10" --> 'J' or "20" --> 'T'.
#
#
# **Example 4:**
#
#
#
#     Input: s = "1"
#     Output: 1
#
#
#
#
# **Constraints:**
#
#   * `1 <= s.length <= 100`
#   * `s` contains only digits and may contain leading zero(s).
#
#
## @lc code=start
using LeetCode

function num_decodings(s)
    len = length(s)
    dp = fill(0, len + 1)
    dp[1] = 1
    for i in 1:len
        s[i] != '0' && (dp[i + 1] += dp[i])
        (i > 1 && s[i - 1] != '0' && parse(Int, s[i-1:i]) <= 26) && (dp[i + 1] += dp[i - 1])
    end
    dp[end]
end
## @lc code=end


"""
    abstract type Rotation{N,T} <: StaticMatrix{N,N,T}

An abstract type representing `N`-dimensional rotations. More abstractly, they represent
unitary (orthogonal) `N`×`N` matrices.
"""
abstract type Rotation{N,T} <: StaticMatrix{N,N,T} end

Base.@pure StaticArrays.Size(::Type{Rotation{N}}) where {N} = Size(N,N)
Base.@pure StaticArrays.Size(::Type{Rotation{N,T}}) where {N,T} = Size(N,N)
Base.@pure StaticArrays.Size(::Type{R}) where {R<:Rotation} = Size(supertype(R))
Base.adjoint(r::Rotation) = inv(r)
Base.transpose(r::Rotation{N,T}) where {N,T<:Real} = inv(r)
Base.real(R::Type{<:Rotation}) = R

# Generate zero-matrix with SMatrix
# Note that zeros(Rotation3,dims...) is not Array{<:Rotation} but Array{<:StaticMatrix{3,3}}
Base.zero(::Rotation{N,T}) where {N,T} = @SMatrix zeros(T,N,N)
Base.zero(::Type{Rotation}) = error("The dimension of rotation is not specified.")
Base.zero(::Type{<:Rotation{N}}) where N = @SMatrix zeros(N,N)
Base.zero(::Type{<:Rotation{N,T}}) where {N,T} = @SMatrix zeros(T,N,N)
Base.zeros(::Type{R}) where {R<:Rotation} = zeros(R, ()) # avoid StaticArray constructor
Base.zeros(::Type{R}, dims::Base.DimOrInd...) where {R<:Rotation} = zeros(typeof(zero(R)),dims...)
Base.zeros(::Type{R}, dims::NTuple{N, Integer}) where {R<:Rotation, N} = zeros(typeof(zero(R)),dims)
Base.zeros(::Type{R}, dims::Tuple{}) where {R<:Rotation} = zeros(typeof(zero(R)),dims) # avoid ambiguity

# Generate identity rotation matrix
Base.one(r::Rotation) = one(typeof(r))

# Rotation angles and axes can be obtained by converting to the AngleAxis type
rotation_angle(r::Rotation{3}) = rotation_angle(AngleAxis(r))
rotation_axis(r::Rotation{3}) = rotation_axis(AngleAxis(r))

# `convert` goes through the constructors, similar to e.g. `Number`
Base.convert(::Type{R}, rot::R) where {N,R<:Rotation{N}} = rot
Base.convert(::Type{R}, rot::Rotation{N}) where {N,R<:Rotation{N}} = R(rot)

# Rotation matrices should be orthoginal/unitary. Only the operations we define,
# like multiplication, will stay as Rotations, otherwise users will get an
# SMatrix{3,3} (e.g. rot1 + rot2 -> SMatrix)
Base.@pure StaticArrays.similar_type(::Union{R,Type{R}}) where {R <: Rotation} = SMatrix{size(R)..., eltype(R), prod(size(R))}
Base.@pure StaticArrays.similar_type(::Union{R,Type{R}}, ::Type{T}) where {R <: Rotation, T} = SMatrix{size(R)..., T, prod(size(R))}

@inline function Base.:/(r1::Rotation, r2::Rotation)
    r1 * inv(r2)
end

@inline function Base.:\(r1::Rotation, r2::Rotation)
    inv(r1) * r2
end

@inline function Base.:\(r::Rotation, v::AbstractVector)
    inv(r) * v
end

# This definition is for avoiding anbiguity
@inline function Base.:\(r::Rotation, v::StaticVector)
    inv(r) * v
end

################################################################################
################################################################################
"""
    struct RotMatrix{N,T} <: Rotation{N,T}

A statically-sized, N×N unitary (orthogonal) matrix.

Note: the orthonormality of the input matrix is *not* checked by the constructor.
"""
struct RotMatrix{N,T,L} <: Rotation{N,T} # which is <: AbstractMatrix{T}
    mat::SMatrix{N, N, T, L} # The final parameter to SMatrix is the "length" of the matrix, 3 × 3 = 9
    RotMatrix{N,T,L}(x::AbstractArray) where {N,T,L} = new{N,T,L}(convert(SMatrix{N,N,T,L}, x))
    # fixes #49 ambiguity introduced in StaticArrays 0.6.5
    RotMatrix{N,T,L}(x::StaticArray) where {N,T,L} = new{N,T,L}(convert(SMatrix{N,N,T,L}, x))
end
RotMatrix(x::SMatrix{N,N,T,L}) where {N,T,L} = RotMatrix{N,T,L}(x)

Base.zero(::Type{RotMatrix}) = error("The dimension of rotation is not specified.")

# These functions (plus size) are enough to satisfy the entire StaticArrays interface:
@inline function RotMatrix(t::Tuple)
    n = sqrt(length(t))
    if !isinteger(n)
        throw(DimensionMismatch("The length of input tuple $(t) must be square number."))
    end
    N = Int(n)
    RotMatrix(SMatrix{N,N}(t))
end
@inline (::Type{RotMatrix{N}})(t::Tuple) where N = RotMatrix(SArray{Tuple{N,N}}(t))
@inline RotMatrix{N,T}(t::Tuple) where {N,T} = RotMatrix(SArray{Tuple{N,N},T}(t))
@inline RotMatrix{N,T,L}(t::Tuple) where {N,T,L} = RotMatrix(SArray{Tuple{N,N},T}(t))

# Create aliases RotMatrix2{T} = RotMatrix{2,T,4} and RotMatrix3{T} = RotMatrix{3,T,9}
for N in 2:3
    L = N*N
    RotMatrixN = Symbol(:RotMatrix, N)
    @eval begin
        const $RotMatrixN{T} = RotMatrix{$N, T, $L}
        @inline $RotMatrixN(t::Tuple) = RotMatrix(SArray{Tuple{$N,$N}}(t))
    end
end

Base.@propagate_inbounds Base.getindex(r::RotMatrix, i::Int) = r.mat[i]
@inline Base.Tuple(r::RotMatrix) = Tuple(r.mat)

@inline RotMatrix(θ::Number) = RotMatrix{2}(θ)
@inline function (::Type{<:RotMatrix{2}})(θ::Number)
    s, c = sincos(θ)
    RotMatrix(@SMatrix [c -s; s c])
end
@inline function RotMatrix{2,T}(θ::Number) where T
    s, c = sincos(θ)
    RotMatrix(@SMatrix T[c -s; s c])
end

Base.one(::Type{R}) where {N,R<:RotMatrix{N}} = R(I)

# A rotation is more-or-less defined as being an orthogonal (or unitary) matrix
Base.inv(r::RotMatrix) = RotMatrix(r.mat')

# By default, composition of rotations will go through RotMatrix, unless overridden
@inline Base.:*(r1::Rotation, r2::Rotation) = RotMatrix(r1) * RotMatrix(r2)
@inline Base.:*(r1::RotMatrix, r2::Rotation) = r1 * RotMatrix(r2)
@inline Base.:*(r1::Rotation, r2::RotMatrix) = RotMatrix(r1) * r2
@inline Base.:*(r1::RotMatrix, r2::RotMatrix) = RotMatrix(r1.mat * r2.mat) # TODO check that this doesn't involve extra copying.

# Special case multiplication of 3×3 rotation matrices: speedup using cross product
@inline function Base.:*(r1::RotMatrix{3}, r2::RotMatrix{3})
    ret12 = r1 * r2[:, SVector(1, 2)]
    ret3 = ret12[:, 1] × ret12[:, 2]
    RotMatrix([ret12 ret3])
end

"""
    struct Angle2d{T} <: Rotation{2,T}
        theta::T
    end

A 2×2 rotation matrix parameterized by a 2D rotation by angle.
Only the angle is stored inside the `Angle2d` type, values
of `getindex` etc. are computed on the fly.
"""
struct Angle2d{T} <: Rotation{2,T}
    theta::T
    Angle2d{T}(theta::Number) where T = new{T}(theta)
end

@inline function Angle2d(theta::Number)
    Angle2d{rot_eltype(typeof(theta))}(theta)
end

params(r::Angle2d) = SVector{1}(r.theta)

Angle2d(r::Rotation{2}) = Angle2d(rotation_angle(r))
Angle2d{T}(r::Rotation{2}) where {T} = Angle2d{T}(rotation_angle(r))
@inline (::Type{R})(t::Tuple) where R<:Angle2d = convert(R, RotMatrix2(t))

Base.one(::Type{A}) where {A<: Angle2d} = A(0)

rotation_angle(rot::Angle2d) = rot.theta
function rotation_angle(rot::Rotation{2})
    c = @inbounds rot[1,1]
    s = @inbounds rot[2,1]
    atan(s, c)
end

@inline function Base.:*(r::Angle2d, v::StaticVector)
    if length(v) != 2
        throw(DimensionMismatch("Cannot rotate a vector of length $(length(v))"))
    end
    x,y = v
    s,c = sincos(r.theta)
    T = Base.promote_op(*, typeof(s), eltype(v))
    similar_type(v,T)(c*x - s*y, s*x + c*y)
end

Base.:*(r1::Angle2d, r2::Angle2d) = Angle2d(r1.theta + r2.theta)
Base.:^(r::Angle2d, t::Real) = Angle2d(r.theta*t)
Base.:^(r::Angle2d, t::Integer) = Angle2d(r.theta*t)
Base.inv(r::Angle2d) = Angle2d(-r.theta)

@inline function Base.getindex(r::Angle2d, i::Int)
    if i == 1
        cos(r.theta)
    elseif i == 2
        sin(r.theta)
    elseif i == 3
        -sin(r.theta)
    elseif i == 4
        cos(r.theta)
    else
        throw(BoundsError(r,i))
    end
end

################################################################################
################################################################################

_isrotation_eps(T::Type{<:Integer}) = zero(T)
_isrotation_eps(T) = eps(T)

"""
    isrotation(r)
    isrotation(r, tol)

Check whether `r` is a rotation matrix, where `r * r'` is within
`tol` of the identity matrix (using the Frobenius norm). `tol` defaults to
`1000 * eps(float(eltype(r)))` or `zero(T)` for integer `T`.
"""
isrotation

function isrotation(r::StaticMatrix{N,N,T}, tol::Real = 1000 * _isrotation_eps(eltype(T))) where {N,T<:Real}
    m = SMatrix(r)
    d = norm(m*m'-one(SMatrix{N,N}))
    return d ≤ tol && det(m) > 0
end

function isrotation(r::AbstractMatrix{T}, tol::Real = 1000 * _isrotation_eps(eltype(T))) where T<:Real
    if !=(size(r)...)
        return false
    end
    d = norm(r*r'-one(r))
    return d ≤ tol && det(r) > 0
end

function isrotation(r::AbstractMatrix{T}, tol::Real = 1000 * _isrotation_eps(eltype(real(T)))) where T<:Number
    if !isreal(r)
        return false
    end
    return isrotation(real(r), tol)
end

"""
    nearest_rotation(M) -> RotMatrix

Get the nearest special orthonormal matrix from given matrix `M`.
See [Wahba's problem](https://en.wikipedia.org/wiki/Wahba%27s_problem) for more information.
"""
function nearest_rotation(M::StaticMatrix{N,N}) where N
    u, _, v = svd(M)
    s = sign(det(u * v'))
    d = @SVector ones(N-1)
    R = u * Diagonal(push(d,s)) * v'
    return RotMatrix{N}(R)
end

function nearest_rotation(M::AbstractMatrix{T}) where T
    N = size(M,1)
    L = N^2
    M_ = convert(SMatrix{N,N,T,L}, M)
    return nearest_rotation(M_)
end

# A simplification and specialization of the Base.show function for AbstractArray makes
# everything sensible at the REPL.
function Base.show(io::IO, ::MIME"text/plain", X::Rotation)
    if !haskey(io, :compact)
        io = IOContext(io, :compact => true)
    end
    summary(io, X)
    if !isa(X, RotMatrix)
        n_fields = length(fieldnames(typeof(X)))
        print(io, "(")
        for i in 1:n_fields
            print(io, getfield(X, i))
            if i < n_fields
                print(io, ", ")
            end
        end
        print(io, ")")
    end
    print(io, ":")
    println(io)
    io = IOContext(io, :typeinfo => eltype(X))
    Base.print_array(io, X)
end


"""
    find_best_item(examinee, items)

# Description
Finds the best item for the `examinee` among the vector `items` using maximum information criterion.

# Arguments
  - **`examinee::AbstractExaminee`**: The examinee.
  - **`items:::Vector{<:AbstractItem}`**: Set of items in which searching for the best.
  - **`method`**: Optional. "max-info" | "D-gain".
  max-info takes the item with the maximum latent information,
  D-gain takes the item with the maximum gain in the determinant of the expected information matrix if examinee would answer to the item (D-VC in __Ren, van der Linden, Diao, 2017__).

# Output
It returns an item of generic type.

# References
[^RenDiao2017]

[^RenDiao2017]:Ren H, van der Linden WJ, Diao Q. Continuous online item calibration: Parameter recovery and item calibration. Psychometrika. 2017;82:498–522. doi: 10.1007/s11336-017-9553-1.
"""
function find_best_item(
    examinee::AbstractExaminee,
    items::Vector{<:AbstractItem};
    method = "max-info",
)
    if method in ["max-info", "D-gain"]
        if method == "max-info"
            infos = latent_information(examinee, items)
        elseif method == "D-gain"
            infos = map(i -> D_gain_method(i, examinee), items)
        end
        return items[findmax(infos)[2]].idx::Int64
    else
        error("only max-info and D-gain optimality are available.")
    end
end

"""
    find_best_examinee(item, examinees; method = "D")

# Description
Finds the best examinee among the `examinees` vector, for the `item` using maximum expected information criterion.

# Arguments
- **`item::AbstractItem`**: Required. Set of items in which searching for the best.
- **`examinees::Vector{<:AbstractExaminee}`**: Required. The examinee.
- **`method`**: Optional. "D" | "A".
  D stands for D-optimality (determinant of the expected information matrix),
  A stands for A-optimality (trace of the expected information matrix).

# Output
It returns the idx of the best examinee.

"""
function find_best_examinee(
    item::AbstractItem,
    examinees::Vector{<:AbstractExaminee};
    method = "D",
)
    if method in ["D", "A"]
        if method == "D"
            infos = map(e -> D_method(item, e), examinees)
        elseif method == "A"
            infos = map(e -> A_method(item, e), examinees)
        end
        return examinees[findmax(infos)[2]].idx::Int64
    else
        error("only D-optimalit and A-optimality are available.")
    end
end


using Test
using GeometryTypes
using SurfaceTopology

@info "Topology function tests"

faces =   Face{3,Int}[
    [6,   8,  11],
    [8,   6,   7],
    [5,   1,   4],
    [1,   5,   7],
    [5,   8,   7],
    [5,  10,  11],
    [8,   5,  11],
    [1,   3,   2],
    [3,   1,   7],
    [3,   6,   2],
    [6,   3,   7],
    [9,   5,   4],
    [5,  12,  10],
    [9,  12,   5],
    [10,  12,   4],
    [12,   9,   4]
]

triangles = []
for i in FaceRing(5,faces)
    push!(triangles,i)
end

@test sort(triangles)==[3,4,5,6,7,12,13,14]

triverticies = []
for i in EdgeRing(5,faces)
    #println("i")
    push!(triverticies,i)
end

@test (1,4) in triverticies

verticies = []
for i in VertexRing(5,faces)
    push!(verticies,i)
end

@test sort(verticies)==[1,4,7,8,9,10,11,12] #[1,2,6,7]

@info "Testing FaceDS"

# points = zero(faces)
fb = FaceDS(faces)

triangles = []
for i in FaceRing(5,fb)
    push!(triangles,i)
end
@test sort(triangles)==[3,4,5,6,7,12,13,14]

triverticies = []
for i in EdgeRing(5,fb)
    push!(triverticies,i)
end

@test Face(1,4) in triverticies

verticies = []
for i in VertexRing(5,fb)
     push!(verticies,i)
end

@test sort(verticies)==[1,4,7,8,9,10,11,12]

@info "Testing EdgeDS"

eb = EdgeDS(faces)

verticies = []
for i in VertexRing(5,eb)
     push!(verticies,i)
end

@test sort(verticies)==[1,4,7,8,9,10,11,12]

triverticies = []
for i in EdgeRing(5,eb)
    push!(triverticies,i)
end

@test Face(1,4) in triverticies

@info "Topology tests for Cached DS"

# At the moment limited to a closed surfaces

faces = Face{3,Int64}[
    [1, 12, 6], [1, 6, 2], [1, 2, 8], [1, 8, 11], [1, 11, 12], [2, 6, 10], [6, 12, 5],
    [12, 11, 3], [11, 8, 7], [8, 2, 9], [4, 10, 5], [4, 5, 3], [4, 3, 7], [4, 7, 9],
    [4, 9, 10], [5, 10, 6], [3, 5, 12], [7, 3, 11], [9, 7, 8], [10, 9, 2]
]

cds = CachedDS(faces)

@test sort(collect(VertexRing(3,faces)))==sort(collect(VertexRing(3,cds)))

@test sort(collect(EdgeRing(3,faces)))==sort(collect(EdgeRing(3,cds)))


using MomentTensors, Test

@testset "Indexing" begin
    let v = [11, 22, 33, 12, 13, 23], m = MT(v),
            I = [1 4 5; 4 2 6; 5 6 3], a = Array(m)
        for i in 1:6
            @test m[i] == v[i]
        end
        for i in 1:3, j in i:3
            # Symmetry
            @test m[i,j] == m[j,i]
            @test m[i,j] == a[i,j]
            @test m[i,j] == a[j,i]
            # Correct value
            @test m[i,j] == 10i + j
            @test a[i,j] == 10i + j
            # Correct vector index
            @test m[i,j] == m[I[i,j]]
            @test m[i,j] == m.m[I[i,j]]
        end
        for (i, s1) in enumerate((:r, :t, :p)), (j, s2) in enumerate((:r, :t, :p))
            @test m[i,j] == m[Symbol(s1, s2)]
        end
        for (i, s1) in enumerate((:r, :θ, :ϕ)), (j, s2) in enumerate((:r, :θ, :ϕ))
            @test m[i,j] == m[Symbol(s1, s2)]
        end
        @test size(m) == (3, 3)
        @test_throws ArgumentError m[:xy]
        @test_throws BoundsError m[7]
        @test_throws BoundsError m[1,4]
    end
end


#This file contains a list of the currently available mean functions

abstract type Mean end

# Calculate mean for matrix of observations
function mean(m::Mean, X::MatF64)
    nobs = size(X, 2)
    μ = Array{Float64}(undef, nobs)
    @inbounds for i in 1:nobs
        μ[i] = mean(m, view(X, :, i))
    end
    μ
end

# Calculates the stack [dm / dθᵢ] of mean matrix gradients
function grad_stack(m::Mean, X::MatF64)
    nobs = size(X, 2)
    mat = Array{Float64}(undef, nobs, num_params(m))
    @inbounds for i in 1:nobs
        mat[i, :] = grad_mean(m, view(X, :, i))
    end
    mat
end

#————————————————————————————————————————————

include("mZero.jl")          # Zero mean function
include("mConst.jl")         # Constant mean function
include("mLin.jl")           # Linear mean function
include("mPoly.jl")          # Polynomial mean function
include("composite_mean.jl") # Composite means


module Kruskal

using LinearAlgebra
using Statistics
using Optim

export kruskalmds

# naming similar to [Kruskal, Nonmetric Multidimensional Scaling: A Numerical Method]
# X is a vector with values [x₁,y₁,...,x₂,y₂,...,xₙ,yₙ,...], i.e. a vectorized pxN matrix

# gradient-free S, for debugging purposes
# function stressreference{T}(X::AbstractVecOrMat{T},p,D)
#     N = div(length(X),p)

#     num = den = zero(T)

#     # for i>j
#     ind = 1 # index in vectorization of upper triangular part
#     for j=2:N
#         for i=1:j-1
#             i2 = p*(i-1) # 0-based index of xᵢ in X
#             j2 = p*(j-1) # 0-based index of xⱼ in X

#             t = zero(T)
#             for dim=1:p
#                 t += (X[i2+dim]-X[j2+dim])^2
#             end
#             num += (sqrt(t)-D[ind])^2
#             den += t

#             ind += 1
#         end
#     end
#     sqrt(num/den)
# end

mutable struct SData{T}
    lastX::Vector{T} # Vectorization of pxN data point matrix. Needed to figure out whether we moved or not since last call to _update
    p::Int # Number of dimensions in low-dimensional representation.
    N::Int # Number of data points
    targetD::Vector{T} # Vectorization of upper triangular part of target distance matrix.
    num::T
    den::T
    S::T
    numGrad::Vector{T} # gradient of numerator
    denGrad::Vector{T} # gradient of denominator
end
SData(p,N,targetD::Vector{T}) where {T} = SData(NaN*zeros(T,p*N),p,N,targetD,zero(T),zero(T),zero(T),Vector{T}(undef,p*N),Vector{T}(undef,p*N))

function update!(sData::SData{T},X) where {T}
    sData.lastX == X && return # nothing to do

    targetD = sData.targetD
    N = sData.N # number of data points
    p = sData.p
    numGrad = sData.numGrad
    denGrad = sData.denGrad
    num = den = zero(T)
    numGrad[:] .= 0
    denGrad[:] .= 0

    # for i<j, point indices
    ind = 1 # index in vectorization of upper triangular part
    for j=2:N
        for i=1:j-1
            i2 = p*(i-1) # 0-based index of xᵢ in X
            j2 = p*(j-1) # 0-based index of xⱼ in X

            t = zero(T)
            for dim=1:p
                t += (X[i2+dim]-X[j2+dim])^2
            end
            st = sqrt(t)
            num += (st-targetD[ind])^2
            den += t

            u = 2*(st-targetD[ind])/st # common computations for all dimensions in numGrad
            for dim=1:p
                v = (X[i2+dim]-X[j2+dim])
                #w = u*v
                w = abs(v)<1e-12 ? 0.0 : u*v # handle edge case by letting 0*Inf := 0
                numGrad[i2+dim] += w
                numGrad[j2+dim] -= w
                denGrad[i2+dim] += 2v
                denGrad[j2+dim] -= 2v
            end

            ind += 1
        end
    end

    sData.num = num
    sData.den = den
    sData.S = sqrt(num/den)
    nothing
end

# gradient-aware S (i.e. sharing data with gradient computation)
function stress(X,sData::SData)
    update!(sData,X)
    sData.S
end

function stressgradient(X,storage,sData::SData)
    update!(sData,X)

    targetD = sData.targetD
    num = sData.num
    den = sData.den
    S = sData.S
    numGrad = sData.numGrad
    denGrad = sData.denGrad

    C = 1.0/(2*S) # constant factor
    for i=1:length(X)
        storage[i] = C*(numGrad[i]*den - num*denGrad[i]) / den^2
    end

    nothing
end

function updatelocation(D::AbstractVector{T}, X0::AbstractMatrix{T}, tol) where {T}
    p = size(X0,1)
    N = size(X0,2)
    X0 = X0[:]

    # # Simple, gradient-free version
    # res = optimize(x->stressreference(x,p,D), X0, BFGS())
    # @assert Optim.converged(res)
    # X = reshape(Optim.minimizer(res),p,N)

    sData = SData(p,N,D) # init storage for reused computations
    # res = optimize(x->stress(x,sData), (storage,x)->stressgradient(x,storage,sData), X0, BFGS())
    res = optimize(x->stress(x,sData), (storage,x)->stressgradient(x,storage,sData), X0, BFGS(), Optim.Options(x_tol=tol, f_tol=tol))
    #res = optimize(x->stress(x,sData), X0, BFGS()) # doesn't use gradient, for comparison
    Optim.converged(res) || error("Kruskal MDS: Location optimization did not coverge.")
    X = reshape(Optim.minimizer(res),p,N)

    # Since the optimum is preserved when X is uniformly scaled, normalize scaling to keep numerical stability
    X = X .- mean(X;dims=2) # center X (doesn't change stress)
    X = X / sqrt(mean(sum(X.^2;dims=1);dims=2)[1]) # scale so that RMS distance to origin is equal to 1

    X, Optim.minimum(res)
end

# convert distance matrix to pdist-style vector
function distancevector(D::AbstractMatrix)
    # vectorize upper triangular part of D
    N = size(D,1)
    dist = zeros(div(N*(N-1),2))
    ind=1
    for j=2:N
        for i=1:j-1
            dist[ind] = D[i,j]
            ind += 1
        end
    end
    dist
end

# convert pdist-style distance vector to matrix
function distancematrix!(D::AbstractMatrix, dist::AbstractVector)
    N = size(D,1)

    for i=1:N
        D[i,i] = 0.0
    end

    ind=1
    for j=2:N
        for i=1:j-1
            D[i,j] = D[j,i] = dist[ind]
            ind += 1
        end
    end
    D
end
distancematrix(dist::AbstractVector{T}) where {T} = (M=length(dist); N=round(Int,1/2+sqrt(1/4+2M)); distancematrix!(Matrix{T}(undef,N,N),dist))

function isotonicregression(Y::AbstractVector{T}, tieGroups::Vector{UnitRange}=[]) where {T}
    X = Tuple{T,Int}[] # tuple with values and integer weight

    # reorder values in each tieGroup to put them in increasing order
    isempty(tieGroups) || (Y=copy(Y)) # avoid modifying the original vector
    tiePerms = map!(rng->sortperm(Y[rng]), Vector{Vector{Int}}(undef,length(tieGroups)), tieGroups)
    for (g,p) in zip(tieGroups,tiePerms)
        Y[g] = Y[g[p]]
    end

    for y in Y
        yw = 1

        while !isempty(X) && y<=X[end][1]
            # merge with previous point
            x,xw = pop!(X)
            y,yw = (x*xw+y*yw)/(xw+yw), xw+yw
        end
        push!(X, (y,yw)) # store point
    end

    # unwrap to full vector by duplicating elements by their weight
    Z = Vector{T}(undef,length(Y))
    zi = 1
    for x in X
        Z[zi:zi+x[2]-1] .= x[1]
        zi += x[2]
    end

    # inverse tieGroup reordering
    for (g,p) in zip(tieGroups,tiePerms)
        Z[g[p]] = Z[g]
    end

    Z
end

function pdist(X)
    # create pdist-style distance vector
    p = size(X,1)
    N = size(X,2)
    dist = similar(X, div(N*(N-1),2))
    ind=1
    for j=2:N
        for i=1:j-1
            dist[ind] = sqrt(sum((X[:,i]-X[:,j]).^2)) # D[i,j]
            ind += 1
        end
    end
    dist
end

function duplicateranges(v)
    @assert issorted(v)
    count = 0
    out = UnitRange[]
    for i=1:length(v)
        if i<length(v) && v[i]==v[i+1]
            count+=1
        else
            count>0 && push!(out, i-count:i)
            count = 0
        end
    end
    out
end

function kruskalmds(D::AbstractMatrix{T}, p::Integer, X0=convert(Matrix{T},randn(size(D,1),p)); maxIter=1000, tol=1e-6) where {T<:AbstractFloat}
    @assert p>0
    @assert D≈D' "Distance matrix must be symmetric"
    N = size(D,1)
    @assert size(X0)==(N,p)
    X0 = X0' # work with transpose internally.

    dist = distancevector(D)
    perm = sortperm(dist)
    iperm = invperm(perm)
    tieGroups = duplicateranges(dist[perm])

    X = X0 .- mean(X0;dims=2) # center X (doesn't change stress)
    X = X / sqrt(mean(sum(X.^2;dims=1);dims=2)[1]) # scale so that RMS distance to origin is equal to 1 (doesn't change stress, if the monotone function is updated accordingly)

    S = Inf
    converged = false

    # println("i: 0, S=", Sreference(X0,p,dist))

    for i=1:maxIter # nbr iterations
        prevS = S
        modelDist = pdist(X)[perm] # compute distance and permute
        permDist = isotonicregression(modelDist, tieGroups) # nonnegativity constraint will be enforced automatically since all values in modelDist are nonnegative
        dist = permDist[iperm]

        XOld = X
        X,S = updatelocation(dist,X,tol)
        stepSize = norm(X-XOld)

        # println("i: ", i, ", S=", S, ", stepSize=", stepSize, ", prevS-S=", prevS-S, ", tol*S=", tol*S)
        stepSize<tol && (converged=true; break)
        prevS-S<tol*S && (converged=true; break)
        prevS = S
    end

    # post-processing
    # the stress is invariant under rotation, so we can normalize rotation by principal axes
    F = svd(X)
    X = F.V*Diagonal(F.S)

    X, S, converged, distancematrix(dist)
end
kruskalmds(D::AbstractMatrix, p::Integer, X0=randn(size(D,1),p); kwargs...) = kruskalmds(convert(Matrix{Float64},D),p,X0;kwargs...)

end


function unpack(states_history::Vector{<:State};
    dims::Vector{Int}=Vector{Int}())
    # set default to condense all dimensions
    if length(dims) == 0
        dims = collect(1:length(states_history[1].x))
    end
    # set output sizes
    μ = zeros(typeof(states_history[1].x[1]),
        length(states_history), length(dims))
    Σ = zeros(typeof(states_history[1].x[1]),
        length(states_history), length(dims), length(dims))
    # iterate over belief_history and place elements appropriately
    for (i, state) in enumerate(states_history)
        μ[i,:] = state.x[dims]
        Σ[i,:,:] = state.P[dims,dims]
    end
    return μ, Σ
end


"""
$(SIGNATURES)

Random Interventional Distribution of `x` given `Θ`

```jldoctest
μ = uniform([-100.0, 100.0])
x = normal(μ, 1.0)
x_ = rid(x, μ)
mean(x_)
```

Given two random variables ``X`` and ``Θ``, the random interventional distribution
of ``X`` given ``Θ`` is a a random distribution (`RandVar` with `elemtype` `RandVar`).
In particular, each realization of ``rid(X, Θ)`` is the random variable ``X`` subject
to the intervention ``Θ = Θ`` where ``Θ ∼ Θ`` is a realization of ``Θ``:

The random interventional distribution of a random variable `X` with elem type `tau_1`
given ``Θ: Ω → τ_2`` is a random variable
``x ∥ do(θ): Ω → (Ω →  τ_1)``, defined as:

``
x ∥ do(θ) = ω → x | do(θ = θ(ω))
``

"""
rid(x::RandVar, θ::RandVar) = ciid(ω -> replace(x, θ => θ(ω)))
rid(x::RandVar, θs::RandVar...) = ciid(ω -> replace(x, (θi => θi(ω) for θi in θs)...))


function othersum(si::Vector{Int64},s::Vector{Float64},m::Int64)
  # OTHERSUM Compute the sum of each column of the matrix, without each
  # individual entry.  This corresponds to a matrix where each entry is the
  # sum of each column but with each entry subtracted.
  # rowsum = accumarray(si,s,[m,1])
  rowsum = zeros(Float64,m)
  for i = 1:length(si)
    rowsum[si[i]] += s[i]
  end
  os = rowsum[si] - s
  return os
end


using DecimalNumbers, Base.Test

@testset "from int" begin
    d = DecimalNumbers.Decimal(1)
    @test d.value == 1
    @test d.exp == 0
    # denormalize
    d = DecimalNumbers.Decimal(10)
    @test d.value == 1
    @test d.exp == 1

    d = DecimalNumbers.Decimal(23487300)
    @test d.value == 234873
    @test d.exp == 2

    # specific integer type
    d = DecimalNumbers.Decimal(Int8(40))
    @test typeof(d.value) == Int8
    @test d.value === Int8(4)
    @test d.exp === Int8(1)
end

@testset "to int" begin
    d = DecimalNumbers.Decimal(1)
    @test Int(d) === 1

    d = DecimalNumbers.Decimal(23487300)
    @test Int(d) === 23487300

    @test_throws InexactError Int(DecimalNumbers.Decimal(3.14))
end

@testset "from float" begin
    d = DecimalNumbers.Decimal(3.0)
    @test d.value == 3
    @test d.exp == 0

    d = DecimalNumbers.Decimal(3.14)
    @test d.value == 314
    @test d.exp == -2
    @test typeof(d.value) == Int64

    d = DecimalNumbers.Decimal(Float16(-3.14))
    @test d.value === Int16(-314)
    @test d.exp === Int16(-2)
end

@testset "comparisons" begin
    @test DecimalNumbers.Decimal(1) == DecimalNumbers.Decimal(1)
    @test DecimalNumbers.Decimal(1) != DecimalNumbers.Decimal(10)
    @test DecimalNumbers.Decimal(1, -1) != DecimalNumbers.Decimal(10)
    @test DecimalNumbers.Decimal(1, -1) == DecimalNumbers.Decimal(1, -1)
    @test DecimalNumbers.Decimal(1, -2) < DecimalNumbers.Decimal(1, -1)
    @test DecimalNumbers.Decimal(1) < DecimalNumbers.Decimal(2)
    # denormalized
    a = DecimalNumbers.Decimal(10, 1)
    b = DecimalNumbers.Decimal(100, 0)
    @test a == b
    @test !(a < b)
    @test !(b < a)
end

@testset "parse" begin
    @test parse(DecimalNumbers.Decimal{Int64}, "-101") == DecimalNumbers.Decimal(-101)
    @test parse(DecimalNumbers.Decimal{Int64}, "101") == DecimalNumbers.Decimal(101)
    @test parse(DecimalNumbers.Decimal{Int64}, "101.") == DecimalNumbers.Decimal(101)
    @test parse(DecimalNumbers.Decimal{Int64}, "101.0") == DecimalNumbers.Decimal(101)
    @test parse(DecimalNumbers.Decimal{Int64}, "1.01") == DecimalNumbers.Decimal(101, -2)
    @test parse(DecimalNumbers.Decimal{Int64}, ".101") == DecimalNumbers.Decimal(101, -3)
    @test parse(DecimalNumbers.Decimal{Int64}, "0.101") == DecimalNumbers.Decimal(101, -3)
    @test parse(DecimalNumbers.Decimal{Int64}, "0.0101") == DecimalNumbers.Decimal(101, -4)
end

@testset "show" begin
    @test string( DecimalNumbers.Decimal(101, 3)) == "dec\"101000.0\""
    @test string( DecimalNumbers.Decimal(101, 2)) == "dec\"10100.0\""
    @test string( DecimalNumbers.Decimal(101, 1)) == "dec\"1010.0\""
    @test string( DecimalNumbers.Decimal(-101)) == "dec\"-101.0\""
    @test string( DecimalNumbers.Decimal(101)) == "dec\"101.0\""
    @test string( DecimalNumbers.Decimal(101, 0)) == "dec\"101.0\""
    @test string( DecimalNumbers.Decimal(101, -2)) == "dec\"1.01\""
    @test string( DecimalNumbers.Decimal(101, -3)) == "dec\"0.101\""
    @test string( DecimalNumbers.Decimal(101, -4)) == "dec\"0.0101\""
end

@testset "math" begin
    local T, xD, x, i
    try
        for T in (Int8, Int16, Int32, Int64, Int128)
            xD = DecimalNumbers.Decimal(T(0))
            x = T(0)
            for i in rand(T, 10)
                @test abs(DecimalNumbers.Decimal(i)) == abs(i)
                @test DecimalNumbers.Decimal(i) + xD == i + x
                @test DecimalNumbers.Decimal(i) - xD == i - x
                @test DecimalNumbers.Decimal(widen(i)) * DecimalNumbers.Decimal(widen(i)) == widen(i) * widen(i)
                xD += DecimalNumbers.Decimal(i)
                x += i
            end
        end
    catch e
        @show T, xD, x, i
        rethrow(e)
    end

    @test DecimalNumbers.Decimal(1) + DecimalNumbers.Decimal(1) == DecimalNumbers.Decimal(2)
    @test DecimalNumbers.Decimal(10) + DecimalNumbers.Decimal(1) == DecimalNumbers.Decimal(11)
    @test DecimalNumbers.Decimal(10, 2) + DecimalNumbers.Decimal(1, -1) == DecimalNumbers.Decimal(10001, -1)
    @test DecimalNumbers.Decimal(1, -1) + DecimalNumbers.Decimal(2, -1) == DecimalNumbers.Decimal(3, -1)
    @test DecimalNumbers.Decimal(1) - DecimalNumbers.Decimal(1) == DecimalNumbers.Decimal(0)
    @test DecimalNumbers.Decimal(1) * DecimalNumbers.Decimal(1) == DecimalNumbers.Decimal(1)
    @test DecimalNumbers.Decimal(10) * DecimalNumbers.Decimal(1, -1) == DecimalNumbers.Decimal(1)
    @test DecimalNumbers.Decimal(1) / DecimalNumbers.Decimal(1) == DecimalNumbers.Decimal(1)
    @test DecimalNumbers.Decimal(10) / DecimalNumbers.Decimal(1) == DecimalNumbers.Decimal(10)
    @test DecimalNumbers.Decimal(-10) / DecimalNumbers.Decimal(1) == DecimalNumbers.Decimal(-10)
    @test DecimalNumbers.Decimal(1) / DecimalNumbers.Decimal(1.5) == DecimalNumbers.Decimal(66666666666666667, -17)
    @test DecimalNumbers.Decimal(1) / DecimalNumbers.Decimal(0.5) == DecimalNumbers.Decimal(2)

    # division
    # Int8
    @test DecimalNumbers.Decimal(Int8(22)) / DecimalNumbers.Decimal(Int8(7)) == DecimalNumbers.Decimal(31, -1)
    # Int16
    @test DecimalNumbers.Decimal(Int16(22)) / DecimalNumbers.Decimal(Int16(7)) == DecimalNumbers.Decimal(3143, -3)
    # Int32
    @test DecimalNumbers.Decimal(Int32(22)) / DecimalNumbers.Decimal(Int32(7)) == DecimalNumbers.Decimal(314285714, -8)
    # Int64
    @test DecimalNumbers.Decimal(Int64(22)) / DecimalNumbers.Decimal(Int64(7)) == DecimalNumbers.Decimal(314285714285714286, -17)
    # Int128
    @test DecimalNumbers.Decimal(Int128(22)) / DecimalNumbers.Decimal(Int128(7)) == DecimalNumbers.Decimal(31428571428571428571428571428571428571, -37)
end


# ---
# title: 303. Range Sum Query - Immutable
# id: problem303
# author: Qling
# date: 2021-03-01
# difficulty: Easy
# categories: Dynamic Programming
# link: <https://leetcode.com/problems/range-sum-query-immutable/description/>
# hidden: true
# ---
#
# Given an integer array `nums`, find the sum of the elements between indices
# `i` and `j` `(i ≤ j)`, inclusive.
#
# Implement the `NumArray` class:
#
#   * `NumArray(int[] nums)` Initializes the object with the integer array `nums`.
#   * `int sumRange(int i, int j)` Return the sum of the elements of the `nums` array in the range `[i, j]` inclusive (i.e., `sum(nums[i], nums[i + 1], ... , nums[j])`)
#
#
#
# **Example 1:**
#
#
#
#     **Input**
#     ["NumArray", "sumRange", "sumRange", "sumRange"]
#     [[[-2, 0, 3, -5, 2, -1]], [0, 2], [2, 5], [0, 5]]
#     **Output**
#     [null, 1, -1, -3]
#
#     **Explanation**
#     NumArray numArray = new NumArray([-2, 0, 3, -5, 2, -1]);
#     numArray.sumRange(0, 2); // return 1 ((-2) + 0 + 3)
#     numArray.sumRange(2, 5); // return -1 (3 + (-5) + 2 + (-1))
#     numArray.sumRange(0, 5); // return -3 ((-2) + 0 + 3 + (-5) + 2 + (-1))
#
#
#
#
# **Constraints:**
#
#   * `0 <= nums.length <= 104`
#   * `-105 <= nums[i] <= 105`
#   * `0 <= i <= j < nums.length`
#   * At most `104` calls will be made to `sumRange`.
#
#
## @lc code=start
using LeetCode

struct NumArray
    sums::Vector
    function NumArray(nums::Vector)
        n = length(nums)
        sums = nums[:]

        for i = 2:n
            sums[i] = sums[i-1] + nums[i]
        end
        new(sums)
    end
end

function sum_range(nums::NumArray, i::Int, j::Int)
    i == j && return 0
    i == 1 && return nums.sums[j]
    return nums.sums[j] - nums.sums[i-1]
end
## @lc code=end


# ---
# title: 1027. Longest Arithmetic Subsequence
# id: problem1027
# author: Indigo
# date: 2022-04-14
# difficulty: Medium
# categories: Dynamic Programming
# link: <https://leetcode.com/problems/longest-arithmetic-subsequence/description/>
# hidden: true
# ---
#
# Given an array `A` of integers, return the **length** of the longest
# arithmetic subsequence in `A`.
#
# Recall that a _subsequence_ of `A` is a list `A[i_1], A[i_2], ..., A[i_k]`
# with `0 <= i_1 < i_2 < ... < i_k <= A.length - 1`, and that a sequence `B` is
# _arithmetic_ if `B[i+1] - B[i]` are all the same value (for `0 <= i < B.length
# - 1`).
#
#
#
# **Example 1:**
#
#
#
#     Input: A = [3,6,9,12]
#     Output: 4
#     Explanation:
#     The whole array is an arithmetic sequence with steps of length = 3.
#
#
# **Example 2:**
#
#
#
#     Input: A = [9,4,7,2,10]
#     Output: 3
#     Explanation:
#     The longest arithmetic subsequence is [4,7,10].
#
#
# **Example 3:**
#
#
#
#     Input: A = [20,1,15,3,10,5,8]
#     Output: 4
#     Explanation:
#     The longest arithmetic subsequence is [20,15,10,5].
#
#
#
#
# **Constraints:**
#
#   * `2 <= A.length <= 1000`
#   * `0 <= A[i] <= 500`
#
#
## @lc code=start
using LeetCode

function longest_arith_seq_length(nums::Vector{Int})
    len = length(nums)
    len == 1 && return 0
    dp = fill(0, len, 1001)
    res = 0
    for i in 1:len, j in 1:(i - 1)
        d = nums[i] - nums[j] + 501
        dp[i, d] = max(dp[i, d], dp[j, d] + 1)
        res = max(res, dp[i, d])
    end
    return res + 1
end
## @lc code=end


# TODO: INNER AND GEOMETRIC PRODUCT DEDICATED TO BLADES
# TODO: NEW STRUTUCT VERSOR
##############################################################################
# Multivector convertion                                                     #
##############################################################################
"""
```
multivector(b::BasisBlade) (Liga function)
multivector(a::Number) (Liga function)
multivector(v::Vector{Float64}) (Liga function)
multivector(A::Blade) (Liga function)
```
This function is used to create a MultiVector type
from numbers, vectors, basis blade or Blades.

## Example (in 𝔾₃₁)

```julia-repl
julia> multivector(e1)
```
returns the multivector `(1.0e1)`.

## Example (in 𝔾₃₁)

```julia-repl
julia> multivector([1.0,2.0,3.0])
```
returns the multivector `(1.0 e1)+(2.0 e2)+(3.0 e3)`.

## Example (in 𝔾₃₁)

```julia-repl
julia> multivector(Blade([[1.0,2.0,3.0]],1,true))
```
returns the multivector `(1.0 e1)+(2.0 e2)+(3.0 e3)`.
"""
function multivector(a::BasisBlade)
	ms=spzeros(GIdLayout[4])
	if a.index<0
		ms[-a.index]=-1.0
	else
		ms[a.index]=1.0
	end
	return MultiVector(ms)
end
function multivector(a::Number)
	ms=spzeros(GIdLayout[4])
	ms[1]=Float64(a)
	return MultiVector(ms)
end

# rever estas funções
function multivector(v::Vector{Float64})
	ms=spzeros(GIdLayout[4])
	for i=1:length(v)
		ms[i+1]=v[i]
	end
	return MultiVector(ms)
end
function multivector(A::Blade)
	ms=multivector(A.vectors[1])
	for i=2:A.grade
		ms=outer(ms,multivector(A.vectors[i]))
	end
	if A.scalar≈1.0
		return ms
	else
		return A.scalar*ms
	end
end

##############################################################################
#  Blade convertion and constructor                                          #
##############################################################################
"""
```
blade(scalar,args::Vector{Float64}...) (Liga Function)
```
This function is useful for two purposes:
 1. Is a simple to create a Blade type element;
 2. Convert a Float64 vector into Blade type element.

 ## Example (in 𝔾₃₁)

 ```julia-repl
julia> blade(1.0,[1.0,2.0],[1.0,-1.0])
```
returns the Blade type element `1.0*[1.0,2.0]∧[1.0,-1.0]`.
"""
function blade(scalar,args::Vector{Float64}...)
	nargs=length(args)
	v=Vector{Vector{Float64}}(undef,nargs)
	for i=1:nargs
		v[i]=args[i]
	end
	return Blade(scalar,v,nargs,true)
end

##############################################################################
# Grade like functions                                                       #
##############################################################################
"""
```
grade(b::BasisBlade) (Liga function)
```
This function is used to determine the grade of a basis blade element

## Example (in 𝔾₃₁)

```julia-repl
julia> grade(e123)
```
return the value (Int) `3`.
"""
function grade(b::BasisBlade)
	if b.index==0
		return 0
	else
		return length(findall(LogicalBasisBlade[abs(b.index)]))
	end
end

"""
```
gradeplus(b::BasisBlade) (Liga function)
```
This function is used to determine the grade plus a Basis Blade element.
It returns an integer.
"""
function gradeplus(b::BasisBlade)
	if b.index==0
		return 0
	else
		return length(findall(LogicalBasisBlade[abs(b.index)][1:GIdLayout[1]]))
	end
end

"""
```
grademinus(b::BasisBlade) (Liga function)
```
This function is used to determine the grade minus of a basis blade element
It returns an integer.
"""
function grademinus(b::BasisBlade)
	if b.index==0
		return 0
	else
		return length(findall(LogicalBasisBlade[abs(b.index)][GIdLayout[1]+1:GIdLayout[1]+GIdLayout[2]]))
	end
end

"""
```
gradeprojection(b::BasisBlade,k::Int) (Liga function)
```
This function is used to determine the grade projection of a basis blade.
This function returns a Basis Blade element.

## Example (in 𝔾₃₁)

```julia-repl
julia> gradeprojection(-e123,3)
```
returns the Basis Blade `-e123`
"""
function gradeprojection(b::BasisBlade,k::Int)
	if grade(b)==k
		return b
	else
		return BasisBlade(0)
	end
end

"""
```
gradeprojection(b::MultiVector,k::Int) (Liga function)
```
This function is used to determine the grade projection of a multivector.
This function returns a MultiVector element.

## Example (in 𝔾₃₁)

```julia-repl
julia> gradeprojection(1.0*id+e2-2.0*e12p,3)
```
returns the multivector  `- ( 2.0 e12+ )`.
"""
function gradeprojection(b::MultiVector,k::Int)
	ind=findnz(b.comp)
	nnv=nnz(b.comp)
	ms=spzeros(GIdLayout[4])
	for i=1:nnv
		bb=gradeprojection(BasisBlade(ind[1][i]),k)
		if bb.index!=0
			ms[bb.index]=ind[2][i]
		end
	end
	return MultiVector(ms)
end

############################################################################
# Geometric product functions                                              #
############################################################################

"""
```
geometric(a::GAAbstractType,b::GAAbstractType) (Liga function)
```
This function is used to calculate the geometric product of
GAAbstractType elements (Basis Blade, MultiVector or Blades).
It returns a MultiVector type.

## Example (in 𝔾₃)

```julia-repl
julia> geometric(e12,e12)
```
returns a multivector  `-1.0id`.

Note that we can use other structs in arguments.

## Example (in 𝔾₃)

```julia-repl
julia> geometric(1.0+3.0*e12,e1)
```
returns `( 1.0 e1 ) - ( 3.0 e2)`.

## Example (in 𝔾₄₁ conformal)
```julia-repl
julia> u=2.0*id + 1.5*e1 -3.0*e2 +4.0*e∞ -3.0*e0

julia> v=3.0ep

julia> geometric(u,v)
```
returns `( 16.5 id ) + ( 6.0 e+ ) + ( 4.5 e1+ ) - ( 9.0 e2+ ) - ( 7.5 e+- )`.

You can use the infix operator ∘ instead of geometric command.

## Example (in 𝔾₃)

```julia-repl
julia> u = 1.0+3.0*e12

julia> v = e1

julia> u ∘ v
```
"""
function geometric(a::GAAbstractType,b::GAAbstractType)
	if !isa(a,MultiVector)
		a=multivector(a)
	end
	if !isa(b,MultiVector)
		b=multivector(b)
	end
	ind1=findnz(a.comp)
	ind2=findnz(b.comp)
	nnv1=nnz(a.comp)
	nnv2=nnz(b.comp)
	ms=spzeros(GIdLayout[4])
	for i=1:nnv1
		vl=ind1[2][i]
		for j=1:nnv2
			ind=BBgeoprodTable[ind1[1][i],ind2[1][j]].index
			if ind>0
				ms[ind]=ms[ind]+vl*ind2[2][j]
			end
			if ind<0
				ms[-ind]=ms[-ind]-vl*ind2[2][j]
			end
		end
	end
	return MultiVector(ms)
end

"""
```
∘(a::GAAbstractType,b::GAAbstractType) (Liga function)
```
It is an infix operator for geometric product. This function is
used to calculate the geometric product of GAAbstractType elements
(Basis Blade, MultiVector or Blades). It returns a MultiVector type.

## Example (in 𝔾₃)

```julia-repl
julia> u = 1.0+3.0*e12

julia> v = e1

julia> u ∘ v
```
"""
function Base.:∘(a::GAAbstractType,b::GAAbstractType)
	return geometric(a,b)
end

################################################################################
# inner product                                                                #
################################################################################

"""
```
inner(a::GAAbstractType,b::GAAbstractType) (Liga function)
```
This function is used to determine the inner product
of GAAbstractType elements (Basis Blade, MultiVector or Blades).

## Example (in 𝔾₃)

```julia-repl
julia> inner(1.0+3.0*e12,e1)
```
returns `- ( 3.0 e2 )`.

## Example (in 𝔾₄₁ conformal)

```julia-repl
julia> u=2.0*id + 1.5*e1 -3.0*e2 +4.0*e∞ -3.0*e0

julia> v=3.0*ep

julia> inner(u,v)
```
returns `( 16.5 id )`.

An alternative way to run this functions is using the infix
operator (⋅).
"""
function inner(a::GAAbstractType,b::GAAbstractType)
	if !isa(a,MultiVector)
		a=multivector(a)
	end
	if !isa(b,MultiVector)
		b=multivector(b)
	end
	ind1=findnz(a.comp)
	ind2=findnz(b.comp)
	nnv1=nnz(a.comp)
	nnv2=nnz(b.comp)
    ms=spzeros(GIdLayout[4])
	for i=1:nnv1
		vl=ind1[2][i]
		for j=1:nnv2
			ind=BBinnerprodTable[ind1[1][i],ind2[1][j]].index
			if ind>0
				ms[ind]=ms[ind]+vl*ind2[2][j]
			end
			if ind<0
				ms[-ind]=ms[-ind]-vl*ind2[2][j]
			end
		end
    end
    return MultiVector(ms)
end

"""
```
⋅(a::GAAbstractType,b::GAAbstractType) (Liga function)
```
This function is used to determine the inner product
of GAAbstractType elements (Basis Blade or MultiVector).

## Example (in 𝔾₄₁ conformal)

```julia-repl
julia> u=2.0*id + 1.5*e1 -3.0*e2 +4.0*e∞ -3.0*e0

julia> v=3.0*ep

julia> u⋅v
```
returns `( 16.5 id )`.
"""
function ⋅(a::GAAbstractType,b::GAAbstractType)
	return inner(a,b)
end

##############################################################################
# outer product                                                              #
##############################################################################

"""
```
outer(a::GAAbstractType,b::GAAbstractType) (Liga function)
```
This function is used to determine the outer product
of GAAbstractType elements (Basis Blade, MultiVector or Blades).

## Example (in 𝔾₃)

```julia-repl
julia> outer(1.0+3.0*e12,e1)
```
returns `( 1.0 e1 )`.

## Example (in 𝔾₄₁ conformal)

```julia-repl
julia> u=2.0*id + 1.5*e1 -3.0*e2 +4.0*e∞ -3.0*e0

julia> v=3.0*ep

julia> outer(u,v)
```
returns `( 6.0 e+ ) + ( 4.5 e1+ ) - ( 9.0 e2+ ) - ( 7.5 e+- )`.

An alternative way to run this function is using the infix
operator (^).

```julia-repl
julia> u=2.0*id + 1.5*e1 -3.0*e2 +4.0*e∞ -3.0*e0

julia> v=3.0*ep

julia> u^v
```
"""
function outer(a::GAAbstractType,b::GAAbstractType)
	if isa(a,Blade) && isa(b,Blade)
		len = length(a.vectors[1])
		if a.grade+b.grade>GIdLayout[1]+GIdLayout[2]
			return Blade(0.0,[zeros(len)],0,true)
		else
			A=zeros(len,a.grade+b.grade)
			for i=1:a.grade
				A[:,i]=a.vectors[i]
			end
			for i=a.grade+1:b.grade
				A[:,i]=b.vectors[i]
			end
			if rank(A)<a.grade+b.grade
				return Blade(0.0,[zeros(len)],0,true)
			else
				return Blade(a.scalar*b.scalar,[a.vectors;b.vectors],a.grade+b.grade,true)
			end
		end
	else
		if !isa(a,MultiVector)
			a=multivector(a)
		end
		if !isa(b,MultiVector)
			b=multivector(b)
		end
		ind1=findnz(a.comp)
		ind2=findnz(b.comp)
		nnv1=nnz(a.comp)
		nnv2=nnz(b.comp)
    	ms=spzeros(GIdLayout[4])
		for i=1:nnv1
			vl=ind1[2][i]
			for j=1:nnv2
				ind=BBouterprodTable[ind1[1][i],ind2[1][j]].index
				if ind>0
					ms[ind]=ms[ind]+vl*ind2[2][j]
				end
				if ind<0
					ms[-ind]=ms[-ind]-vl*ind2[2][j]
				end
			end
    	end
		return MultiVector(ms)
	end
end

"""
```
^(a::GAAbstractType,b::GAAbstractType) (Liga function)
```
In an infix operator to outer product.
This function is used to determine the outer product
of GAAbstractType elements (Basis Blade, MultiVector or Blades).

## Example (in 𝔾₃)

```julia-repl

julia> 1.0+3.0*e12^e1

```
returns `( 1.0 e1 )`.

## Example (in 𝔾₄₁ conformal)

```julia-repl
julia> u=2.0*id + 1.5*e1 -3.0*e2 +4.0*e∞ -3.0*e0

julia> v=3.0*ep

julia> u^v
```
returns `( 6.0 e+ ) + ( 4.5 e1+ ) - ( 9.0 e2+ ) - ( 7.5 e+- )`.

"""
function Base.:^(a::GAAbstractType,b::GAAbstractType)
	return outer(a,b)
end

#############################################################################
# sum and difference operations                                             #
#############################################################################
function Base.:+(a::Number,b::GAAbstractType)
	a=multivector(a)
	if !isa(b,MultiVector)
		b=multivector(b)
	end
	return MultiVector(a.comp+b.comp)
end
function Base.:+(b::GAAbstractType,a::Number)
	a=multivector(a)
	if !isa(b,MultiVector)
		b=multivector(b)
	end
	return MultiVector(a.comp+b.comp)
end
function Base.:+(a::GAAbstractType,b::GAAbstractType)
	if !isa(a,MultiVector)
		a=multivector(a)
	end
	if !isa(b,MultiVector)
		b=multivector(b)
	end
	return MultiVector(a.comp+b.comp)
end
function Base.:-(a::Number,b::GAAbstractType)
	a=multivector(a)
	if !isa(b,MultiVector)
		b=multivector(b)
	end
	return MultiVector(a.comp-b.comp)
end
function Base.:-(b::GAAbstractType,a::Number)
	a=multivector(a)
	if !isa(b,MultiVector)
		b=multivector(b)
	end
	return MultiVector(b.comp-a.comp)
end
function Base.:-(a::GAAbstractType,b::GAAbstractType)
	if !isa(a,MultiVector)
		a=multivector(a)
	end
	if !isa(b,MultiVector)
		b=multivector(b)
	end
	return MultiVector(a.comp-b.comp)
end
#Basis blade signal
function Base.:-(a::BasisBlade)
	if a.index>0
		return BasisBlade(-a.index)
	else
		return BasisBlade(a.index)
	end
end

##############################################################################
# scalar product                                                             #
##############################################################################
"""
```
scalar(a::Number,b::GAAbstractType) (Liga function)
```
This function is used to define the most simple multivector structure.

## Example (in 𝔾₃₁)
```julia-repl
julia> u = scalar(0.13,e1)
```
returns a multivector defined by [0.0,0.13,0.0,...,0.0] (see more about
MultiVectors). Consequently it is a constructor if b is a blade basis. If b is
a MultiVector, it return the scalar product. For example,
```julia-repl
julia> u = scalar(0.13,e2-e1+2.0)
```
returns the multivector ( 0.26 id ) - (0.13 e1 ) + ( 0.13 e2 ).
An alternative way to use this command is by the simbol ``*``. For example,
```julia-repl
julia> u = 0.13*(e2-e1+2.0)
```
"""
function scalar(a::Number,b::GAAbstractType)
	if !isa(b,MultiVector)
		b=multivector(b)
	end
	return MultiVector(a.*(b.comp))
end
function scalar(b::GAAbstractType,a::Number)
	return scalar(a,b)
end
"""
```
scalar(A::Blade,B::Blade) (Liga function)
```
This function returns a scalar product of
two blades
"""
function scalar(A::Blade,B::Blade)
	if A.grade==B.grade
		return inner(A,B)
	else
		return 0.0
	end
end
"""
```
*(a::Number,b::GAAbstractType) (Liga function)
```
This function is used to define the most simple multivector structure.

## Example (in 𝔾₃₁)

```julia-repl
julia> u = scalar(0.13,e1)
```
returns a multivector defined by [0.0,0.13,0.0,...,0.0] (see more about
MultiVectors). Consequently it is a constructor if b is a blade basis. If b is
a MultiVector, it return the scalar product. For example,
```julia-repl
julia> u = scalar(0.13,e2-e1+2.0)
```
returns the multivector ( 0.26 id ) - (0.13 e1 ) + ( 0.13 e2 ).
An alternative way to use this command is by the simbol ``*``. For example,
```julia-repl
julia> u = 0.13*(e2-e1+2.0)
```
"""
function Base.:*(a::Number,b::GAAbstractType)
	return scalar(a,b)
end
function Base.:*(b::GAAbstractType,a::Number)
	return scalar(a,b)
end
function Base.:*(A::Blade,B::Blade)
	return scalar(A,B)
end
##############################################################################
# Involutions                                                                #
##############################################################################

"""
```
reverse(b::BasisBlade) (Liga function)
```
This function calculates the reverse of a Basis Blade.
For example,
```julia-repl
julia> u = reverse(e12)
```
returns the BasisBlade `-e12`.
"""
function reverse(b::BasisBlade)
	k=grade(b)
	if (-1.0)^(k*(k-1)/2.0)>0.0
		return b
	else
		return BasisBlade(-b.index)
	end
end

"""
```
reverse(V::MultiVector) (Liga function)
```
This function calculates the reverse of a MultiVector.
It returns a new MultiVector.

## Example (in 𝔾₃₁)

```julia-repl
julia> u=1.0*id -2.0*e1 -5.0*e∞ +3.0*e∞0
julia> reverse(u)
```
returns the multivector `(1.0 id) - ( 2.0 e1) - ( 5.0 e∞ ) - ( 3.0 e∞0 )`.
"""
function reverse(b::MultiVector)
	ind=findnz(b.comp)
	nnv=nnz(b.comp)
	ms=spzeros(GIdLayout[4])
	for i=1:nnv
		a=reverse(BasisBlade(ind[1][i]))
		if a.index>0
			ms[a.index]=ind[2][i]
		else
			ms[-a.index]=-ind[2][i]
		end
	end
	return MultiVector(ms)
end

"""
```
reverse(B::Blade) (Liga function)
```
This function calculates the reverse of a Blade.
It returns a new Blade.

## Example (in 𝔾₃₁)
```julia-repl
julia> B=Blade([[1.0,0.0,0.0],[0.0,1.0,0.0]],2,true)
julia> reverse(B)
```
returns `-1.0*[1.0, 0.0, 0.0]∧[0.0, 1.0, 0.0]`.
"""
function reverse(A::Blade)
	#tmp=Vector{Float64}(undef,GIdLayout[])
	A.scalar=(-1.0)^((A.grade*(A.grade-1))/2.0)
	return A
end

"""
```
conjugate(b::BasisBlade) (Liga function)
```
This function calculates the conjugate of a Basis Blade.

## Example (in 𝔾₃₁)

```julia-repl
julia> u = conjugate(e12)
```
returns the BasisBlade `-e12`.
"""
function conjugate(b::BasisBlade)
	#pergunta: como fica no conformal?
	r=grademinus(b)
	if (-1.0)^r>0.0
		return reverse(b)
	else
		return -reverse(b)
	end
end

"""
```
conjugate(V::MultiVector) (Liga function)
```
This function calculates the conjugate of a MultiVector
and return a multivector.

## Example (in 𝔾₃₁)

```julia-repl
julia> u = -1.0+3.0*e1-1.0*e12

julia> conjugate(u)
```
returns the MultiVector `- ( 1.0 id ) + ( 3.0 e1 ) + ( 1.0 e12 )`.
"""
function conjugate(b::MultiVector)
	ind=findnz(b.comp)
	nnv=nnz(b.comp)
	ms=spzeros(GIdLayout[4])
	for i=1:nnv
		a=conjugate(BasisBlade(ind[1][i]))
		if a.index>0
			ms[a.index]=ind[2][i]
		else
			ms[-a.index]=-ind[2][i]
		end
	end
	return MultiVector(ms)
end

function conjugatevector(v::Vector{Float64})
	for i=2:length(v)+1
		if grademinus(BasisBlade(i))==1
			v[i-1]=-v[i-1]
		end
	end
	return v
end

"""
```
conjugate(B::Blade) (Liga function)
```
This function calculates the conjugate of a Blade
and return a new Blade.

## Example (in 𝔾₃₂)

```julia-repl
julia> u = blade([1.0,2.0,3.0,4.0,5.0],[-1.0,-1.0,-1.0,-1.0,3.0])

julia> conjugate(u)
```
returns the Blade `[-1.0, -1.0, -1.0, 1.0, -3.0]∧[1.0, 2.0, 3.0, -4.0, -5.0]`.
"""
function conjugate(A::Blade)
	v=Vector{Vector{Float64}}(undef,A.grade)
	for i=1:A.grade
		v[i]=conjugatevector(A.vectors[A.grade+1-i])
	end
	return Blade(v,A.grade,true)
end

###############################################################################
#               Duality                                                       #
###############################################################################
"""
```
dual(b::GAAbstractType) (Liga function)
```
This function calculates the dual of a GAAbstractType
and returns a multivector element.

## Example (in 𝔾₃₁)

```julia-repl
julia> u = dual(e12)
```
returns the multivector ( 1.0 e+- ).

## Example (in 𝔾₃₂)

```julia-repl
julia> b=blade([1.0,0.0,3.0,0.0,5.0],[0.0,-1.0,-1.0,-1.0,0.0])
```

returns the multivector  `- ( 5.0 e123 ) - ( 5.0 e124 ) + ( 5.0 e134 ) - ( 3.0 e125 ) - ( 1.0 e235 ) - ( 3.0 e145 ) - ( 1.0 e245 ) + ( 1.0 e345 )`.
"""
function dual(b::GAAbstractType)
	if !isa(b,MultiVector)
		b=multivector(b)
	end
	#b=multivector(a)
	ind=findnz(b.comp)
	nnv=nnz(b.comp)
	ms=ind[2][1]*bbdual(BasisBlade(ind[1][1]))
	for i=2:nnv
		ms=ms+ind[2][i]*bbdual(BasisBlade(ind[1][i]))
	end
	return ms
end

"""
```
bbdual(b::BasisBlade) (Liga function)
```
This function calculates the dual of a BasisBlade type
and returns a multivector element. It is an auxiliary function.
"""
function bbdual(a::BasisBlade)
	#cj=conjugate(I)
	#if cj.index<0
	#	return -bbgeometric(a,BasisBlade(-cj.index))
	#else
	#	return  bbgeometric(a,cj)
	#end
	return geometric(a,conjugate(I))
end
#function dual(b::MultiVector)
#	ind=findnz(b.comp)
#	nnv=nnz(b.comp)
#	ms=ind[2][1]*bbdual(BasisBlade(ind[1][1]))
#	for i=2:nnv
#		ms=ms+ind[2][i]*bbdual(BasisBlade(ind[1][i]))
#	end
#	return ms
#end
#function dual(A::Blade)
#	return dual(multivector(A))
#end

###############################################################################
#               Magnitude                                                     #
###############################################################################
"""
```
magnitude(A::GAAbstractType) (Liga function)
```
This function calculates the magnitude of a general GAAbstractType
and returns a scalar.

## Example in 𝔾₄₁ (layout(4,1,"GA"))

```julia-repl
julia> magnitude(2.0*id -2.0e3)
```
returns `2.8284271247461903`.
"""
function magnitude(A::GAAbstractType)
	if !isa(A,MultiVector)
		A=multivector(A)
	end
	return norm(A.comp,2)
end

###############################################################################
#              Inverse                                                        #
###############################################################################
"""
```
inverse(b::BasisBlade) (Liga function)
```
This function calculates the inverse of a Basis Blade.
For example, in layout(3,1,"GA")
```julia-repl
julia> u = inverse(e12)
```
returns the BasisBlade -e12. Note that conjugate (for basis blade) is equal to
inverse command.
"""
function inverse(a::BasisBlade)
	return conjugate(a)
end
"""
```
inverse(b::MultiVector) (Liga function)
```
This function calculates the inverse of a multivector element.
For example, in layout(3,1,"GA")
```julia-repl
julia>  u = inverse(e1+e12-(1.5*en))
```
returns the multivector ( 0.5714 e1 ) - ( 0.0952 e- ) - ( 0.5714 e12 ) - ( 0.7619 e2- ).

"""
function inverse(a::MultiVector)
	len=GIdLayout[4]
	A=spzeros(len,len)
	for i=1:len
		A[i,:]=a.comp'*Γ[i]
	end
	b=zeros(len)
	b[1]=1.0
	#display(det(A))
	return MultiVector(sparse(A\b))
end

#need to check!
function inverse(A::Blade)
	B=reverse(A)
	B.vectors[1]=(1.0/geometric(A,B).comp[1])*B.vectors[1]
	return Blade(B.vectors,A.grade,true)
end
###############################################################################
# embedding                                                                   #
###############################################################################
"""
```
embedding(v::Vector{Float64}) (Liga function)
```
This function makes sense just in Conformal or Projective layout.
It was define to embedding a real vector in corresponding enviromment.
For exemplo, let us suppose that layout(4,1,"Conformal") was defined.
So, it is possible embedding a vector (like [1.0,2.0,3.0]) 	in conformal
space as the following.

```julia-repl
julia>  embedding([1.0,2.0,3.0])
```
returns the multivector ( 1.0 e1 ) + ( 2.0 e2 ) + ( 3.0 e3 ) + ( 6.5 e+ ) + ( 7.5 e- ).
If projective space was defined (like layout(4,0,"Projective")) the same vector
becomes ( 1.0 e1 ) + ( 2.0 e2 ) + ( 3.0 e3 ) + ( 1.0 e4 ).
"""
function embedding(x::Vector{Float64})
	if GIdLayout[3]=="GA"
		error("function defined but not enable for this layout.")
	else
		ms=spzeros(GIdLayout[4])
		n=length(x)
		if n!=GIdLayout[1]-1
			error("something wrong with layout dimension and vector dimension")
		else
			if GIdLayout[3]=="Projective"
				ms[2:n+1]=x
				ms[n+2]=1.0
				return MultiVector(ms)
			end
			if GIdLayout[3]=="Conformal"
				ms[2:n+1]=x
				a=0.5*(x'*x)
				ms[n+2]=a-0.5
				ms[n+3]=a +0.5
				return MultiVector(ms)
			end
		end
	end
end
###############################################################################
# convertions                                                                 #
###############################################################################
function Base.convert(::Type{Float64}, x::MultiVector)
	return x.comp[1]
end
function Base.Float64(x::MultiVector)
	return convert(Float64,x)
end

##############################################################################
# copy function                                                              #
# ############################################################################
function Base.copy(A::MultiVector)
	return MultiVector(copy(A.comp))
end
function Base.copy(A::Blade)
	return Blade(copy(A.vectors),copy(A.grade),copy(A.checked))
end

##############################################################################
# equal function                                                             #
# ############################################################################
function Base.isequal(A::MultiVector,B::MultiVector)
	return isequal(A.comp,B.comp)
end

function Base.:(==)(A::MultiVector,B::MultiVector)
	return A.comp==B.comp
end
##############################################################################
# approx function                                                            #
##############################################################################
function Base.:≈(A::MultiVector,B::MultiVector)
	return isapprox(A.comp,B.comp,atol=LigaPrecision)
end


using ABCDistances
Libdl.dlopen("/usr/lib/liblapack.so.3", Libdl.RTLD_GLOBAL); ##Needed to avoid PyPlot problems on my work machine
##I sometimes also need run "export LD_LIBRARY_PATH=$HOME/.julia/v0.6/Conda/deps/usr/lib/:$LD_LIBRARY_PATH" - see https://github.com/JuliaPy/Conda.jl/issues/105
using PyPlot
using Distributions
import Distributions.length, Distributions._rand!, Distributions._pdf ##So that these can be extended

P = [ 1 0; ##Prey growth; requires 1 unit of prey
      1 1; ##Predation; requires 1 predator and 1 prey
      0 1] ##Predator death; requires 1 unit of predator
Q = [ 2 0; ##Prey growth; 2 units of prey produced
      0 2; ##Predation; 2 units of predator produced
      0 0] ##Predator death; nothing produced

stoichiometry_LV = Stoichiometry(P, Q)

##An illustration of the complete data simulation function
srand(1)
(LV_times, LV_states) = gillespie_sim(stoichiometry_LV, [50, 100], [0.5,0.0025,0.3], 100.0, 10000)

PyPlot.figure();
PyPlot.subplot(121);
plot(LV_states[1,:], LV_states[2,:])
PyPlot.title("Trajectory")
PyPlot.xlabel("Prey");
PyPlot.ylabel("Predators");
PyPlot.subplot(122);
plot(LV_times, LV_states[1,:]);
plot(LV_times, LV_states[2,:]);
PyPlot.legend(["Prey", "Predators"]);
PyPlot.ylim([0.,510.]); ##Stops legend blocking lines
PyPlot.title("Trace plots");
PyPlot.xlabel("Time");
PyPlot.ylabel("Population");
PyPlot.tight_layout();
PyPlot.savefig("LV_full_data.pdf");

##Simulate observed data as in Owen et al 2014 (their dataset D2), but without time 0
obs_times = [2.0, 4.0, 6.0, 8.0, 10.0, 12.0, 14.0, 16.0, 18.0, 20.0, 22.0, 24.0, 26.0, 28.0, 30.0, 32.0]
state0 = [50, 100]
theta0 = [1.0, 0.005, 0.6]
srand(1)
(LV_success, LV_obs) = gillespie_partial_sim(stoichiometry_LV, state0, theta0, obs_times, 100000)
σ0 = exp(2.3) ##Scale of observation noise
nobs = 2*length(obs_times)
x0 = vec(LV_obs') + σ0*randn(nobs); ##Noisy observations
PyPlot.figure();
plot(obs_times, x0[1:16], "-o");
plot(obs_times, x0[17:32], "-o");
PyPlot.legend(["Prey", "Predators"]);
PyPlot.xlabel("Time");
PyPlot.ylabel("Noisy observation");
PyPlot.tight_layout();
PyPlot.savefig("LV_obs.pdf");

####################################
##ABC ON A SINGLE DATASET
##nb initial state and σ known
##(parameters are the log rates)
####################################
##Define prior
marg_rate_prior = Uniform(-6.0,2.0)

type LVprior <: ContinuousMultivariateDistribution
end

function length(d::LVprior)
    3
end

function _rand!{T<:Real}(d::LVprior, x::AbstractVector{T})
    x = rand(marg_rate_prior, 3)
end

function _pdf{T<:Real}(d::LVprior, x::AbstractVector{T})
    if (all(-6.0 .<= x .<= 2.0))
        return 8.0^-3
    else
        return 0.0
    end
end

##Define model
function sample_sumstats(pars::Array{Float64,1})
    (success, x) = gillespie_partial_sim(stoichiometry_LV, state0, exp.(pars), obs_times, 100000)
    if (success)
        stats = vec(x') + σ0*randn(nobs)
    else
        stats = zeros(x)
    end
    (success, stats)
end

abcinput = ABCInput();
abcinput.prior = LVprior();
abcinput.sample_sumstats = sample_sumstats;
abcinput.abcdist = WeightedEuclidean(x0)
abcinput.nsumstats = nobs;

srand(1);
pmcoutput_alg3 = abcPMC3(abcinput, 200, 1/2, 50000, store_init=true);
pmcoutput_alg3V = abcPMC3V(abcinput, 200, 1/2, 50000);
pmcoutput_alg5 = abcPMC5(abcinput, 200, 1/2, 50000, store_init=true);
pmcoutput_alg4 = abcPMC4(abcinput, 200, 1/2, 50000, store_init=true);
abcinput.abcdist = MahalanobisEmp(x0)
pmcoutput_alg5_Mahalanobis = abcPMC5(abcinput, 200, 1/2, 50000, store_init=true);

##Plot weights
w1 = pmcoutput_alg3.abcdists[1].w;
w2 = pmcoutput_alg5.abcdists[pmcoutput_alg5.niterations].w;
w3 = pmcoutput_alg4.abcdists[pmcoutput_alg4.niterations].w;
PyPlot.figure(figsize=(12,6));
PyPlot.subplot(121);
plot(obs_times, w1[1:16]/sum(w1), "b-o");
plot(obs_times, w3[1:16]/sum(w3), "g-^");
plot(obs_times, w2[1:16]/sum(w2), "y-*");
PyPlot.ylim([0.,0.1])
PyPlot.xlabel("Time");
PyPlot.ylabel("Relative weight");
PyPlot.title("Prey");
PyPlot.legend(["Algorithm 3","Algorithm 4\n(last iteration)","Algorithm 5\n(last iteration)"]);
PyPlot.subplot(122);
plot(obs_times, w1[17:32]/sum(w1), "b-o");
plot(obs_times, w3[17:32]/sum(w3), "g-^");
plot(obs_times, w2[17:32]/sum(w2), "y-*");
PyPlot.ylim([0.,0.1])
PyPlot.ylabel("Relative weight");
PyPlot.xlabel("Time");
PyPlot.title("Predators");
PyPlot.legend(["Algorithm 3","Algorithm 4\n(last iteration)","Algorithm 5\n(last iteration)"]);
PyPlot.tight_layout();
PyPlot.savefig("LV_weights.pdf");

##Plot MSEs. First only those algorithms used in paper.
outputs = (pmcoutput_alg3, pmcoutput_alg4, pmcoutput_alg5);
pnames = ("Prey growth", "Predation", "Predator death");
leg_code = ("b-o", "g-^", "y-*");
PyPlot.figure(figsize=(12,6))
for i in 1:length(outputs)
    s = outputs[i]
    m = parameter_means(s)
    v = parameter_vars(s)
    c = s.cusims ./ 1000;
    for j in 1:3
        PyPlot.subplot(1, 3, j)
        PyPlot.plot(c, log10.(v[j,:] .+ (m[j,:]-log(theta0[j])).^2), leg_code[i])
    end
end
for i in 1:3
    PyPlot.subplot(1, 3, i)
    PyPlot.title(pnames[i])
    PyPlot.xlabel("Number of simulations (000s)")
    PyPlot.ylabel("log₁₀(MSE)")
    PyPlot.legend(["Algorithm 3", "Algorithm 4", "Algorithm 5"])
end
PyPlot.tight_layout();
PyPlot.savefig("LV_mse.pdf");

##Add MSEs for other algorithms
outputs = (pmcoutput_alg3V, pmcoutput_alg5_Mahalanobis);
leg_code = ("r-x", "k-|");
for i in 1:length(outputs)
    s = outputs[i]
    m = parameter_means(s)
    v = parameter_vars(s)
    c = s.cusims ./ 1000;
    for j in 1:3
        PyPlot.subplot(1, 3, j)
        PyPlot.plot(c, log10.(v[j,:] .+ (m[j,:]-log(theta0[j])).^2), leg_code[i])
    end
end

##Simulations used for distance initialisation

##Uncomment these rows to compare all 3 outputs
##ss_toplot = Array[pmcoutput_alg3.init_sims[1],
##                  pmcoutput_alg4.init_sims[pmcoutput_alg4.niterations],
##                  pmcoutput_alg5.init_sims[pmcoutput_alg5.niterations]];
##ss_names = ["Algorithm 3\n(first iteration)", "Algorithm 4\n(last iteration)", "Algorithm 5\n(last iteration)"];
ss_toplot = Array[pmcoutput_alg3.init_sims[1],
                  pmcoutput_alg5.init_sims[pmcoutput_alg5.niterations]];
ss_names = ["Algorithm 3\n(first iteration)", "Algorithm 5\n(last iteration)"];

PyPlot.figure(figsize=(12,12));
plotcounter = 1;
for ss in ss_toplot
    PyPlot.subplot(length(ss_toplot), 2, plotcounter)
    for i in 1:20
        plot(obs_times, ss[1:16,i])
    end
    plot(obs_times, x0[1:16], "ko", markersize=5)
    PyPlot.ylim([0,1000])
    plotcounter += 1
    PyPlot.subplot(length(ss_toplot), 2, plotcounter)
    for i in 1:20
        plot(obs_times, ss[17:32,i])
    end
    plot(obs_times, x0[17:32], "ko", markersize=5)
    PyPlot.ylim([0,1000])
    plotcounter += 1
end
PyPlot.subplot(length(ss_toplot), 2, 1)
PyPlot.title("Prey");
PyPlot.subplot(length(ss_toplot), 2, 2)
PyPlot.title("Predators");
PyPlot.subplot(length(ss_toplot), 2, 2*length(ss_toplot)-1)
PyPlot.xlabel("Time");
PyPlot.subplot(length(ss_toplot), 2, 2*length(ss_toplot))
PyPlot.xlabel("Time");
for i in 1:length(ss_toplot)
    PyPlot.subplot(length(ss_toplot), 2, 2i-1)
    PyPlot.ylabel("Population")
    line_invis = plot((0,0),(0,0),"w-")
    PyPlot.legend([line_invis], [ss_names[i]], handlelength=0)
    PyPlot.subplot(length(ss_toplot), 2, 2i)
    PyPlot.legend([line_invis], [ss_names[i]], handlelength=0)
end
PyPlot.tight_layout();
PyPlot.savefig("LV_paths.pdf");

##Marginal posterior plots
PyPlot.figure();
samplesABC = (pmcoutput_alg3.parameters[:,:,pmcoutput_alg3.niterations], pmcoutput_alg4.parameters[:,:,pmcoutput_alg4.niterations], pmcoutput_alg5.parameters[:,:,pmcoutput_alg5.niterations]);
weightsABC = (pmcoutput_alg3.weights[:,pmcoutput_alg3.niterations], pmcoutput_alg4.weights[:,pmcoutput_alg4.niterations], pmcoutput_alg5.weights[:,pmcoutput_alg5.niterations]);
for i in 1:3 ##Loop over algorithms
    ww = weightsABC[i]
    for j in 1:3 ##Loop over parameters
        pp = samplesABC[i][j,:]
        PyPlot.subplot(130+j)
        PyPlot.plt[:hist](pp, weights=ww, normed=true, alpha=0.5)
    end
end

##Posterior summaries
outputs = (pmcoutput_alg3, pmcoutput_alg4, pmcoutput_alg5);
mapreduce(p -> parameter_means(p)[:,p.niterations], hcat, outputs)' ##Posterior means
sqrt.(mapreduce(p -> parameter_vars(p)[:,p.niterations], hcat, outputs))' ##Posterior sds

##Investigate best choice of alpha (as requested by reviewers)
function get_mse(pars::Array{Float64, 2}, w::Array{Float64, 1})
    mse = 0.0
    for i in 1:size(pars)[2]
        mse += sum((pars[:, i] .- log(theta0)).^2) * w[i]
    end
    mse
end

alphas = 0.05:0.1:0.95;
MSEs_alg5 = zeros(alphas);
MSEs_alg4 = zeros(alphas);
MSEs_alg3 = zeros(alphas);
abcinput.abcdist = WeightedEuclidean(x0);
srand(1);
for i in 1:length(alphas)
    x = abcPMC5(abcinput, 200, alphas[i], 50000)
    nits = x.niterations
    MSEs_alg5[i] = (nits == 0) ? Inf : get_mse(x.parameters[:,:,nits], x.weights[:,nits])
    x = abcPMC3(abcinput, 200, alphas[i], 50000)
    nits = x.niterations
    MSEs_alg3[i] = (nits == 0) ? Inf : get_mse(x.parameters[:,:,nits], x.weights[:,nits])
    x = abcPMC4(abcinput, 200, alphas[i], 50000)
    nits = x.niterations
    MSEs_alg4[i] = (nits == 0) ? Inf : get_mse(x.parameters[:,:,nits], x.weights[:,nits])
end

PyPlot.figure();
plot(alphas, log10.(MSEs_alg5), "r-x");
plot(alphas, log10.(MSEs_alg3), "b-o");
plot(alphas, log10.(MSEs_alg4), "g-*");
##Best alpha value around 0.5


using OptimalApplication
using DataFrames
using PrettyTables
using Plots
using Statistics: median, quantile

function plot_params()
    Ts = 2.0 .^ (-4:2:4)
    rs = (2.0 .^ (-5:0))

    n_markets = 1000

    sizerangelog2 = (3, 9)

    sizes = round.(
        Int,
        2.0 .^ (
            sizerangelog2[1] .+ (sizerangelog2[2] - sizerangelog2[1]) * rand(n_markets)
        )
    )

    rats = zeros(length(rs), length(Ts), n_markets)

    @time for k in 1:n_markets
        mkt = VariedCostsMarket(sizes[k])
        opt = optimalportfolio_dynamicprogram(mkt)[2]

        Threads.@threads for j in 1:length(Ts)
            for i in 1:length(rs)
                rats[i, j, k] = optimalportfolio_simulatedannealing(
                    mkt;
                    temp=Ts[j],
                    red=rs[i]
                )[2]
            end
        end

        rats[:, :, k] /= optimalportfolio_dynamicprogram(mkt)[2]
    end

    xts = 2 .^ range(sizerangelog2...)

    pl = plot(
        layout=grid(length(rs), length(Ts)),
        link=:all,
        xscale=:log10,
        xticks=(xts, xts),
        legend=nothing,
        size=(1200, 1200),
        xlim=2 .^ (sizerangelog2 .+ (-0.5, 0.5)),
    )

    avgs = zeros(length(rs), length(Ts))
    for j in 1:length(Ts), i in 1:length(rs)
        avgs[i, j] = quantile(rats[i, j, :], 0.1)
    end
    # [sum(rats[i, j, :]) / length(rats[i, j, :]) for i in 1:length(rs), j in 1:length(Ts)]
    best_avg = maximum(avgs)
    @show best_avg

    for j in 1:length(Ts), i in 1:length(rs)
        hline!(pl[i, j], [avgs[i, j]], c=:navy)
        scatter!(
            pl[i, j],
            sizes,
            rats[i, j, :],
            c=avgs[i, j] ≥ best_avg ? :olivedrab : :crimson,
            msc=:auto,
            alpha=0.7,
            title="T = $(round(Ts[j], digits=2)), r = $(round(rs[i], digits=4))",
            titlefontsize=12,
        )
        if i < length(rs)
            plot!(pl[i, j], xticks=(xts, fill("", length(xts))))
        end
        if j > 1
            plot!(pl[i, j], yticks=(Plots.get_ticks(pl[1, 1], :y)[1], fill("", length(xts))))
        end
    end

    return pl, rats, sizes
end

pl, rats, sizes = plot_params()
display(pl)
# savefig("benchmark/annealingparams-plot.png")
# savefig("benchmark/annealingparams-plot.pdf")


# collection functionality for BZ integration.
# For now this simply defined the Monkorst-Pack grid (uniform grid in
# BZ, due to periodicity, this yields spectral accuracy)
# TODO:
#   * loading of BZ-grid files
#   * better exploit BZ symmetries?
#   * high-accuracy adaptive integration?

# ================ storage of k-point dependent arrays ================

"""
store k-point dependent arrays
"""
set_k_array!(at::AbstractAtoms, q, symbol, k) = set_transient!(at, (symbol, k), q)

"""
retrieve k-point dependent arrays
"""
get_k_array(at::AbstractAtoms, symbol, k) = get_data(at, (symbol, k))

"""
check that a k-array exists
"""
has_k_array(at::AbstractAtoms, symbol, k) = has_data(at, (symbol, k))

# ===================== Gamma-point =================

"""
`GammaPoint`: defines Γ-point BZ integration, i.e., a single quadrature point
in the origin.
"""
mutable struct GammaPoint <: BZQuadratureRule end

Base.length(::GammaPoint) = 1

w_and_pts(::GammaPoint) = ([1.0], [JVecF([0.0,0.0,0.0])])

# ================ Monkhorst-Pack Grid ===================

"""
`MPGrid`: defines a Monkhorst-Pack grid for BZ integration

## Constructors:
```
MPGrid(cell::Matrix{Float64}, nkpoints)
MPGrid(at::AbstractAtoms, nkpoints)
```
 * `nkpoints::Tuple{Int64,Int64,Int64}`
"""
mutable struct MPGrid  <: BZQuadratureRule
   k::Vector{JVecF}
   w::Vector{Float64}
end

Base.length(q::MPGrid) = length(q.w)

w_and_pts(qrule::MPGrid) = (qrule.w, qrule.k)

MPGrid(cell::AbstractMatrix, nkpoints::Tuple{Int64, Int64, Int64}) =
   MPGrid(monkhorstpackgrid(cell, nkpoints)...)

MPGrid(at::AbstractAtoms, nkpoints::Tuple{Int64, Int64, Int64}) =
   MPGrid(cell(at), nkpoints)

"""
 `monkhorstpackgrid(cell, nkpoints)` : constructs an MP grid for the
computational cell defined by `cell` and `nkpoints`.
MonkhorstPack: K = {b/kz * j + shift}_{j=-kz/2+1,...,kz/2} with shift = 0.0.
Returns

### Parameters

* 'cell' : 3 × 1 array of lattice vector for (super)cell
* 'nkpoints' : 3 × 1 array of number of k-points in each direction. Now
it can only be (0, 0, kz::Int).

### Output

* `K`: `JVecsF` vector of length Nk
* `weights`: integration weights; scalar (uniform grid) or Nk vector.
"""
function monkhorstpackgrid(cell::AbstractMatrix,
                           nkpoints::Tuple{Int64, Int64, Int64})
   kx, ky, kz = nkpoints
   ## We need to check somewhere that 'nkpoints' and 'pbc' are compatable,
   ## e.g., if pbc[1]==false, then kx!=0 should return an error.
	# if kx != 0 || ky != 0
   #    error("This boundary condition has not been implemented yet!")
   # end
	## We want to sample the Γ-point (which is not really necessary?)
	if mod(kx,2) == 1 || mod(ky,2) == 1 || mod(kz,2) == 1
	     throw(ArgumentError("k should be an even number in Monkhorst-Pack
                grid so that the Γ-point can be sampled!"))
	end
	# compute the lattice vector of reciprocal space
   B = 2 * pi * inv(cell)
   b1, b2, b3 = JVec(B[:,1]), JVec(B[:,2]), JVec(B[:,3])

	# We can exploit the symmetry of the BZ (somewhat; more coudl be done here).
	nx, ny, nz = nn = max(kx, 1), max(ky, 1), max(kz, 1)
   kxs, kys, kzs = (kx==0 ? nx : (kx/2)), (ky==0 ? ny : (ky/2)), (kz==0 ? nz : (kz/2))
	N = nx * ny * nz
	K = zerovecs(N)
	weight = zeros(N)

	# evaluate K and weight
   #   TODO: make this loop use vectors more efficiently
   for k1 = 1:nx, k2 = 1:ny, k3 = 1:nz
      # TODO: use `sub2ind` here
      k = k1 + (k2-1) * nx + (k3-1) * nx * ny
      @assert k == sub2ind((nx, ny, nz), k1, k2, k3)
      # check when kx==0 or ky==0 or kz==0
      K[k] = (k1 - kxs) * b1/nx + (k2 - kys) * b2/ny + (k3 - kzs) * b3/nz
      weight[k] = 1.0 / ( nx * ny * nz )
   end

    return K, weight
end

# ================ BZ iterators ================

# basic iterator for BZ zone integration
#  for (w, k) in quadrule
#       ...

Base.start(q::BZQuadratureRule) = (w_and_pts(q), 1)
Base.done(q::BZQuadratureRule, state) = (state[2] > length(q))
Base.next(::BZQuadratureRule, state) =
   (state[1][1][state[2]], state[1][2][state[2]]), (state[1], state[2]+1)

# we can replace the double-loop over bz and then over the eigenvalues with
#
#   for (w, k, e, ψ) in BZiter(tbm)
#       ...
# (if ψ is a view of C_k[:, s] then this doesn't require allocation either)

"""
`BZiter`: replaces the double-loop
```
update!(at, tbm)
for (w, k) in bzquad
   for s = 1:length(w)
```
with a single-loop
```
for (w, k, ϵ, ψ) in BZiter(tbm, at)
```
Note in particular that it calls `update!` at the beginning of the
loop to precompute all the k-arrays.
"""
mutable struct BZiter
   tbm::TBModel
   at::AbstractAtoms
   w::Vector{Float64}
   k::Vector{JVecF}
end

mutable struct BZstate
   ik::Int    # index into the w, k arrays in BZ
   is::Int    # index into the epsn_k and C_k arrays in BZstate
   epsn_k::Vector{Float64}
   C_k::Matrix{Complex128}
end

BZiter(tbm::TBModel, at::AbstractAtoms) = BZiter(tbm, at, w_and_pts(tbm.bzquad)...)

Base.start(::BZiter) = BZstate(1, 1, Vector{Float64}(0), Matrix{Complex128}(0,0))

Base.done(bz::BZiter, state::BZstate) = (state.ik > length(bz.w))

function Base.next(bz::BZiter, state::BZstate)
   is, ik = state.is, state.ik
   if is == 1
      if ik == 1; update!(bz.at, bz.tbm); end
      state.epsn_k = get_k_array(bz.at, :epsn, bz.k[ik])
      state.C_k = get_k_array(bz.at, :C, bz.k[ik])
   end
   # construct the next item to return to the iteration
   nextitem = (bz.w[ik], bz.k[ik], state.epsn_k[is], state.C_k[:, is])
   # update the state
   state.is += 1
   if state.is > length(state.epsn_k)
      state.is = 1
      state.ik += 1
   end
   return nextitem, state
end


# This script contains some code for symbolic manipulation of linear least-squares problems. It can be used to derive or verify matrix expressions associated with problems on the form Ax = y
using SymPy
using SymPy: expand, coeff
SymPy.Sym(name, size::Tuple;kwargs...) = Sym[symbols("$name$i$j";kwargs...) for i = 1:size[1], j=1:size[2]]
skewcoords(R) = [R[3,2];R[1,3];R[2,1]]
twistcoords(xi) = [xi[1:3, 4]; skewcoords(xi[1:3, 1:3])]
skew(s) = [0 -s[3] s[2];s[3] 0 -s[1]; -s[2] s[1] 0]
trinv(T) = [T[1:3,1:3]' -T[1:3,1:3]'*T[1:3,4];0 0 0 1]

## Calib NAXP ==============================================================================
# N' * H*Pf = norm(N)^2

F = Sym("F",(3,3),real=true)
d = Sym("d",(3,1),real=true)
N =  Sym("N",(3,1),real=true)
P =  Sym("P_",(3,1),real=true)

RA = Sym("RA",(3,3), real=true)
TA = Sym("TA",(3,1), real=true)
RX = Sym("RX",(3,3), real=true)
TX = Sym("TX",(3,1), real=true)
X = [RX TX;0 0 0 1]
A = [RA TA;0 0 0 1]
Ps = [P;1]
N0 = [N;0]

xh = symbols("xh", real=true)
yh = symbols("yh", real=true)

# H = F'[[1 0;0 1;0 0], -d];

w = X[1:3,[1, 2, 4]]
w = w[:]
Prb = A*(X*Ps)
eq1 = N'*Prb[1:3]
eq = eq1[:] # Vector of interesting equations,
# terms of order zeros should be placed on RHS

AA     = Array{Sym}(length(eq), length(w))
BB     = Array{Sym}(length(eq), 1)
AAz    = falses(size(AA))

for i = 1:length(eq)
    for j = 1:length(w)
        AA[i,j] = coeff(expand(eq[i]),w[j]) # Find all coefficients of parameter j in eq i
    end
end

println("Trying to find expressions common in all terms")
change,res  = cse(AA[:]);
res         = reshape(res,size(AA));

byhand = (reshape(repmat((N'A[1:3,1:3]),3,1)',9,1) .*[reshape(repmat(P[1:2],1,3)',6,1);1;1;1])
byhand2 = [N'A[1:3,1:3]*Ps[1] N'A[1:3,1:3]*Ps[2] N'A[1:3,1:3]]

@assert all(simplify.(byhand) .== simplify.(byhand2[:]))
@assert all(simplify.(byhand) .== simplify.(AA[:]))


"Utility functions"

using Zygote: gradient

function Jacobian(F, x)
    y = F(x)
    n = length(y)
    m = length(x)
    T = eltype(y)
    j = Array{T, 2}(undef, n, m)
    for i in 1:n
        j[i, :] .= gradient(x -> F(x)[i], x)[1]
    end
    return j
end

function wMatrix(γ, order)
    mW = huge*Matrix{Float64}(I, order, order)
    mW[1, 1] = γ
    return mW
end

function transition(γ, order)
    V = zeros(order, order)
    V[1] = 1/γ
    return V
end

function shift(dim)
    S = Matrix{Float64}(I, dim, dim)
    for i in dim:-1:2
           S[i,:] = S[i-1, :]
    end
    S[1, :] = zeros(dim)
    return S
end

function uvector(dim, pos=1)
    u = zeros(dim)
    u[pos] = 1
    return dim == 1 ? u[pos] : u
end

function defineCS(order)
    uvector(order), shift(order)
end


using AlmostBandedMatrices

n = 10
r = 2
V = randn(n,r)
A = A_old = SemiSeparableMatrix(LowRankMatrix(randn(n,r), randn(r,r), V),
                    LowRankMatrix(randn(n,r), randn(r,r), V),0,0)

A = deepcopy(A_old)
      A₁ = A - (A[:,1]*A[1,:]')/A[1,1]

A = deepcopy(A_old)
      v = A[:,1];
      v ./= v[1]
      v .*= 1
      v[1] = 0
      L = I - v*[1 zeros(1,n-1)]
      L*A

A = deepcopy(A_old)
      # A.L.Σ .= A.L.Σ*(I - A.L.V[1,:]*(A.U.Σ'*A.U.U[1,:])'/A[1,1])
      A.L.Σ .= A.L.Σ*(I - A.L.V[1,:]*A.U.U[1,:]'*A.U.Σ/A[1,1])
      A
A = deepcopy(A_old)
A.L.Σ*(I - A.L.V[1,:]*(A.U.Σ*A.U.U[1,:])'/A[1,1])

A.L.V[1,:]*A.U.U[1,:]'
A.L.V[1,:]
A.U.U[1,:]
A[2,1]

A.L[2,1]

A[2,1]

A.L[2,1]

A.L.U*A.L.Σ*A.L.V[1,:]

A - (A.L.U*A.L.Σ*A.L.V[1,:]*A[1,:]')/A[1,1]    - A₁
A - (A.L.U*A.L.Σ*A.L.V[1,:]*(A.U.U[1:1,:]*A.U.Σ*A.U.V'))/A[1,1] - A₁
(A.L.U*A.L.Σ*A.L.V') - (A.L.U*A.L.Σ*A.L.V[1,:]*(A.U.V*A.U.Σ'*A.U.U[1,:])')/A[1,1] - A₁
A.L.U*A.L.Σ*(I - A.L.V[1,:]*(A.U.Σ'*A.U.U[1,:])'/A[1,1])*A.U.V'- A₁
A.L.V[1,:]
(A.L.V')

(I - A.L.V[1,:]*(A.U.Σ*A.U.U[1,:])'/A[1,1])

(I - A.L.V[1,:]*(A.U.Σ*A.U.U[1,:])'/A[1,1])

A.L.V[1,:]

(A.U.U[1:1,:]*A.U.Σ*A.U.V')
(A.U.V*A.U.Σ*A.U.U[1,:])
(A.U.U[1:1,:]*A.U.Σ*A.U.V')

A[1,:]
A.L.U

A.L.U*A.L.Σ*A.L.V[1,:]

A = deepcopy(A_old)
      v = A[:,1];
      v ./= v[1]
      v .*= 1
      v[1] = 0
      v
      A.L.V[1,:] ./= A[1,1]
       # A.L.V[2:n,:] .-= (A.U.U[1:1,:] * A.U.Σ * A.U.V[2:n,:]') .* A.L.V[1,1]
       A.L.V[2:n,1] .-= A.U.U[1,1] * A.U.Σ[1] * A.L.V[2:n,1].* A.L.V[1,1]
       A.L.V[2:n,2] .-= A.U.U[1,2] * A.U.Σ[2] * A.L.V[2:n,2].* A.L.V[1,1]
      # A.L.V[2:n,1] .-= A.U.U[1,1] * A.U.Σ[1] * A.L.V[2:n,1].* A.L.V[1,1]
      # A.L.Σ[1] .*= (1 - A.U.U[1,1] * A.U.Σ[1] * A.L.V[1,1])
      A

A = randn(n,n)
      v = A[:,1];
      v ./= v[1]
      v .*= 1
      v[1] = 0
      L = I - v*[1 zeros(1,n-1)]
      L*A

A - (A[:,1]*A[1,:]')/A[1,1]

A - (A[:,1]*[A[1,1] zeros(1,n-1)])/A[1,1]

(A.U.U[1:1,:] * A.U.Σ * A.U.V[2:n,:]')

A.L.V[2:n,:]
A.L.V[1,1]

(A.U.U[1:1,:] * A.U.Σ * A.U.V[2:n,:]')

(A.U.U[1,:]' * A.U.Σ * A.U.V[2:n,:]')
A.U.U[1,:]

A.U.U[1,:]' * A.U.Σ * A.V

L = I - v*[1 zeros(1,n-1)]
      L*A_old

A

A = deepcopy(A_old)
  v = A[:,1];
  v ./= v[1]
  v .*= 1
  v[1] = 0
  A11 = A[1,1]
  A.L.Σ .-= A.U.Σ
  A.L.V[1,1] ./= A[1,1]

  A.L.V

L = I - v*[1 zeros(1,n-1)]
  A - L*A_old

A.U.V

A.L.V[2:n,:] .-= A.U.U[1,1] .* A.L.V[1,1] .* A.U.V[2:n,:]

A = deepcopy(A_old)
   v = A[:,1];
   v ./= v[1]
   v .*= 1
   v[1] = 0
   v
   A.L.V[1,:] ./= A[1,1]
   for j = 2:n
      A.L.V[j,:] .-= A[1,j]*A.L.V[1,:]
   end
   L = I - v*[1 zeros(1,n-1)]
   A - L*A_old

A = deepcopy(A_old)
      v = A[:,1];
      v ./= v[1]
      v .*= 1
      v[1] = 0
      v
      A.L.V[1,:] ./= A[1,1]
      # A.L.V[2:n,:] .-= A.U.U[1,1] * A.U.Σ[1] * A.U.V[2:n,1].* A.L.V[1,1]
      # A.L.V[2:n,1] .-= A.U.U[1,1] * A.U.Σ[1] * A.L.V[2:n,1].* A.L.V[1,1]
      A.L.Σ[1] .*= (1 - A.U.U[1,1] * A.U.Σ[1] * A.L.V[1,1])
      L = I - v*[1 zeros(1,n-1)]
      A - L*A_old

L*A_old

A

A.U.U[1,1] * A.U.Σ[1] .* A.L.V[1,1]

A.L.Σ[1]

A.L.V

A[1,2:n]

A = deepcopy(A_old)
   v = A[:,1];
   v ./= v[1]
   v .*= 1
   v[1] = 0
   v
   A.L.V[1,:] ./= A[1,1]
   A.L.V[2:n,:] .-= A.U.U[1,1] .* A.L.V[1,1] .* A.U.V[2:n,:]
   A
   L = I - v*[1 zeros(1,n-1)]
   A - L*A_old

A.L.V[2:n,:]

A.U.V[:,1]

A.U.U[1,:] .* A.L.V[1,:]
A.L.V[1,:]
A.U.U

v

v

A

j = 1

n = 100;
   u = randn(n); v = randn(n);
   w = randn(n); z = randn(n);

   A = triu(u*v') + tril(w*z')
   v = A[:,1];
   v ./= v[1]
   v[1] = 0
   L = (I - v*[1 zeros(1,length(v)-1)])


@testset "Test SplineSparse" begin

    @testset "full" begin
        s = SplineSparse(1, 2, 3, 1:4, 1:2)
        want = [1 2 0; 0 3 4]
        @test full(s) == want

        s = SplineSparse(2, 2, 6, 1:8, [1, 5, 2, 4])
        want = [1 2 0 0 3 4
                0 5 6 7 8 0]

        @test full(s) == want
    end

    @testset "row_kron" begin
        s1 = SplineSparse(1,2, 3, rand(1:10, 12), rand(1:2, 6))
        s2 = SplineSparse(1,2, 4, rand(1:10, 12), rand(1:3, 6))
        want = row_kron(full(s1), full(s2))
        s12 = row_kron(s1, s2)

        @test full(s12) == want
        @test full(row_kron(s12, s1)) == row_kron(full(s12), full(s1))
        @test full(row_kron(s12, s12)) == row_kron(full(s12), full(s12))

        Base.zero(::Type{String}) = ""

        s1 = SplineSparse(1,2, 2, ["a", "b", "c", "d"], [1, 1])
        s2 = SplineSparse(1,3, 3, map(string, 1:6), [1, 1])

        want = ["a1" "a2" "a3" "b1" "b2" "b3"
                "c4" "c5" "c6" "d4" "d5" "d6"]
        have = row_kron(s1, s2)

        @test full(have) == want

    end

    @testset "getindex" begin
        s = SplineSparse(1, 2, 10, rand(12), rand(1:9, 6))
        full_s = full(s)

        for r in 1:size(s, 1)
            for c in 1:size(s, 1)
                @test s[r, c] == full_s[r, c]
            end
        end
    end

    @testset "*" begin
        s1 =  SplineSparse(1, 2, 3, 1:4, [1, 2])
        s2 =  SplineSparse(1, 3, 4, 1:6, [2, 1])
        s12 = SplineSparse(2, 3, 12, [1, 2, 3, 2, 4, 6, 12, 15, 18, 16, 20, 24],
                           [2, 6, 5, 9])

        x1 = rand(3)
        x2 = rand(4)
        x12 = rand(12)

        @test s1*x1 == full(s1)*x1
        @test s2*x2 == full(s2)*x2
        @test s12*x12 == full(s12)*x12
    end

end