
"""
    $SIGNATURES

Return a random sample as a tangent vector from a belief represented by coordinates on a manifold at point p.

Notes

"""
function sampleTangent end

# Sampling MKD
function sampleTangent(M::AbstractGroupManifold, x::ManifoldKernelDensity, p=mean(x))
    # get legacy matrix of coordinates and selected labels
    coords, lbls = sample(x.belief,1)
    X = hat(x.manifold, p, coords)
    return X
end

function sampleTangent(x::ManifoldKernelDensity, p=mean(x)) 
  return sampleTangent(x.manifold, x, p)
end

# Sampling Distributions
function sampleTangent(M::AbstractManifold, z::Distribution, p, basis::AbstractBasis) 
  return get_vector(M, p, rand(z), basis)
end

function sampleTangent(M::AbstractGroupManifold, z::Distribution, p=identity_element(M)) 
  return hat(M, p, rand(z,1)[:]) #TODO find something better than (z,1)[:]
end


"""
    $SIGNATURES

Return a random sample point on a manifold from a belief represented by coordinates at point p.

Notes

"""
function samplePoint(M::AbstractManifold, sbelief, p, basis::AbstractBasis, retraction_method::AbstractRetractionMethod=ExponentialRetraction())
  X = sampleTangent(M, sbelief, p, basis)
  return retract(M, p, X, retraction_method)
end
function samplePoint(M::AbstractGroupManifold, sbelief, p=identity_element(M), retraction_method::AbstractRetractionMethod=ExponentialRetraction())
  X = sampleTangent(M, sbelief, p)
  return retract(M, p, X, retraction_method)
end

function samplePoint(M::AbstractGroupManifold, sbelief::ManifoldKernelDensity, p=identity_element(M, mean(sbelief)), retraction_method::AbstractRetractionMethod=ExponentialRetraction())
  X = sampleTangent(M, sbelief, p)
  return retract(M, p, X, retraction_method)
end

function samplePoint(x::ManifoldKernelDensity, p=mean(x)) 
  return samplePoint(x.manifold, x, p)
end

# FIXME: rather use manifolds
function samplePoint(distr::SamplableBelief)
  Base.depwarn("samplePoint(distr::SamplableBelief) should be replaced by samplePoint(M<:AbstractManifold, distr::SamplableBelief, ...)", :samplePoint)
  rand(distr,1)
end

## default getSample
"""
    $SIGNATURES

Sample the factor in `CalcFactor`. A default `getSample` method is provided that should cover most use cases, 
if more advanced sampling is required, the `getSample` function should be extended.

The default behavior for `getSample` is as follows:
- The `SamplableBelief`` shall be in the field `Z` and that shall be enough to fully define the factor, i.e. `Z<:SamplableBelief` should be the only field.
- Sampling on `<:AbstractManifoldMinimize` factors defined on Group Manifolds: 
  - `getSample` normally returns a tangent vector at the identity element, however it should just match the custom factor definition.
- Sampling on prior (`<:AbstractPrior`) factors : 
  - `getSample` must return a point on the manifold that matches the point representation of the variable.

Notes
- Users should overload this method should their factor not only use field `Z` for the `SamplableBelief`.
- See the Custom Factors section in the Caesar.jl documentation for more examples and details.
- Also see issue https://github.com/JuliaRobotics/IncrementalInference.jl/issues/1441

See also: [`getMeasurementParametric`](@ref)
"""
function getSample end

function getSample(cf::CalcFactor{<:AbstractPrior})
  M = getManifold(cf.factor)
  if hasfield(typeof(cf.factor), :Z)
    X = samplePoint(M, cf.factor.Z)
  else
    error("""Factor $(typeof(cf.factor)) does not have a field `Z`, to use the default `getSample` method, use `Z` for the measurement. 
              Alternatively, provide a `getSample` method. See IIF issue #1441 and Custom Factors in the Caesar documentation.""")
  end
  return X
end

function getSample(cf::CalcFactor{<:AbstractRelative}) 
  M = getManifold(cf.factor)
  if hasfield(typeof(cf.factor), :Z)
    X = sampleTangent(M, cf.factor.Z)
  else
    error("""Factor $(typeof(cf.factor)) does not have a field `Z`, to use the default `getSample` method, use `Z` for the measurement. 
              Alternatively, provide a `getSample` method. See IIF issue #1441 and Custom Factors in the Caesar documentation.""")
  end
  return X
end
