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

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

  function f!(F,x)
    F .= 0.0
    F[1:6, 1] .= prop_nlsolve(tanh.(x), start, craft, μ, tf-t0) .- final
  end

  return nlsolve(f!, atanh.(x0), ftol=tol, autodiff=:forward, iterations=1_000)

end