module OncoDiversity

using DataFrames
using CSV
using Query
using Statistics
using Dates
import DataFrames: rename!

export diversity, calcOverQ, findInflection, findInflectionLocal, reportInflection, calcIPdiversity, patientdiversity,
  rename!, requirecol, countframe, countframefull, summarize, filteredDataset,
  Dataset, CDR3SeqData,
  AbstractAnalysis, CDR3SeqAnalysis,
  DiversityScores

abstract type AbstractAnalysis end
struct CDR3SeqAnalysis <: AbstractAnalysis end
struct SCRNASeqAnalysis<: AbstractAnalysis end
struct FlowCytoAnalysis <: AbstractAnalysis end
struct CytokineAnalysis <: AbstractAnalysis end
struct SomaScanAnalysis <: AbstractAnalysis end

abstract type Dataset end

"""    CDR3SeqData{T,U}

store two data tables that make up a CDR3 Dataset,
the per (patient, sequence) count and the physiochemisty data for each sequence.
These tables can be joined together usind `DataFrame(ds::CDR3SeqData)`.

"""
struct CDR3SeqData{T,U} <: Dataset
  counts::T
  physiochem::U
end

"""    diversity(seq, q)

Computes the generalized diversity index for a given value of q and the associated frequencies of the dataset.

```math
  ^qD = \\left( \\sum_i^n (p_i^q) \\right)^{1/(1-q)}
```
where p_i are the frequency across types and q is the order of diversity and you are summing over all types present in the dataset.

"""
function diversity(seq, q)
  0 <= q || error("q=$(q) must be greater than zero")

  if(q == 1.0)
    return exp(-sum(seq .* log.(seq)))
  else
    return sum(seq.^q)^(1/(1-q))
  end

end

"""    calcOverQ(origDF::DataFrame,rangeQ::Array{Float64})

Computes the diversity score over a range of q values. Returns a DataFrame where each column is a different q value and each row is a different patient.

Input: DataFrame of frequencies per patient and range of q
Output: DataFrame of diversity scores across the range of q per patient

"""
function calcOverQ(origDF::DataFrame,rangeQ::Array{Float64})
  qDdf = DataFrame(patient=unique(origDF[:,:patient]))

  for q in rangeQ
      tempDF = patientdiversity(origDF,q);
      qDdf[:,Symbol(q)]=tempDF[:,:diversity]
  end

  return qDdf
end

"""    findInflection(q::Array, qD::Array)

Computes the inflection point of a sequence numerically.

"""
function findInflection(q::Array, qD::Array)
  approxLogDeriv = q[2:end].*diff(qD)./diff(q);
  return approxLogDeriv;
end

"""    reportInflection(q::Array, qD::Array)

Computes the inflection point and the magnitude of the slope at that point using findInflection and findInflectionLocal functions.

"""
function reportInflection(q::Array, qD::Array)
  diffVector = findInflection(q, qD)
  inflectPt = argmin(diffVector)
  slopeInflectPt = abs(diffVector[inflectPt])
  qInflecPt = q[inflectPt+1]
  return (qInflecPt, slopeInflectPt)
end

"""    calcIPdiversity(df::DataFrame, qrange::Array)

Computes diversity over a range of q values (qrange) and an input DataFrame of frequencies across types, then numerically determines the inflection point and the magnitude of the slope at that point, and returns a DataFrame with a column for patient name, a column for q at which the inflection point occurs, and a column for the magnitude of the slope at the inflection point.

Input: DataFrame and array of q values
Output: DataFrame of inflection point q and slope diversity per patient

"""
function calcIPdiversity(df::DataFrame, qrange::Array)

  # set up new dataframe to capture the inflection point (InfPt) and slope at the inflection point (InflPtS)
  newDF = DataFrame(
            patient=unique(df[:,:patient]),
            InflPt=zeros(size(unique(df[:,:patient]))[1]),
            InflPtS=zeros(size(unique(df[:,:patient]))[1]))

  # calculate qD over a supplied range of q's
  dfRangeQ = calcOverQ(df,qrange)

  # calculate the IP & slope and fill into the dataframe
  counter = 1;
  for r in eachrow(dfRangeQ)

      qDs = collect(r[2:end])

      (IP,IPslope) = reportInflection(qrange,qDs)
      newDF[counter,:InflPt] = IP
      newDF[counter,:InflPtS] = IPslope
      counter = counter+1;
  end

  return newDF
end

"""    patientdiversity(countframe::DataFrame,q::Real)

Computes a DataFrame of diversity scores across patients from a countframe DataFrame and a single value of q.

Input: DataFrame and a real number
Output: DataFrame

"""
function patientdiversity(countframe::DataFrame,q::Real)
  #@show totUniq = length(unique(df.CDR3))

  diversityframe = @from i in countframe begin
          @group i by i.patient into g
          @select {
              patient=first(g.patient),
              diversity=diversity(g.count ./ sum(g.count), q),
              numclust = length(g.count),
              checkrange = length(g.count) > diversity(g.count ./ sum(g.count), q)
          }
      @collect DataFrame
  end
  return diversityframe
end

"""    requirecol(df::DataFrame, colname::AbstractString)

Raises an assertion error if colname is not a column of DataFrame.

"""
function requirecol(df::DataFrame, colname::AbstractString)
  @assert colname in names(df) "Expected column named: $colname, names are $(names(df))"
end

rename!(df::DataFrame, s::String, t::Symbol) = rename!(df, Symbol(s)=>t)

"""    countframe(a::CDR3SeqAnalysis, typeDF::DataFrame)

Counts the number of occurances of CDR3 by patients, after checking that patient and CDR3 columns are present in the DataFrame, and returns as a DataFrame of the counts.

Input: CDR3SeqAnalysis struct and DataFrame of CDR3 recoveries
Output: DataFrame

"""
function countframe(a::CDR3SeqAnalysis, typeDF::DataFrame)
  ## checks that you have a column of individual identifies and CDR3 sequences
  requirecol(typeDF, "patient")
  requirecol(typeDF, "CDR3")

  ## check if there is a column called abundances -- for cases where you do not need to count individual reads

  ## try seeing if there is a column called abundance
  ## otherwise assumes that 1 row in the dataframe = 1 read
  try
    requirecol(typeDF, "abundance")
    ## if requirecol not satisfied, will jump to catch

    countDF = @from i in typeDF begin
      @where i.abundance > 0
      @select {
        patient=i.patient,
        CDR3=i.CDR3,
        count=i.abundance
      }
      @collect DataFrame
    end;

  catch
    countDF = @from i in typeDF begin
      @group i by i.patient, i.CDR3 into g
      @select {
          patient=first(g.patient),
          CDR3=first(g.CDR3),
          count=length(g.CDR3)
      }
      @collect DataFrame
    end;
  end

  #return countDF

end

"""    countframefull(a::CDR3SeqAnalysis, df::DataFrame)

Merges df, a DataFrame of counts, with all the physiochemical information associated with the CDR3 sequences.

Input: CDR3SeqAnalysis struc and DataFrame
Output: DataFrame

"""
function countframefull(a::CDR3SeqAnalysis, df::DataFrame)
  rename!(df, "Filename", :patient)
  return innerjoin(countframe(CDR3SeqAnalysis(), df), df, on=[:patient, :CDR3])
end

