export oe_to_rθh, rθh_to_xyz, oe_to_xyz, xyz_to_oe, conv_T

function oe_to_rθh(oe::Vector,μ::Real) :: Vector

  a,e,i,Ω,ω,ν = oe
  return [a*(1-e^2)/(1+e*cos(ν)),
  0,
  0,
  (μ/sqrt(μ*a*(1-e^2)))*e*sin(ν),
  (μ/sqrt(μ*a*(1-e^2)))*(1+e*cos(ν)),
  0]

end

function rθh_to_xyz(rθh_vec::Vector,oe::Vector)

  a,e,i,Ω,ω,ν = oe
  θ = ω+ν
  cΩ,sΩ,ci,si,cθ,sθ = cos(Ω),sin(Ω),cos(i),sin(i),cos(θ),sin(θ)
  DCM = [cΩ*cθ-sΩ*ci*sθ -cΩ*sθ-sΩ*ci*cθ sΩ*si;
  sΩ*cθ+cΩ*ci*sθ -sΩ*sθ+cΩ*ci*cθ -cΩ*si;
  si*sθ si*cθ ci]
  DCM = kron(Matrix(I,2,2),DCM)
  return DCM*rθh_vec

end

function oe_to_xyz(oe::Vector,μ::Real)

  return rθh_to_xyz(oe_to_rθh(oe,μ),oe)

end

function xyz_to_oe(cart_vec::Vector,μ::Real)

  r_xyz, v_xyz = cart_vec[1:3],cart_vec[4:6]
  r = norm(r_xyz)
  h_xyz = cross(r_xyz,v_xyz) #km^2/s
  h = norm(h_xyz) #km^2/s
  ξ = dot(v_xyz,v_xyz)/2 - μ/r #km^2 s^-2
  a = -μ/(2ξ) #km
  e_xyz = cross(v_xyz,h_xyz)/μ - r_xyz/r
  e = norm(e_xyz)
  if h_xyz[3]/h < 1.
    i = acos(h_xyz[3]/h) #rad
  else
    i = acos(1.)
  end
  n_xyz = cross([0,0,1],h_xyz)
  if dot(n_xyz,[1,0,0])/norm(n_xyz) < 1.
    Ω = acos(dot(n_xyz,[1,0,0])/norm(n_xyz))
  else
    Ω = acos(1.)
  end
  if dot(n_xyz,e_xyz)/(norm(n_xyz)*e) < 1.
    ω = acos(dot(n_xyz,e_xyz)/(norm(n_xyz)*e))
  else
    ω = acos(1.)
  end
  if abs((dot(r_xyz,e_xyz))/(r*norm(e_xyz))) < 1.
    ν = acos((dot(r_xyz,e_xyz))/(r*norm(e_xyz)))
  else
    ν = acos(1.)
  end
  Ω = dot(n_xyz,[0,1,0]) > 0. ? Ω : -Ω
  ω = dot(e_xyz,[0,0,1]) > 0. ? ω : -ω
  ν = dot(r_xyz,v_xyz) > 0. ? ν : -ν
  return [a,e,i,Ω,ω,ν]

end

"""
Converts a series of thrust vectors from R,Θ,H frame to cartesian
"""
function conv_T(Tm::Vector{Float64},
                Ta::Vector{Float64},
                Tb::Vector{Float64},
                init_state::Vector{Float64},
                m::Float64,
                craft::Sc,
                time::Float64,
                μ::Float64)

  Txs = Float64[]
  Tys = Float64[]
  Tzs = Float64[]
  n::Int = length(Tm)

  for i in 1:n
    mag, α, β = Tm[i], Ta[i], Tb[i]
    if mag > 1 || mag < 0
      @error "ΔV input is too high: $mag"
    elseif α > π || α < -π
      @error "α angle is incorrect: $α"
    elseif β > π/2 || β < -π/2
      @error "β angle is incorrect: $β"
    end
  end

  state = init_state
  for i in 1:n
    mag, α, β = Tm[i], Ta[i], Tb[i]
    thrust_rθh = mag * [cos(β)*sin(α), cos(β)*cos(α), sin(β)]
    _,_,i,Ω,ω,ν = xyz_to_oe(state, μ)
    θ = ω+ν
    cΩ,sΩ,ci,si,cθ,sθ = cos(Ω),sin(Ω),cos(i),sin(i),cos(θ),sin(θ)
    DCM = [cΩ*cθ-sΩ*ci*sθ -cΩ*sθ-sΩ*ci*cθ sΩ*si;
           sΩ*cθ+cΩ*ci*sθ -sΩ*sθ+cΩ*ci*cθ -cΩ*si;
           si*sθ          si*cθ           ci      ]
    Tx, Ty, Tz = DCM*thrust_rθh

    state = prop_one([Tx, Ty, Tz], state, craft, μ, time/n)[1]
    push!(Txs, Tx)
    push!(Tys, Ty)
    push!(Tzs, Tz)
  end
  return Txs, Tys, Tzs
end