using NLsolve

export nlp_solve, mass_est

function mass_est(T)
  ans = 0
  n = Int(length(T)/3)
  for i in 1:n ans += norm(T[i,:]) end
  return ans/n
end

struct Result
  converged::Bool
  zero::Matrix{Float64}
end

function nlp_solve(start::Vector{Float64},
                   final::Vector{Float64},
                   craft::Sc,
                   μ::Float64,
                   t0::Float64,
                   tf::Float64,
                   x0::Matrix{Float64};
                   tol=1e-6,
                   num_iters=1_000)

  function f!(F,x)
    try
      F .= 0.0
      F[1:6, 1] .= prop(tanh.(x), start, copy(craft), μ, tf-t0)[2][1:6] .- final[1:6]
    catch e
      F .= 10000000.0
    end
  end

  result = Result(false, zeros(size(x0)))
  try
    nl_results = nlsolve(f!, atanh.(x0), ftol=tol, autodiff=:forward, iterations=num_iters)
    result = Result(converged(nl_results), tanh.(nl_results.zero))
  catch e
  end

  return result

end