function CDR3SeqData(df::DataFrame)
  requirecol(df, "CDR3")
  try
    requirecol(df, "patient")
  catch AssertionError
    requirecol(df, "Filename")
    rename!(df, "Filename", :patient)
  end
  return CDR3SeqData(
    countframe(CDR3SeqAnalysis(), df),
    select(df, Not(:patient)) |> @groupby(_.CDR3) |> x-> map(first, x) |> DataFrame
  )
end

DataFrame(data::CDR3SeqData) = innerjoin(data.counts, data.physiochem, on=[:CDR3])

"""    summarize(df::CDR3SeqData)

Computes a summary of the CDR3 sequences recovered in the dataset per patient including the total number of recoveries, minimum number on recoveries per sequence, maximum number of recoveries per sequence, average number of recoveries per sequence, median number of recoveries per sequence, and number of unique CDR3 recoveries.

Input: CDR3SeqData struct
Output: DataFrame

"""
function summarize(df::CDR3SeqData)
  statframe = @from i in df.counts begin
  @group i by i.patient into g
  @select {
      patient=first(g.patient),
      total_count=sum(g.count),
      min_count = minimum(g.count),
      max_count = maximum(g.count),
      avg_count = mean(g.count),
      median_count = median(g.count),
      num_unique_CDR3 = length(g.CDR3)
  }
  @collect DataFrame
  end

  return statframe

end

"""    filteredDataset(::Type{CDR3SeqData}, df::DataFrame, receptorname::String)

Subsets the dataset based on a receptor type of interest

Input: CDR3SeqData type, a DataFrame of counts, and a string that matches the types of the dataset
Output: DataFrame

Example: df_filtered = filteredDataset(CDR3SeqData, df, "TRA")

"""
function filteredDataset(::Type{CDR3SeqData}, df::DataFrame, receptorname::String)

  ## subset the type group only
  recepframe = @from i in df begin
    @where i.Receptor == receptorname
    @select i
    @collect DataFrame
  end;

  println("Number of recoveries: ", size(recepframe))
  println("Cohort size: ", length(unique(recepframe[:,:Filename])))

  ds = CDR3SeqData(recepframe)

  return ds
end

"""    filteredDataset(::Type{CDR3SeqData}, df::DataFrame, receptorname::Vector{String})

Subsets the dataset based on receptor types of interest, allowing users to subset based on combintations of types.

Input: CDR3SeqData type, a DataFrame of counts, and a string that matches the types of the dataset
Output: DataFrame

Example: df_filtered = filteredDataset(CDR3SeqData, df, ["TRA","TRB"])

"""
function filteredDataset(::Type{CDR3SeqData}, df::DataFrame, receptorname::Vector{String})
  ## subset the type group only
  recepframe = @from i in df begin
    @where i.Receptor in receptorname
    @select i
    @collect DataFrame
  end;

  println("Number of recoveries: ", size(recepframe))
  println("Cohort size: ", length(unique(recepframe[:,:Filename])))

  ds = CDR3SeqData(recepframe)

  return ds
end

"""    DiversityScores{T}

Stores a dataframe of diversity scores along with the range of qs that it was computed over.
"""
struct DiversityScores{T} <: Dataset
  qs::T
  data::DataFrame
end

DataFrame(ds::DiversityScores) = ds.data

"""    DiversityScores(countDF::DataFrame, rangeQ::AbstractVector, samplecol=:patient)

Computes point estimates of diversity from a countframe DataFrame, range of q values, and optional sample column identifier.

Point estimates of diversity of interest:
* low q diversity (q=0.01)
* high q diversity (q=100)
* delta qD diversity = low q diversity - high q diversity
* inflection point q
* magnitude of slope at the inflection point
* diversity qD evaluated at q=inflection point q
* Shannon diversity (log qD at q=1)
* Simpson diversity (1/qD at q=2)

Input: DataFrame of counts, vector of q values, optional sample column symbol (defaults to :patient)
Output: DiversityScores struct

"""
function DiversityScores(countDF::DataFrame, rangeQ::AbstractVector, samplecol=:patient)

  ## richness and evenness measures of diversity
  lowDiv = patientdiversity(countDF,rangeQ[1]);
  ids = lowDiv[:, samplecol];
  highDiv = patientdiversity(countDF,rangeQ[end]);

  ## inflection point related diversity metrics
  #    calcIPdiversity returns the q value of the inflection point & the slope at the inflection point
  dfIPslope = calcIPdiversity(countDF,rangeQ);
  # calculating the diversity qD at q=IP q value
  DatIPqDiv = Array{Float64}(undef,size(dfIPslope)[1])
  for row in 1:size(dfIPslope)[1]
    tmp = patientdiversity(countDF,dfIPslope[row,:InflPt]);
    DatIPqDiv[row,1] = tmp[row,:diversity]
  end

  ## commmonly used metrics of diversity
  ShanDiv = patientdiversity(countDF,1.0);
  SimpDiv = patientdiversity(countDF,2.0);

  ## compile all diversity scores into a single dataframe
  df = DataFrame(patient=ids,
     lowQ=lowDiv[:,:diversity],
     highQ=highDiv[:,:diversity],
     deltaqD=(lowDiv[:,:diversity].-highDiv[:,:diversity]),
     IPq=dfIPslope[:,:InflPt],
     IPslope=dfIPslope[:,:InflPtS],
     qDatIPq=DatIPqDiv,
     Shan=log.(ShanDiv[:,:diversity]),
     Simp= 1.0 ./(SimpDiv[:,:diversity]));
  return DiversityScores(rangeQ, df)
end

# function foo(c::SCRNASeqAnalysis, df::DataFrame)
#   #...SCRNASeq code here
# end

end


function cyclic_paths(n)
    n = (n - 3) % 28960854
    c = BigInt(8)
    d = BigInt(8)

    for i in 1:n
        c = (3 * c)^3 % 13^8
        d = (3 * d)^3 % (6 * 13^8)
    end

    return d, c
end

function main()
    n = 10000
    local c

    for i in 1:3
        n, c = cyclic_paths(n)
    end

    println(c)
end

main()


export ExplicitOp

mutable struct ExplicitOp{T,F1<:FuncOrNothing,F2<:FuncOrNothing} <: AbstractLinearOperator{T,Function,F1,F2}
  nrow :: Int
  ncol :: Int
  symmetric :: Bool
  hermitian :: Bool
  prod :: Function
  tprod :: F1
  ctprod :: F2
end

"""
    ExplicitOp(shape::NTuple{D,Int64}, tr::Trajectory, correctionmap::Array{ComplexF64,D}
            ; echoImage::Bool=false, kargs...) where D

generates a `ExplicitOp` which explicitely evaluates the MRI Fourier signal encoding operator.

# Arguments:
* `shape::NTuple{D,Int64}`             - size of image to encode/reconstruct
* `tr::Trajectory`                     - Trajectory with the kspace nodes to sample
* `correctionmap::Array{ComplexF64,D}` - fieldmap for the correction of off-resonance effects
* `echoImage::Bool=false`              - if true sampling times will only be considered relative to the echo time
                                         this results in complex valued image even for real-valued input.
* `kargs`                              - additional keyword arguments
"""
function ExplicitOp(shape::NTuple{D,Int64}, tr::Trajectory, correctionmap::Array{ComplexF64,D}
                        ; echoImage::Bool=false, kargs...) where D

  nodes,times = kspaceNodes(tr), readoutTimes(tr)
  nrow = size(nodes,2)
  ncol = prod(shape)

  # if echo image is desired echo time is needed as an offset
  if echoImage
      echoOffset = echoTime(tr)
  else
      echoOffset = 0.0
  end

  return ExplicitOp{ComplexF64,Nothing,Function}(nrow, ncol, false, false
            , x->produ(x, shape, nodes, times, echoOffset, correctionmap)
            , nothing
            , y->ctprodu(y, shape, nodes, times, echoOffset, correctionmap))
