using NLsolve, NLopt

function treat_inputs(x::AbstractVector)
  n::Int = length(x)/3
  reshape(x,(3,n))'
end

function single_shoot(start::Vector{Float64},
                      final::Vector{Float64},
                      craft::Sc,
                      μ::Float64,
                      t0::Float64,
                      tf::Float64,
                      n::Int,
                      x0::AbstractVector,
                      tol=1e-2)

  function f!(F,x)
    F[1:6] .= prop(treat_inputs(x), start, craft, μ, tf-t0)[1][end,:] - final
    F[7:3n] .= 0.
  end

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

end

function single_shoot2(start::Vector,
                       final::Vector,
                       craft::Sc,
                       μ::AbstractFloat,
                       t0::AbstractFloat,
                       tf::AbstractFloat,
                       x0::Vector,
                       tol=1e-8)

  n::Int = length(x0)/3
  m0 = craft.mass

  f(x::Vector) = m0 - prop(treat_inputs(x), start, craft, μ, tf-t0)[2][end]
  f_constraint(x::Vector) = norm(prop(treat_inputs(x), start, craft, μ, tf-t0)[1][end,:] - final)

  function nlfunc(x::Vector, grad::Vector)
    try
      if length(grad) != 0
        ForwardDiff.gradient!(grad, f, x)
      end
      f(x)
    catch e
      println("Error was $e")
      throw(e)
    end
  end

  function nlconstraint(x::Vector, grad::Vector)
    if length(grad) != 0
      ForwardDiff.gradient!(grad, f_constraint, x)
    end
    f_constraint(x)
  end

  opt = Opt(:LD_MMA, 3n)
  lower_bounds = Vector{Float64}()
  upper_bounds = Vector{Float64}()
  for i in 1:3n
    if i%3 == 1
      push!(lower_bounds, 0.)
      push!(upper_bounds, 1.)
    elseif i%3 == 2
      push!(lower_bounds, -π)
      push!(upper_bounds, π)
    elseif i%3 == 0
      push!(lower_bounds, -π/2)
      push!(upper_bounds, π/2)
    end
  end
  opt.lower_bounds = lower_bounds
  opt.upper_bounds = upper_bounds
  opt.xtol_rel = 1e-4
  opt.min_objective = nlfunc
  inequality_constraint!(opt, nlconstraint, 1e-8)

  (minf, minx, ret) = optimize(opt, x0)
  numevals = opt.numevals

  return minf, minx, ret, numevals

end