end

function produ(x::Vector{T}, shape::NTuple{2,Int64},
                    nodes::Matrix{Float64}, times::Vector{Float64}, echoOffset::Float64,
                    correctionmap::Matrix{ComplexF64}) where T<:Union{Real,Complex}

   if isempty(correctionmap)
       disturbanceTerm = zeros(ComplexF64,shape...)
   else
       disturbanceTerm = correctionmap
   end

   x= reshape(x,shape)
   out = zeros(ComplexF64,size(nodes,2))
   for nx=1:shape[1]
       for ny=1:shape[2]
           for k=1:size(nodes,2)
               phi = (nodes[1,k]*(nx-shape[1]/2-1)+
                      nodes[2,k]*(ny-shape[2]/2-1))
               e = exp(-2*1im*pi*phi - (times[k]-echoOffset)*disturbanceTerm[nx,ny])
               out[k] += x[nx,ny] * e
           end
       end
   end
   return vec(out)
end

function produ(x::Vector{T}, shape::NTuple{3,Int64},
                    nodes::Matrix{Float64}, times::Vector{Float64}, echoOffset::Float64,
                    correctionmap::Array{ComplexF64,3}) where T<:Union{Real,Complex}

   if isempty(correctionmap)
       disturbanceTerm = zeros(ComplexF64,shape...)
   else
       disturbanceTerm = correctionmap
   end

   x = reshape(x,shape)
   out = zeros(ComplexF64,size(nodes,2))
   for nx=1:shape[1]
       for ny=1:shape[2]
           for nz=1:shape[3]
               for k=1:size(nodes,2)
                   phi = (nodes[1,k]*(nx-shape[1]/2-1)+nodes[2,k]*(ny-shape[2]/2-1)+nodes[3,k]*(nz-shape[3]/2-1))
                   e = exp(-2*1im*pi*phi - (times[k]-echoOffset)*disturbanceTerm[nx,ny,nz])
                   out[k] += x[nx,ny,nz] * e
               end
           end
       end
   end
   return vec(out)
end

function ctprodu(x::Vector{T}, shape::NTuple{2,Int64},
                        nodes::Matrix{Float64}, times::Vector{Float64}, echoOffset::Float64,
                        correctionmap::Matrix{ComplexF64}) where T<:Union{Real,Complex}

  if isempty(correctionmap)
      disturbanceTerm = zeros(ComplexF64,shape...)
  else
      disturbanceTerm = correctionmap
  end

  out = zeros(ComplexF64,shape)
  for nx=1:shape[1]
      for ny=1:shape[2]
          for k=1:size(nodes,2)
              phi = (nodes[1,k]*(nx-shape[1]/2-1)+nodes[2,k]*(ny-shape[2]/2-1))
              e = exp(-2*1im*pi*phi - (times[k]-echoOffset)*disturbanceTerm[nx,ny])
              out[nx,ny] += x[k] * conj(e)
          end
      end
  end
  return vec(out)
end

function ctprodu(x::Vector{T}, shape::NTuple{3,Int64},
                        nodes::Matrix{Float64}, times::Vector{Float64}, echoOffset::Float64,
                        correctionmap::Array{ComplexF64,3}) where T<:Union{Real,Complex}

  if isempty(correctionmap)
      disturbanceTerm = zeros(ComplexF64,shape...)
  else
      disturbanceTerm = correctionmap
  end

  out = zeros(ComplexF64,shape)
  for nx=1:shape[1]
      for ny=1:shape[2]
          for nz=1:shape[3]
              for k=1:size(nodes,2)
                 phi = (nodes[1,k]*(nx-shape[1]/2-1)+nodes[2,k]*(ny-shape[2]/2-1)+nodes[3,k]*(nz-shape[3]/2-1))
                 e = exp(-2*1im*pi*phi - (times[k]-echoOffset)*disturbanceTerm[nx,ny,nz])
                 out[nx,ny,nz] += x[k] * conj(e)
              end
          end
      end
  end
  return vec(out)
end

function adjoint(op::ExplicitOp{T}) where T
  return LinearOperator{T,Function,Nothing,Function}(op.ncol, op.nrow, op.symmetric, op.hermitian,
                        op.ctprod, nothing, op.prod)
end


@testset "Check identifiability from given IO-equations" begin
    R, (x, y, z, t) = Nemo.PolynomialRing(Nemo.QQ, ["x", "y", "z", "t"])

    @test check_identifiability(
        x * (y + z) * t^4 - x^3 * y * z * t^5 + 7 + t * x,
        [x, y, z],
        0.99
    ) == [true, false, false]

    @test check_identifiability(
        [z + t * (x + y) + z^3, z - t^2 * x * y *5 + t^2],
        [x, y],
        [x, y, x + y, x - y, x^2 + y^2],
        0.99
    ) == [false, false, true, false, true]

end


using BenchmarkTools, SparseArrays, LinearAlgebra
using ExpmV

const SUITE = BenchmarkGroup()

dimensions = 2 .^ (5:10)
density = 10. .^ (-4 : -1)

for p in density
    SUITE[p] = BenchmarkGroup(["density"])
    for d in dimensions
        SUITE[p][d] = @benchmarkable ExpmV.expmv(rt,r,rv) setup=((rt,r,rv)=(rand(), sprandn($d,$d,$p/2)+1im*sprandn($d,$d,$p/2), normalize(randn($d)+1im*randn($d))))
    end
end


function NBodyProblem(potential,M,v0,u0,tspan,p=nothing; kwargs...)
    # check number of particles
    @assert length(u0) == length(v0)
    ind = i -> div(i-1, dim) + 1
    N   = length(u0)
    dim = div(N, length(M))
    @assert dim*length(M) == N
    @assert dim ∈ (2, 3)

    let fun = FWrapper{typeof(potential),typeof(tspan[1]),typeof(M),typeof(p)}(potential,tspan[1],M,p), cfg = ForwardDiff.GradientConfig(fun, u0)
        function acceleration!(dv,v,x,p,t)
            fun.t = t
            ForwardDiff.gradient!(dv, fun, x, cfg)
            for x in dv.x
                x ./= -M
            end
        end
        return SecondOrderODEProblem{true}(acceleration!, v0, u0, tspan, p; kwargs...)
    end
end

mutable struct FWrapper{F,T,MType,P}
    f::F
    t::T
    M::MType
    p::P
end
(f::FWrapper)(u) = f.f(f.p,f.t,u.x...,f.M)


using SatelliteToolbox
using Plots
gr()

#=
設定パラメータ
=#
DataNum =100 #シミュレータ反復回数
dt = 60*5 ##シミュレータの計算間隔 [s]
t = DatetoJD(2019, 11, 14, 07, 30, 00)   #シミュレート開始時刻＠ユリウス日

#開始時のみ処理
tles = read_tle("ISS_TLE.txt") #TLE読み込み
orbp = init_orbit_propagator(Val{:sgp4}, tles[1]) #SGP4での軌道モデルの読み込み
eop_IAU2000A = get_iers_eop(:IAU2000A)  #IAU-2000Aでの地球モデルの読み込み
epoch_JD = DatetoJD(2000+tles[1].epoch_year, 1, 1, 00, 00, 00) + tles[1].epoch_day  #TLEの元期＠ユリウス日
o, r, v = propagate!(orbp, t - epoch_JD)

#ループ毎処理関数定義
function SatPlaceCal(n)
    #=  衛星位置の赤道面座標系での計算　=#
    o, r, v = step!(orbp, dt)
    #=  衛星位置の地心直交座標系への変換  =#
    xg = rECItoECEF(GCRF(), ITRF(), t + n*dt/(24*60*60), eop_IAU2000A) * r
    vg = rECItoECEF(GCRF(), ITRF(), t + n*dt/(24*60*60), eop_IAU2000A) * v
    return xg, vg
end

function plus(a,b)
    c = a+b
    return c
end

x_ecef_log = zeros(100, 3)    #衛星位置＠地心直交座標系
x_geod_log = zeros(100, 3)    #衛星位置＠測地座標系
v_ecef_log = zeros(100, 3)    #衛星速度＠地心直交座標系
x_eci_log = zeros(100, 3)
#=
#シミュレーションループ
for i=1:DataNum
    x_sat, v_sat = SatPlaceCal(i)
    x_ecef_log[i,:] = x_sat
    v_ecef_log[i,:] = v_sat
    println(x_ecef_log[i,:])
    x_geo = ECEFtoGeodetic([-1*x_sat[1]; x_sat[2]; x_sat[3]])
    x_geod_log[i,1]=x_geo[1]
    x_geod_log[i,2]=x_geo[2]
    x_geod_log[i,3]=x_geo[3]
    println(x_geod_log[i,:])
end
=#

#=
#=************************************************
地心直交座標系での衛星位置を3Dプロットしたい場合はコレ
************************************************=#
println(x_ecef_log)

plot(x_ecef_log[:,1], x_ecef_log[:, 2], x_ecef_log[:, 3])
xlabel!("x")
ylabel!("y")
#zlabel!("z")

savefig("test2.png")
=#

#=
anim = @animate for i=1:DataNum
    time=epoch_JD + (i-1)*dt/(60*60*24)
    x_sat, v_sat = SatPlaceCal(i)
    x_ecef_log[i,:] = x_sat
    v_ecef_log[i,:] = v_sat
    x_sat = rECEFtoECI(ITRF(),GCRF(), time, eop_IAU2000A)*x_sat
    x_eci_log[i,:] = x_sat
    println(x_eci_log[i,:])
    plot(x_eci_log[1:i, 1], x_eci_log[1:i, 2])
end

gif(anim, "test7_anim.gif", fps = 10)
=#

#=***********************************************
緯度経度に変換してGifでグラフを作成したい場合はコレ
***********************************************=#
anim = @animate for i=1:DataNum
    x_sat, v_sat = SatPlaceCal(i)
    x_ecef_log[i,:] = x_sat
    v_ecef_log[i,:] = v_sat
    #plot(x_ecef_log[1:i,1], x_ecef_log[1:i, 2], x_ecef_log[1:i, 3])

    x_geo = ECEFtoGeodetic([x_ecef_log[i,1]; x_ecef_log[i,2]; x_ecef_log[i,3]])
    x_geod_log[i,1]=x_geo[1]
    if x_geo[2]>0
        x_geod_log[i,2]=x_geo[2]
    else
        x_geod_log[i,2]=x_geo[2]+2π
    end
    x_geod_log[i,3]=x_geo[3]
    plot(rad2deg.(x_geod_log[1:i,2]),rad2deg.(x_geod_log[1:i,1]))

end

gif(anim, "test5_anim.gif", fps = 50)


# Newton.

function optimize(f::Function, g::Function, H::Function,
                  x0::Vector{T}; kmax::Int64 = 5000, δ::Float64 = 1e-6) where {T<:Real}
    k::Int64 = 0
    δ *= δ
    x = x0
    n::Int64 = length(x)
    dfx = ones(n)
    d2fx = Matrix{Float64}(I, n, n)
    while norm(dfx) > δ && k < kmax
        dfx = g(x)
        d2fx = H(x)
        x -= d2fx \ dfx
        k += 1
    end
    return x, k
end


"""Biharmonic diffusion operator with constant viscosity coefficient
    Viscosity = ν∇⁴ ⃗u. Although constant, the coefficient is actually inside
    Viscosity = ∇⋅ν∇∇² ⃗u."""
function diffusion_constant!(dudx,dudy,dvdx,dvdy,DS,DS_q,DT,νSmag,νSmag_q,Lu,Lv,dLudx,dLudy,dLvdx,dLvdy,
        S11,S12,S21,S22,LLu1,LLu2,LLv1,LLv2,u,v)
    stress_tensor!(dLudx,dLudy,dLvdx,dLvdy,Lu,Lv,u,v)
    viscous_tensor_Constant!(S11,S12,S21,S22,dLudx,dLudy,dLvdx,dLvdy)

    ∂x!(LLu1,S11)
    ∂y!(LLu2,S12)
    ∂x!(LLv1,S21)
    ∂y!(LLv2,S22)
end

""" Smagorinsky-like biharmonic viscosity
    Viscosity = ∇ ⋅ (cSmag Δ⁴ |D| ∇∇² ⃗u)
The Δ⁴-scaling is omitted as gradient operators are dimensionless."""
function diffusion_Smagorinsky!(dudx,dudy,dvdx,dvdy,DS,DS_q,DT,νSmag,νSmag_q,Lu,Lv,dLudx,dLudy,dLvdx,dLvdy,
        S11,S12,S21,S22,LLu1,LLu2,LLv1,LLv2,u,v)
    ∂x!(dudx,u)
    ∂y!(dvdy,v)
    ∂x!(dvdx,v)
    ∂y!(dudy,u)

    # biharmonic diffusion
    stress_tensor!(dLudx,dLudy,dLvdx,dLvdy,Lu,Lv,u,v)
    Smagorinsky_coeff!(νSmag,νSmag_q,DS,DS_q,DT,dudx,dvdy,dudy,dvdx)
    viscous_tensor_Smagorinsky!(S11,S12,S21,S22,νSmag,νSmag_q,dLudx,dLudy,dLvdx,dLvdy)

    ∂x!(LLu1,S11)
    ∂y!(LLu2,S12)
    ∂x!(LLv1,S21)
    ∂y!(LLv2,S22)
end

"""νSmag = cSmag * |D|, where deformation rate |D| = √((∂u/∂x - ∂v/∂y)^2 + (∂u/∂y + ∂v/∂x)^2).
The grid spacing Δ is omitted here as the operators are dimensionless."""
function Smagorinsky_coeff!(νSmag,νSmag_q,DS,DS_q,DT,dudx,dvdy,dudy,dvdx)
    # horizontal shearing strain squared
    m,n = size(DS_q)
    @boundscheck (m+ep,n) == size(dudy) || throw(BoundsError())
    @boundscheck (m,n) == size(dvdx) || throw(BoundsError())

    @inbounds for j ∈ 1:n
        for i ∈ 1:m
            DS_q[i,j] = (dudy[i+ep,j] + dvdx[i,j])^2
        end
    end

    Ixy!(DS,DS_q)

    # horizontal tension squared
    m,n = size(DT)
    @boundscheck (m+ep,n+2) == size(dudx) || throw(BoundsError())
    @boundscheck (m+2,n) == size(dvdy) || throw(BoundsError())

    @inbounds for j ∈ 1:n
        for i ∈ 1:m
            DT[i,j] = (dudx[i+ep,j+1] + dvdy[i+1,j])^2
        end
    end

    # viscosity = Smagorinsky coefficient times deformation rate
    m,n = size(νSmag)
    @boundscheck (m,n) == size(DS) || throw(BoundsError())
    @boundscheck (m,n) == size(DT) || throw(BoundsError())

    @inbounds for j ∈ 1:n
        for i ∈ 1:m
            νSmag[i,j] = cSmag*sqrt(DS[i,j] + DT[i,j])
        end
    end

    Ixy!(νSmag_q,νSmag)
end

"""Biharmonic stress tensor ∇∇²(u,v) = (∂/∂x(∇²u), ∂/∂y(∇²u); ∂/∂x(∇²v), ∂/∂y(∇²v))"""
function stress_tensor!(dLudx,dLudy,dLvdx,dLvdy,Lu,Lv,u,v)
    ∇²!(Lu,u)
    ∇²!(Lv,v)
    ∂x!(dLudx,Lu)
    ∂y!(dLudy,Lu)
    ∂x!(dLvdx,Lv)
    ∂y!(dLvdy,Lv)
end

"""Biharmonic stress tensor times Smagorinsky coefficient
νSmag * ∇∇² ⃗u = (S11, S12; S21, S22)."""
function viscous_tensor_Smagorinsky!(S11,S12,S21,S22,νSmag,νSmag_q,dLudx,dLudy,dLvdx,dLvdy)
    m,n = size(S11)
    @boundscheck (m+2-ep,n) == size(νSmag) || throw(BoundsError())
    @boundscheck (m,n) == size(dLudx) || throw(BoundsError())

    @inbounds for j ∈ 1:n
        for i ∈ 1:m
            S11[i,j] = νSmag[i+1-ep,j] * dLudx[i,j]
        end
    end

    m,n = size(S12)
    @boundscheck (m,n) == size(νSmag_q) || throw(BoundsError())
    @boundscheck (m+ep,n) == size(dLudy) || throw(BoundsError())

    @inbounds for j ∈ 1:n
        for i ∈ 1:m
            S12[i,j] = νSmag_q[i,j] * dLudy[i+ep,j]
        end
    end

    m,n = size(S21)
    @boundscheck (m,n) == size(νSmag_q) || throw(BoundsError())
    @boundscheck (m,n) == size(dLvdx) || throw(BoundsError())

    @inbounds for j ∈ 1:n
        for i ∈ 1:m
            S21[i,j] = νSmag_q[i,j] * dLvdx[i,j]
        end
    end

    m,n = size(S22)
    @boundscheck (m,n+2) == size(νSmag) || throw(BoundsError())
    @boundscheck (m,n) == size(dLvdy) || throw(BoundsError())

    @inbounds for j ∈ 1:n
        for i ∈ 1:m
            S22[i,j] = νSmag[i,j+1] * dLvdy[i,j]
        end
    end
end

"""Biharmonic stress tensor times Smagorinsky coefficient
νSmag * ∇∇² ⃗u = (S11, S12; S21, S22)"""
function viscous_tensor_Constant!(S11,S12,S21,S22,dLudx,dLudy,dLvdx,dLvdy)
    m,n = size(S11)
    @boundscheck (m,n) == size(dLudx) || throw(BoundsError())

    @inbounds for j ∈ 1:n
        for i ∈ 1:m
            S11[i,j] = νB * dLudx[i,j]
        end
    end

    m,n = size(S12)
    @boundscheck (m+ep,n) == size(dLudy) || throw(BoundsError())

    @inbounds for j ∈ 1:n
        for i ∈ 1:m
            S12[i,j] = νB * dLudy[i+ep,j]
        end
    end

    m,n = size(S21)
    @boundscheck (m,n) == size(dLvdx) || throw(BoundsError())

    @inbounds for j ∈ 1:n
        for i ∈ 1:m
            S21[i,j] = νB * dLvdx[i,j]
        end
    end

    m,n = size(S22)
    @boundscheck (m,n) == size(dLvdy) || throw(BoundsError())

    @inbounds for j ∈ 1:n
        for i ∈ 1:m
            S22[i,j] = νB * dLvdy[i,j]
        end
    end
end

"""Update u with bottom friction tendency (Bu,Bv) and biharmonic viscosity."""
function add_drag_diff_tendencies!(u,v,Bu,Bv,LLu1,LLu2,LLv1,LLv2)
    m,n = size(u) .- (2*halo,2*halo)
    @boundscheck (m+2-ep,n+2) == size(Bu) || throw(BoundsError())
    @boundscheck (m,n+2) == size(LLu1) || throw(BoundsError())
    @boundscheck (m+2-ep,n) == size(LLu2) || throw(BoundsError())

    @inbounds for j ∈ 1:n
        for i ∈ 1:m
            u[i+2,j+2] += nΔt_diff*(Bu[i+1-ep,j+1] + LLu1[i,j+1] + LLu2[i+1-ep,j])
        end
    end

    m,n = size(v) .- (2*halo,2*halo) # cut off the halo
    @boundscheck (m+2,n+2) == size(Bv) || throw(BoundsError())
    @boundscheck (m,n+2) == size(LLv1) || throw(BoundsError())
    @boundscheck (m+2,n) == size(LLv2) || throw(BoundsError())

    @inbounds for j ∈ 1:n
        for i ∈ 1:m
             v[i+2,j+2] += nΔt_diff*(Bv[i+1,j+1] + LLv1[i,j+1] + LLv2[i+1,j])
        end
    end
end

if diffusion == "Constant"
    diffusive! = diffusion_constant!
elseif diffusion == "Smagorinsky"
    diffusive! = diffusion_Smagorinsky!
else
    throw(error("Diffusion not correctly specified. Only Constant or Smagorinsky allowed."))
end


abstract type StandardBasisPolynomial{T,X} <: AbstractPolynomial{T,X} end

function showterm(io::IO, ::Type{<:StandardBasisPolynomial}, pj::T, var, j, first::Bool, mimetype) where {T}

    if _iszero(pj) return false end

    pj = printsign(io, pj, first, mimetype)

    if !(_isone(pj) && !(showone(T) || j == 0))
        printcoefficient(io, pj, j, mimetype)
    end

    printproductsign(io, pj, j, mimetype)
    printexponent(io, var, j, mimetype)
    return true
end

"""
    evalpoly(x, p::StandardBasisPolynomial)
    p(x)

Evaluate the polynomial using [Horner's Method](https://en.wikipedia.org/wiki/Horner%27s_method), also known as synthetic division, as implemented in `evalpoly` of base `Julia`.

# Examples
```jldoctest
julia> using Polynomials

julia> p = Polynomial([1, 0, 3])
Polynomial(1 + 3*x^2)

julia> p(0)
1

julia> p.(0:3)
4-element Vector{Int64}:
  1
  4
 13
 28
```
"""
evalpoly(x, p::StandardBasisPolynomial) = EvalPoly.evalpoly(x, p.coeffs) # allows  broadcast  issue #209
constantterm(p::StandardBasisPolynomial) = p[0]

domain(::Type{<:StandardBasisPolynomial}) = Interval(-Inf, Inf)
mapdomain(::Type{<:StandardBasisPolynomial}, x::AbstractArray) = x

function Base.convert(P::Type{<:StandardBasisPolynomial}, q::StandardBasisPolynomial)
    if isa(q, P)
        return q
    else
        T = _eltype(P,q)
        X = indeterminate(P,q)
        return ⟒(P){T,X}([q[i] for i in 0:degree(q)])
    end
end

function Base.one(::Type{P}) where {P<:StandardBasisPolynomial}
    T,X = eltype(P), indeterminate(P)
    ⟒(P){T,X}(ones(T,1))
end
function variable(::Type{P}) where {P<:StandardBasisPolynomial}
    T,X = eltype(P), indeterminate(P)
    ⟒(P){T,X}([zero(T),one(T)])
end

## multiplication algorithms for computing p * q.
## default multiplication between same type.
## subtypes might relax to match T,S to avoid one conversion
function Base.:*(p::P, q::P) where {T,X, P<:StandardBasisPolynomial{T,X}}
    cs = ⊗(P, coeffs(p), coeffs(q))
    P(cs)
end

## put here, not with type defintion, in case reuse is possible
function ⊗(P::Type{<:StandardBasisPolynomial}, p::Vector{T}, q::Vector{S}) where {T,S}
    R = promote_type(T,S)
    fastconv(convert(Vector{R}, p), convert(Vector{R},q))
end

## Static size of product makes generated functions  a good choice
## from https://github.com/tkoolen/StaticUnivariatePolynomials.jl/blob/master/src/monomial_basis.jl
## convolution of two tuples
@generated function ⊗(::Type{<:StandardBasisPolynomial}, p1::NTuple{N,T}, p2::NTuple{M,S}) where {T,N,S,M}
    P = M + N - 1
    exprs = Any[nothing for i = 1 : P]
    for i in 1 : N
        for j in 1 : M
            k = i + j - 1
            if exprs[k] === nothing
                exprs[k] = :(p1[$i] * p2[$j])
            else
                exprs[k] = :(muladd(p1[$i], p2[$j], $(exprs[k])))
            end
        end
    end

    return quote
        Base.@_inline_meta
        tuple($(exprs...))
    end

end

function ⊗(P::Type{<:StandardBasisPolynomial}, p::Dict{Int,T}, q::Dict{Int,S}) where {T,S}
    R = promote_type(T,S)
    c = Dict{Int,R}()
    for (i,pᵢ) ∈ pairs(p)
        for (j,qⱼ) ∈ pairs(q)
            cᵢⱼ = get(c, i+j, zero(R))
            @inbounds c[i+j] = muladd(pᵢ, qⱼ, cᵢⱼ)
        end
    end
    c
end

## ---
function fromroots(P::Type{<:StandardBasisPolynomial}, r::AbstractVector{T}; var::SymbolLike = :x) where {T <: Number}
    n = length(r)
    c = zeros(T, n + 1)
    c[1] = one(T)
    for j in 1:n, i in j:-1:1
        c[(i + 1)] = c[(i + 1)] - r[j] * c[i]
    end
    #return P(c, var)
    if sort(r[imag(r).>0], lt = (x,y) -> real(x)==real(y) ? imag(x)<imag(y) : real(x)<real(y)) == sort(conj(r[imag(r).<0]), lt = (x,y) -> real(x)==real(y) ? imag(x)<imag(y) : real(x)<real(y))
        c = real(c)             # if complex poles come in conjugate pairs, the coeffs are real
    end
    return P(reverse(c), var)
end

function derivative(p::P, order::Integer = 1) where {T, X, P <: StandardBasisPolynomial{T, X}}
    order < 0 && throw(ArgumentError("Order of derivative must be non-negative"))

    # we avoid usage like Base.promote_op(*, T, Int) here, say, as
    # Base.promote_op(*, Rational, Int) is Any, not Rational in analogy to
    # Base.promote_op(*, Complex, Int)
    R = eltype(one(T)*1)
    Q = ⟒(P){R,X}

    order == 0 && return p
    hasnan(p) && return Q(R[NaN])
    order > length(p) && return zero(Q)
    d = degree(p)
    d <= 0 && return zero(Q)
    n = d + 1
    a2 = Vector{R}(undef, n - order)
    @inbounds for i in order:n - 1
        a2[i - order + 1] = reduce(*, (i - order + 1):i, init = p[i])
    end
    Q(a2)
end

function integrate(p::P) where {T, X, P <: StandardBasisPolynomial{T, X}}
    R = eltype(one(T)/1)
    Q = ⟒(P){R,X}

    hasnan(p) && return Q([NaN])
    iszero(p) && return zero(Q)

    n = length(p)
    as = Vector{R}(undef, n + 1)
    as[1] = zero(R)
    for (i, pᵢ) ∈ pairs(p)
        i′ = i + 1
        @inbounds as[i′+1] = pᵢ/i′
    end
    return Q(as)
end

function Base.divrem(num::P, den::Q) where {T, P <: StandardBasisPolynomial{T}, S, Q <: StandardBasisPolynomial{S}}

    assert_same_variable(num, den)
    X = indeterminate(num)

    n = degree(num)
    m = degree(den)

    m == -1 && throw(DivideError())
    if m == 0 && den[0] ≈ 0 throw(DivideError()) end

    R = eltype(one(T)/one(S))

    deg = n - m + 1

    if deg ≤ 0
        return zero(P), num
    end

    q_coeff = zeros(R, deg)
    r_coeff = R[ num[i-1] for i in 1:n+1 ]

    @inbounds for i in n:-1:m
        q = r_coeff[i + 1] / den[m]
        q_coeff[i - m + 1] = q
        @inbounds for j in 0:m
            elem = den[j] * q
            r_coeff[i - m + j + 1] -= elem
        end
    end
    resize!(r_coeff, min(length(r_coeff), m))

    return _convert(num, q_coeff), _convert(num, r_coeff)

end

"""
    gcd(p1::StandardBasisPolynomial, p2::StandardBasisPolynomial; method=:eculidean, kwargs...)

Find the greatest common divisor.

By default, uses the Euclidean division algorithm (`method=:euclidean`), which is susceptible to floating point issues.

Passing `method=:noda_sasaki` uses scaling to circumvent some of these.

Passing `method=:numerical` will call the internal method `NGCD.ngcd` for the numerical gcd. See the help page of [`Polynomials.NGCD.ngcd`](@ref) for details.
"""
function Base.gcd(p1::P, p2::Q, args...;
                  method=:euclidean,
                  kwargs...
                  ) where {T, P <: StandardBasisPolynomial{T}, Q <: StandardBasisPolynomial{T}}

    gcd(Val(method), p1, p2, args...; kwargs...)
end

function Base.gcd(::Val{:euclidean},
                  r₀::StandardBasisPolynomial{T}, r₁;
                  atol=zero(real(T)),
                  rtol=Base.rtoldefault(real(T)),
                  kwargs...) where {T}

    iter = 1
    itermax = length(r₁)

    while !iszero(r₁) && iter ≤ itermax
        _, rtemp = divrem(r₀, r₁)
        r₀ = r₁
        r₁ = truncate(rtemp; atol=atol, rtol=rtol)
        iter += 1
    end
    return r₀
end

Base.gcd(::Val{:noda_sasaki}, p, q; kwargs...) = gcd_noda_sasaki(p,q; kwargs...)

"""
     gcd_noda_sasaki(p,q; atol,  rtol)

Greatest common divisor of two polynomials.
Compute the greatest common divisor `d` of two polynomials `a` and `b` using
the Euclidian Algorithm with scaling as of [1].

References:

[1] M.-T. Noda and T. Sasaki. Approximate GCD and its application to ill-conditioned  algebraic equations. J. Comput. Appl. Math., 38:335–351, 1991.

Author: Andreas Varga

Note: requires Julia `v1.2` or greater.
"""
function  gcd_noda_sasaki(p::StandardBasisPolynomial{T}, q::StandardBasisPolynomial{S};
                          atol::Real=zero(real(promote_type(T,S))),
                          rtol::Real=Base.rtoldefault(real(promote_type(T,S)))
                          ) where {T,S}
    ⟒(typeof(p)) == ⟒(typeof(q)) ||  return gcd_noda_sasaki(promote(p,q);  atol=atol, rtol=rtol)
    ## check symbol
    a, b = coeffs(p), coeffs(q)
    as =  _gcd_noda_sasaki(a,b, atol=atol,  rtol=rtol)

    _convert(p, as)
end

function _gcd_noda_sasaki(a::Vector{T}, b::Vector{S};
              atol::Real=zero(real(promote_type(T,S))),
              rtol::Real=Base.rtoldefault(real(promote_type(T,S)))
              ) where {T,S}

    R = eltype(one(T)/one(S))

    na1 = findlast(!iszero,a) # degree(a) + 1
    na1 === nothing && return(ones(R, 1))

    nb1 = findlast(!iszero,b) # degree(b) + 1
    nb1 === nothing && return(ones(R, 1))

    a1 = R[a[i] for i in 1:na1]
    b1 = R[b[i] for i in 1:nb1]
    a1 ./= norm(a1)
    b1 ./= norm(b1)

    tol = atol + rtol

    # determine the degree of GCD as the nullity of the Sylvester matrix
    # this computation can be replaced by simply setting nd = 1, in which case the Sylvester matrix is not formed

    nd = na1 + nb1 - 2 - rank([NGCD.convmtx(a1,nb1-1) NGCD.convmtx(b1,na1-1)], atol = tol) # julia 1.1
    nd == 0 && (return [one(R)])

    sc = one(R)
    while na1 > nd
         na1 < nb1 && ((a1, b1, na1, nb1) = (b1, a1, nb1, na1))
         @inbounds for i in na1:-1:nb1
            s = -a1[i] / b1[nb1]
            sc = max(sc, abs(s))
            @inbounds for j in 1:nb1-1
                a1[i-nb1+j] += b1[j] * s
            end
        end
        a1 ./= sc
        na1 = findlast(t -> (abs(t) > tol),a1[1:nb1-1])
        na1 === nothing && (na1 = 0)
        resize!(a1, na1)
    end

    return nb1 == 1 ? [one(R)] : b1

end

Base.gcd(::Val{:numerical}, p, q, args...; kwargs...) = ngcd(p,q, args...; kwargs...).u

## --------------------------------------------------

function companion(p::P) where {T, P <: StandardBasisPolynomial{T}}
    d = length(p) - 1
    d < 1 && throw(ArgumentError("Series must have degree greater than 1"))
    d == 1 && return diagm(0 => [-p[0] / p[1]])

    R = eltype(one(T)/one(T))

    comp = diagm(-1 => ones(R, d - 1))
    ani = 1 / p[end]
    for j in  0:(degree(p)-1)
        comp[1,(d-j)] = -p[j] * ani # along top row has smaller residual than down column
    end
    return comp
end

function  roots(p::P; kwargs...)  where  {T, P <: StandardBasisPolynomial{T}}

    R = eltype(one(T)/one(T))
    d = degree(p)
    if d < 1
        return R[]
    end
    d == 1 && return R[-p[0] / p[1]]

    as = [p[i] for i in 0:d]
    K  = findlast(!iszero, as)
    if K == nothing
        return R[]
    end
    k =  findfirst(!iszero, as)

    k  == K && return zeros(R, k-1)

    # find eigenvalues of the  companion matrix
    comp  = companion(⟒(P)(as[k:K], indeterminate(p)))
    L = eigvals(comp; kwargs...)
    append!(L, zeros(eltype(L), k-1))

    L
end

function vander(P::Type{<:StandardBasisPolynomial}, x::AbstractVector{T}, n::Integer) where {T <: Number}
    A = Matrix{T}(undef, length(x), n + 1)
    A[:, 1] .= one(T)
    @inbounds for i in 1:n
        A[:, i + 1] = A[:, i] .* x
    end
    return A
end

## as noted at https://github.com/jishnub/PolyFit.jl, using method from SpecialMatrices is faster
## https://github.com/JuliaMatrices/SpecialMatrices.jl/blob/master/src/vandermonde.jl
## This is Algorithm 2 of https://www.maths.manchester.ac.uk/~higham/narep/narep108.pdf
## Solve V(αs)⋅x = y where V is (1+n) × (1+n) Vandermonde matrix (Vᵀ in the paper)
function solve_vander!(ys, αs)
    n = length(ys) - 1
    for k in 0:n-1
        for j in n:-1:k+1
            ys[1+j] = (ys[1+j] - ys[1+j-1])/(αs[1+j] - αs[1+j-k-1])
        end
    end
    for k in n-1:-1:0
        for j in k:n-1
            ys[1+j] = ys[1+j] - αs[1+k] * ys[1 + j + 1]
        end
    end

    nothing
end

# intercept one (typical) case for a faster variant
function fit(P::Type{<:StandardBasisPolynomial},
             x::AbstractVector{T},
             y::AbstractVector{T},
             deg::Integer = length(x) - 1;
             weights = nothing,
             var = :x,) where {T}

    if deg == length(x) -1 && weights == nothing
        _polynomial_fit(P, x, y; var=var)
    else
        _fit(P, x, y, deg; weights=weights, var=var)
    end
end

function _polynomial_fit(P::Type{<:StandardBasisPolynomial}, x::AbstractVector{T}, y; var=:x) where {T}
    R = float(T)
    coeffs = Vector{R}(undef, length(x))
    copyto!(coeffs, y)
    solve_vander!(coeffs, x)
    P(R.(coeffs), var)
end

## --------------------------------------------------
## More accurate fitting for higher degree polynomials

"""
    polyfitA(x, y, n=length(x)-1; var=:x)
    fit(ArnoldiFit, x, y, n=length(x)-1; var=:x)

Fit a degree ``n`` or less polynomial through the points ``(x_i, y_i)`` using Arnoldi orthogonalization of the Vandermonde matrix.

The use of a Vandermonde matrix to fit a polynomial to data is exponentially ill-conditioned for larger values of ``n``. The Arnoldi orthogonalization fixes this problem.

# Returns

Returns an instance of `ArnoldiFit`. This object can be used to evaluate the polynomial. To manipulate the polynomial, the object can be `convert`ed to other polynomial types, though there may be some loss in accuracy when doing polynomial evaluations afterwards for higher-degree polynomials.

# Citations:

The two main functions are translations from example code in:

*VANDERMONDE WITH ARNOLDI*;
PABLO D. BRUBECK, YUJI NAKATSUKASA, AND LLOYD N. TREFETHEN;
[arXiv:1911.09988](https://people.maths.ox.ac.uk/trefethen/vander_revised.pdf)

# Examples:

```
f(x) = 1/(1 + 25x^2)
N = 80; xs = [cos(j*pi/N) for j in N:-1:0];
p = fit(Polynomial, xs, f.(xs));
q = fit(ArnoldiFit, xs, f.(xs));
maximum(abs, p(x) - f(x) for x ∈ range(-1,stop=1,length=500)) # 3.304586010148457e16
maximum(abs, q(x) - f(x) for x ∈ range(-1,stop=1,length=500)) # 1.1939520722092922e-7

N = 250; xs = [cos(j*pi/N) for j in N:-1:0];
p = fit(Polynomial, xs, f.(xs));
q = fit(ArnoldiFit, xs, f.(xs));
maximum(abs, p(x) - f(x) for x ∈ range(-1,stop=1,length=500)) # 3.55318186254542e92
maximum(abs, q(x) - f(x) for x ∈ range(-1,stop=1,length=500)) # 8.881784197001252e-16

p = fit(Polynomial, xs, f.(xs), 10); # least-squares fit
q = fit(ArnoldiFit, xs, f.(xs), 10);
maximum(abs, q(x) - p(x) for x ∈ range(-1,stop=1,length=500)) # 4.6775083806238626e-14
Polynomials.norm(q-p, Inf) # 2.2168933355715126e-12 # promotes `q` to `Polynomial`
```

"""
function polyfitA(x, y, n=length(x)-1; var=:x)
    m = length(x)
    T = eltype(y)
    Q = ones(T, m, n+1)
    #H = UpperHessenberg(zeros(T, n+1, n))
    H = zeros(T, n+1, n)

    q = zeros(T, m)

    @inbounds for k = 1:n
        q .= x .* Q[:,k]
        for j in 1:k
            λ = dot(Q[:,j], q)/m
            H[j,k] = λ
            q .-= λ * Q[:,j]
        end
        H[k+1,k] = norm(q)/sqrt(m)
        Q[:,k+1] .= q/H[k+1,k]
    end
    d = Q \ y
    ArnoldiFit{eltype(d),typeof(H),Symbol(var)}(d, H)
end

function polyvalA(d, H::AbstractMatrix{S}, s::T) where {T, S}
    R = promote_type(T,S)
    n = length(d) - 1
    W = ones(R, n+1)
    @inbounds for k in 1:n
        w = s .* W[k]
        for j in 1:k
            w -= H[j,k] * W[j]
        end
        W[k+1] = w/H[k+1,k]
    end
    sum(W[i]*d[i] for i in eachindex(d))
end

# Polynomial Interface
"""
    ArnoldiFit

A polynomial type produced through fitting a degree ``n`` or less polynomial to data ``(x_1,y_1),…,(x_N, y_N), N ≥ n+1``, This uses Arnoldi orthogonalization to avoid the exponentially ill-conditioned Vandermonde polynomial. See [`Polynomials.polyfitA`](@ref) for details.
"""
struct ArnoldiFit{T, M<:AbstractArray{T,2}, X}  <: AbstractPolynomial{T,X}
    coeffs::Vector{T}
    H::M
end
export ArnoldiFit
@register ArnoldiFit
domain(::Type{<:ArnoldiFit}) = Interval(-Inf, Inf)

Base.show(io::IO, mimetype::MIME"text/plain", p::ArnoldiFit) = print(io, "ArnoldiFit of degree $(length(p.coeffs)-1)")

evalpoly(x, p::ArnoldiFit) = polyvalA(p.coeffs, p.H, x)

fit(::Type{ArnoldiFit}, x::AbstractVector{T}, y::AbstractVector{T}, deg::Int=length(x)-1;  var=:x, kwargs...) where{T} = polyfitA(x, y, deg; var=var)

Base.convert(::Type{P}, p::ArnoldiFit) where {P <: AbstractPolynomial} = p(variable(P,indeterminate(p)))

## --------------------------------------------------
"""
    compensated_horner(p::P, x)
    compensated_horner(ps, x)

Evaluate `p(x)` using a compensation scheme of S. Graillat, Ph. Langlois, N. Louve [Compensated Horner Scheme](https://cadxfem.org/cao/Compensation-horner.pdf). Either a `Polynomial` `p` or its coefficients may be passed in.

The Horner scheme has relative error given by

`|(p(x) - p̂(x))/p(x)| ≤ α(n) ⋅ u ⋅ cond(p, x)`, where `u` is the precision (`2⁻⁵³ = eps()/2`)).

The compensated Horner scheme has relative error bounded by

`|(p(x) - p̂(x))/p(x)| ≤  u +  β(n) ⋅ u² ⋅ cond(p, x)`.

As noted, this reflects the accuracy of a backward stable computation performed in doubled working precision `u²`. (E.g., polynomial evaluation of a `Polynomial{Float64}` object through `compensated_horner` is as accurate as evaluation of a `Polynomial{Double64}` object (using the `DoubleFloat` package), but significantly faster.

Pointed out in https://discourse.julialang.org/t/more-accurate-evalpoly/45932/5.
"""
function compensated_horner(p::P, x) where {T, P <: StandardBasisPolynomial{T}}
    compensated_horner(coeffs(p), x)
end

# rexpressed from paper to compute horner_sum in same pass
# sᵢ -- horner sum
# c -- compensating term
@inline function compensated_horner(ps, x)
    n, T = length(ps), eltype(ps)
    aᵢ = ps[end]
    sᵢ = aᵢ * EvalPoly._one(x)
    c = zero(T) * EvalPoly._one(x)
    for i in n-1:-1:1
	aᵢ = ps[i]
        pᵢ, πᵢ = two_product_fma(sᵢ, x)
	sᵢ, σᵢ = two_sum(pᵢ, aᵢ)
        c = fma(c, x, πᵢ + σᵢ)
    end
    sᵢ + c
end

function compensated_horner(ps::Tuple, x::S) where {S}
    ps == () && return zero(S)
    if @generated
        n = length(ps.parameters)
        sσᵢ =:(ps[end] * EvalPoly._one(x), zero(S))
        c = :(zero(S) * EvalPoly._one(x))
        for i in n-1:-1:1
            pπᵢ = :(two_product_fma($sσᵢ[1], x))
	    sσᵢ = :(two_sum($pπᵢ[1], ps[$i]))
            Σ = :($pπᵢ[2] + $sσᵢ[2])
            c = :(fma($c, x, $Σ))
        end
        s = :($sσᵢ[1] + $c)
        s
    else
        compensated_horner(ps, x)
    end
end

# error-free-transformations (EFT) of a∘b = x + y where x, y floating point, ∘ mathematical
# operator (not ⊕ or ⊗)
@inline function two_sum(a, b)
    x = a + b
    z = x - a
    y = (a - (x-z)) + (b-z)
    x, y
end

# return x, y, floats,  with x + y = a * b
@inline function two_product_fma(a, b)
    x = a * b
    y = fma(a, b, -x)
    x, y
end

# Condition number of a standard basis polynomial
# rule of thumb: p̂ a compute value
# |p(x) - p̃(x)|/|p(x)| ≤ α(n)⋅u ⋅ cond(p,x), where u = finite precision of compuation (2^-p)
function LinearAlgebra.cond(p::P, x) where {P <: StandardBasisPolynomial}
    p̃ = map(abs, p)
    p̃(abs(x))/ abs(p(x))
end