####################### RVRP FEATURES #############################

# Location based features  #
const HAS_OPENING_TIME_WINDOWS = 1
const HAS_MULTIPLE_OPENING_TIME_WINDOWS = 2
const HAS_FLEXIBLE_OPENING_TIME_WINDOWS = 3

# Product based features #
const HAS_PRODUCT_CONFLICT_CLASSES = 4
const HAS_PRODUCT_PROHIBITED_PREDECESSOR_CLASSES = 5
const HAS_PRODUCT_PICKUPONLY_SHARING_CLASSES = 6
const HAS_PRODUCT_DELIVERYONLY_SHARING_CLASSES = 7
const HAS_PRODUCT_SHIPMENT_SHARING_CLASSES = 8
const HAS_PRODUCT_CAPACITY_CONSUMPTIONS = 9
const HAS_PRODUCT_PROPERTIES_REQUIREMENTS = 10
const HAS_MULTIPLE_PRODUCT_CAPACITY_CONSUMPTIONS = 11
const HAS_MULTIPLE_PRODUCT_PROPERTIES_REQUIREMENTS = 12

# Request based features
const HAS_SHIPMENT_REQUESTS = 109
const HAS_PICKUPONLY_REQUESTS = 110
const HAS_DELIVERYONLY_REQUESTS = 111
const HAS_ALTERNATIVE_PICKUP_LOCATIONS = 112 #
const HAS_ALTERNATIVE_DELIVERY_LOCATIONS = 113 #
const HAS_MAX_DURATION = 114
const HAS_DURATION_UNIT_COSTS = 115 #
const HAS_PICKUP_TIME_WINDOWS = 116
const HAS_DELIVERY_TIME_WINDOWS = 117
const HAS_MULTIPLE_PICKUP_TIME_WINDOWS = 118
const HAS_MULTIPLE_DELIVERY_TIME_WINDOWS = 119
const HAS_FLEXIBLE_PICKUP_TIME_WINDOWS = 120 #
const HAS_FLEXIBLE_DELIVERY_TIME_WINDOWS = 121 #
const HAS_FLEXIBLE_REQUESTS = 122 #

# VehicleCategory based features
const HAS_VEHICLE_CAPACITIES = 224
const HAS_VEHICLE_PROPERTIES = 225
const HAS_COMPARTMENT_CAPACITIES = 226 #
const HAS_COMPARTMENT_PROPERTIES = 227 #
const HAS_MULTIPLE_VEHICLE_CAPACITIES = 228
const HAS_MULTIPLE_VEHICLE_PROPERTIES = 229
const HAS_MULTIPLE_COMPARTMENT_CAPACITIES = 230 #
const HAS_MULTIPLE_COMPARTMENT_PROPERTIES = 231 #

# HomogeneousVehicleSet based features
const HAS_TRAVEL_TIME_UNIT_COST = 333
const HAS_SERVICE_TIME_UNIT_COST = 334
const HAS_WAITING_TIME_UNIT_COST = 335
const HAS_TRAVEL_DISTANCE_UNIT_COST = 339
const HAS_MAX_WORKING_TIME = 337 #
const HAS_MAX_TRAVEL_DISTANCE = 338 #
const HAS_MIN_NB_VEHICLES = 340 #
const HAS_MAX_NB_VEHICLES = 341
const HAS_FLEXIBLE_NB_VEHICLES_RANGE = 342 #
const HAS_WORKING_TIME_WINDOW = 343
const HAS_FLEXIBLE_WORKING_TIME_WINDOWS = 345 #
const HAS_FIXED_COST_PER_VEHICLE = 346
const HAS_OPEN_DEPARTURE = 347
const HAS_OPEN_ARRIVAL = 348
const HAS_ARRIVAL_DIFFERENT_FROM_DEPARTURE = 349
const HAS_ALTERNATIVE_DEPARTURE_LOCATIONS = 350 #
const HAS_ALTERNATIVE_ARRIVAL_LOCATIONS = 351 #
const HAS_MULTIPLE_WORKING_TIME_WINDOWS = 352 #
const HAS_MULTIPLE_COSTS = 353 #

# Instance based features
const HAS_WORK_PERIODS = 447 # TO BE REMOVED
const HAS_MUTLIPLE_TRAVEL_TIME_PERIODS = 448
# const HAS_VEHICLE_CATEGORIES = 449 (not needed, duplicate of caps props)
const HAS_MULTIPLE_VEHICLE_CATEGORIES = 450
const HAS_MULTIPLE_VEHICLE_SETS = 451
const HAS_ENERGY_FEATURES = 452 # to be detailed later as needed #
const HAS_X_Y = 453

function check_if_supports(supported_vec::Vector{BitSet},
                           instance_features::BitSet)
    for supported in supported_vec
        if issubset(instance_features, supported)
            return true
        end
    end
    return false
end

function check_id(id_to_index_dict, key, error_prefix::String)
    if !haskey(id_to_index_dict, key)
        error(error_prefix, "There is no object with id $key")
    end
end

function check_positive_range(range::Range, error_prefix::String)
    if range.lb < 0.0
        error(error_prefix, "lb must be >= 0")
    elseif range.ub < 0.0
        error(error_prefix, "lb must be >= 0")
    elseif range.ub < range.lb
        error(error_prefix, "lb must be less or equal ub")
    end
end

function check_time_windows(tws::Vector{Range}, error_prefix::String)
    check_positive_range(tws[1], "Time window ")
    for idx in 2:length(tws)
        check_positive_range(tws[idx], "Time window ")
        if (tws[idx-1].lb > tws[idx].lb || tws[idx-1].ub > tws[idx].ub
            || tws[idx-1].ub > tws[idx].lb)
            error(error_prefix, "must not intersect and must be increasing: ", tws)
        end
    end
end

function check_locations(locations::Vector{Location},
                         computed_data::RvrpComputedData,
                         coordinate_mode::Int, features::BitSet)

    for loc in locations
        # If in long_lat mode
        if coordinate_mode == 0
            if loc.lat_y < - 90.0 || loc.lat_y > 90.0
                error("Location $(loc.id) must have lat_y in [-90.0, +90.0]")
            elseif loc.long_x < - 180.0 || loc.long_x > 180.0
                error("Location $(loc.id) must have long_x in [-180.0, +180.0]")
            end
        end
        if loc.index < 1 || loc.index > length(locations)
            error("Location $(loc.id) must have index in [1, length(locations)]")
        end
        check_time_windows(loc.opening_time_windows,
                           string("Time windows of location ", loc.id, " "))
    end

    # filling LOCATION based features
    for loc in locations
        if loc.opening_time_windows != [Range()]
            union!(features, HAS_OPENING_TIME_WINDOWS)
        end
        if length(loc.opening_time_windows) > 1
            union!(features, HAS_MULTIPLE_OPENING_TIME_WINDOWS)
        end
    end

end

function check_location_groups(location_groups::Vector{LocationGroup},
                               computed_data::RvrpComputedData)

    for grp in location_groups
        for loc_id in grp.location_ids
            check_id(computed_data.location_id_2_index, loc_id,
                     "LocationGroup $(grp.id), location_ids : ")
        end
    end
end

function check_requests(requests::Vector{Request},
                        computed_data::RvrpComputedData)

    for req in requests
        if req.request_type in [0, 1]
            check_id(computed_data.location_group_id_2_index,
                     req.pickup_location_group_id,
                     "Request $(req.id), pickup_location_group_id : ")
        elseif req.request_type in [0, 2]
            check_id(computed_data.location_group_id_2_index,
                     req.delivery_location_group_id,
                     "Request $(req.id), delivery_location_group_id : ")
        end
        check_positive_range(req.product_quantity_range,
                 "Request $(req.id), product_quantity_range : ")
        check_positive_range(req.pickup_time_windows[1].soft_range,
                 "Request $(req.id), pickup_time_windows[1].soft_range : ")
        check_positive_range(req.delivery_time_windows[1].soft_range,
                 "Request $(req.id), delivery_time_windows[1].soft_range : ")
        if req.precedence_status < 0 || req.precedence_status > 2
            error("Request $(req.id) must have precedence_status in {0,1,2}")
        elseif req.pickup_service_time < 0
            error("Request $(req.id) must have pickup_service_time > 0")
        elseif req.delivery_service_time < 0
            error("Request $(req.id) must have delivery_service_time > 0")
        elseif req.max_duration < 0
            error("Request $(req.id) must have max_duration > 0")
        elseif req.duration_unit_cost < 0
            error("Request $(req.id) must have duration_unit_cost > 0")
        end
        check_time_windows([tw.soft_range for tw in req.pickup_time_windows],
            string("Time windows of pickup of request ", req.id, " "))
        check_time_windows([tw.soft_range for tw in req.delivery_time_windows],
            string("Time windows of delivery of request ", req.id, " "))

        # filling REQUEST based features
        features = computed_data.features
        if req.request_type == 0
            union!(features, HAS_SHIPMENT_REQUESTS)
        elseif req.request_type == 1
            union!(features, HAS_PICKUPONLY_REQUESTS)
        elseif req.request_type == 0
            union!(features, HAS_DELIVERYONLY_REQUESTS)
        end
        if req.max_duration < MAXNUMBER
            union!(features, HAS_MAX_DURATION)
        end
        if req.pickup_time_windows[1].soft_range.lb > 0 ||
           req.pickup_time_windows[1].soft_range.ub < MAXNUMBER
            union!(features, HAS_PICKUP_TIME_WINDOWS)
        end
        if req.delivery_time_windows[1].soft_range.lb > 0 ||
           req.delivery_time_windows[1].soft_range.ub < MAXNUMBER
            union!(features, HAS_DELIVERY_TIME_WINDOWS)
        end
    end
end

function check_vehicle_categories(vehicle_categories::Vector{VehicleCategory},
                                  computed_data::RvrpComputedData)

    for vc in vehicle_categories
        if vc.loading_option < 0 || vc.loading_option > 2
            error("VehicleCategory $(vc.id) must have loading_option in",
                  "{0,1,2}")
        end

        # filling VehicleCategory based features
        features = computed_data.features
        if length(vc.capacities.of_vehicle) > 0
            union!(features, HAS_VEHICLE_CAPACITIES)
        end
        if length(vc.capacities.of_compartments) > 0
            union!(features, HAS_COMPARTMENT_CAPACITIES)
        end
        if length(vc.properties.of_vehicle) > 0
            union!(features, HAS_VEHICLE_PROPERTIES)
        end
        if length(vc.properties.of_compartments) > 0
            union!(features, HAS_COMPARTMENT_PROPERTIES)
        end
        if length(vc.capacities.of_vehicle) > 1
            union!(features, HAS_MULTIPLE_VEHICLE_CAPACITIES)
        end
        if length(vc.capacities.of_compartments) > 1
            union!(features, HAS_MULTIPLE_COMPARTMENT_CAPACITIES)
        end
        if length(vc.properties.of_vehicle) > 1
            union!(features, HAS_MULTIPLE_VEHICLE_PROPERTIES)
        end
        if length(vc.properties.of_compartments) > 1
            union!(features, HAS_MULTIPLE_COMPARTMENT_PROPERTIES)
        end
    end
end

function check_vehicle_cost(vs_id::String, cost_period::CostPeriod,
                            computed_data::RvrpComputedData)
    check_positive_range(cost_period.period,
        "VehicleSet $(vs_id), cost_period.period : ")
    if cost_period.travel_time_unit_cost < 0
        error("VehicleSet $(vs_id) must have all travel_time_unit_cost > 0")
    elseif cost_period.service_time_unit_cost < 0
        error("VehicleSet $(vs_id) must have all service_time_unit_cost > 0")
    elseif cost_period.waiting_time_unit_cost < 0
        error("VehicleSet $(vs_id) must have all waiting_time_unit_cost > 0")
    elseif cost_period.fixed_cost < 0
        error("VehicleSet $(vs_id) must have all fixed_cost > 0")
    end

    # filling CostPeriod based features
    features = computed_data.features
    if cost_period.travel_time_unit_cost > 0
        union!(features, HAS_TRAVEL_TIME_UNIT_COST)
    end
    if cost_period.service_time_unit_cost > 0
        union!(features, HAS_SERVICE_TIME_UNIT_COST)
    end
    if cost_period.waiting_time_unit_cost > 0
        union!(features, HAS_WAITING_TIME_UNIT_COST)
    end
    if cost_period.travel_distance_unit_cost > 0
        union!(features, HAS_TRAVEL_DISTANCE_UNIT_COST)
    end
    if cost_period.fixed_cost > 0
        union!(features, HAS_FIXED_COST_PER_VEHICLE)
    end
end

function check_vehicle_sets(vehicle_sets::Vector{HomogeneousVehicleSet},
                            computed_data::RvrpComputedData)

    for vs in vehicle_sets
        check_id(computed_data.vehicle_category_id_2_index,
                 vs.vehicle_category_id,
                 "VehicleSet $(vs.id), vehicle_category_id : ")
        check_id(computed_data.location_group_id_2_index,
                 vs.departure_location_group_id,
                 "VehicleSet $(vs.id), departure_location_group_id : ")
        check_id(computed_data.location_group_id_2_index,
                 vs.arrival_location_group_id,
                 "VehicleSet $(vs.id), arrival_location_group_id : ")
        check_positive_range(vs.nb_of_vehicles_range.soft_range,
                 "VehicleSet $(vs.id), nb_of_vehicles_range.soft_range : ")
        check_time_windows([tw.soft_range for tw in vs.work_periods],
            string("Work periods of HomogeneousVehicleSet ", vs.id, " "))
        for wp in vs.work_periods
            check_positive_range(wp.soft_range,
                "VehicleSet $(vs.id), work_period.soft_range : ")
        end
        for cost_period in vs.cost_periods
            check_vehicle_cost(vs.id, cost_period, computed_data)
        end

        # filling HomogeneousVehicleSet based features
        features = computed_data.features
        if vs.route_mode in [1,3]
            union!(features, HAS_OPEN_ARRIVAL)
        end
        if vs.route_mode in [1,3]
            union!(features, HAS_OPEN_DEPARTURE)
        end
        if vs.nb_of_vehicles_range.soft_range.ub < MAXNUMBER
            union!(features, HAS_MAX_NB_VEHICLES)
        end
        if length(vs.work_periods) > 1
            union!(features, HAS_MULTIPLE_WORKING_TIME_WINDOWS)
        end
        if length(vs.cost_periods) > 1
            union!(features, HAS_MULTIPLE_COSTS)
        end
        if vs.vehicle_category_id == "default_id"
            computed_data.uses_default_vehicle_category = true
        end
        for wp in vs.work_periods
            if wp.soft_range.lb > 0 ||
                wp.soft_range.ub < MAXNUMBER
                union!(features, HAS_WORKING_TIME_WINDOW)
            end
        end
    end
end

function check_product_specification_classes(
    product_specification_classes::Vector{ProductSpecificationClass},
    computed_data::RvrpComputedData)

    features = computed_data.features

    for prod_spec_class in product_specification_classes
        for (k,v) in prod_spec_class.capacity_consumptions
            if v[1] < 0.0 || v[2] < 0.0
                error("Product ", prod_spec_class.id,
                      " must have a non-negative consumption of all ",
                      "of all capacity measures.",
                      " Consumption is ", v[1], " per lot of ", v[2])
            end
        end
        for (k,v) in prod_spec_class.property_requirements
            if v < 0.0
                error("Product ", prod_spec_class.id,
                      " must have a non-negative requirement for all properties.",
                      " Requirement is ", v[1])
            end
        end
    end

    nb_capacities_specificartions = 0
    nb_properties_requirements = 0
    for prod_spec_class in product_specification_classes
        if !isempty(prod_spec_class.capacity_consumptions)
            union!(features, HAS_PRODUCT_CAPACITY_CONSUMPTIONS)
            nb_capacities_specificartions += 1
        end
        if !isempty(prod_spec_class.property_requirements)
            union!(features, HAS_PRODUCT_PROPERTIES_REQUIREMENTS)
            nb_properties_requirements += 1
        end
    end
    if nb_capacities_specificartions > 1
        union!(features, HAS_MULTIPLE_PRODUCT_CAPACITY_CONSUMPTIONS)
    end
    if nb_properties_requirements > 1
        union!(features, HAS_MULTIPLE_PRODUCT_PROPERTIES_REQUIREMENTS)
    end

end

function check_ids_in_vector(vec::Vector{T}) where T <: Union{
    Location, TravelSpecification, LocationGroup,
    ProductSharingClass, ProductSpecificationClass,
    ProductCompatibilityClass, Request, VehicleCategory,
    HomogeneousVehicleSet}
    all_ids = Set{String}()
    for obj in vec
        if obj.id in all_ids
            error("Id ", obj.id, " is used more that ",
                  "once in vector of ", Symbol(eltype(vec)))
        end
        push!(all_ids, obj.id)
    end
end

function check_repeated_ids(data::RvrpInstance)

    check_ids_in_vector(data.locations)
    check_ids_in_vector(data.travel_specifications)
    check_ids_in_vector(data.location_groups)
    check_ids_in_vector(data.product_compatibility_classes)
    check_ids_in_vector(data.product_specification_classes)
    check_ids_in_vector(data.product_sharing_classes)
    check_ids_in_vector(data.requests)
    check_ids_in_vector(data.vehicle_categories)
    check_ids_in_vector(data.vehicle_sets)

end

function check_instance(data::RvrpInstance, computed_data::RvrpComputedData)

    if data.id == ""
        error("Invalid instance id: ", data.id)
    end
    check_repeated_ids(data)

    tt_period = data.travel_periods[1]
    check_id(computed_data.travel_specification_id_2_index,
             tt_period.travel_specification_id,
             "TavelTimePeriods[1] : ")

    features = computed_data.features
    check_locations(data.locations, computed_data, data.coordinate_mode, features)
    check_location_groups(data.location_groups, computed_data)
    check_requests(data.requests, computed_data)
    check_vehicle_categories(data.vehicle_categories, computed_data)
    check_vehicle_sets(data.vehicle_sets, computed_data)
    check_product_specification_classes(data.product_specification_classes, computed_data)

    if !(data.coordinate_mode in [0, 1])
        error("Invalid value for coordinate_mode: $data.coordinate_mode")
    end

    features = computed_data.features

    if (in(HAS_PRODUCT_CAPACITY_CONSUMPTIONS, features) &&
        !in(HAS_VEHICLE_CAPACITIES, features))
        error("Instance is inconsistent. It features produtct capacity",
              " consumptions, but does not feature vehicles with capacity",
              " specifications")
    end
    if (in(HAS_PRODUCT_PROPERTIES_REQUIREMENTS, features) &&
        !in(HAS_VEHICLE_PROPERTIES, features))
        error("Instance is inconsistent. It features produtct property",
              " requirements, but does not feature vehicles property",
              " specifications")
    end

    # filling Instance based features
    # if length(data.vehicle_categories) > 0
    #     union!(features, HAS_VEHICLE_CATEGORIES)
    # end
    if (length(data.vehicle_categories) == 2 &&
        computed_data.uses_default_vehicle_category) ||
       (length(data.vehicle_categories) > 2)

        union!(features, HAS_MULTIPLE_VEHICLE_CATEGORIES)
    end
    if length(data.vehicle_sets) > 1
        union!(features, HAS_MULTIPLE_VEHICLE_SETS)
    end
    if length(data.travel_periods) > 1
        union!(features, HAS_MUTLIPLE_TRAVEL_TIME_PERIODS)
    end
    if data.coordinate_mode == 1
        union!(features, HAS_X_Y)
    end

    # TODO check matrices sizes (based on features)
end

function print_features(features::BitSet)
    println("The features are : ")
    # Location based features  #
    if in(HAS_OPENING_TIME_WINDOWS, features)
        println("HAS_OPENING_TIME_WINDOWS,")
    end
    if in(HAS_MULTIPLE_OPENING_TIME_WINDOWS, features)
        println("HAS_MULTIPLE_OPENING_TIME_WINDOWS,")
    end
    if in(HAS_FLEXIBLE_OPENING_TIME_WINDOWS, features)
        println("HAS_FLEXIBLE_OPENING_TIME_WINDOWS,")
    end

    # Product based features #
    if in(HAS_PRODUCT_CONFLICT_CLASSES, features)
        println("HAS_PRODUCT_CONFLICT_CLASSES,")
    end
    if in(HAS_PRODUCT_PROHIBITED_PREDECESSOR_CLASSES, features)
        println("HAS_PRODUCT_PROHIBITED_PREDECESSOR_CLASSES,")
    end
    if in(HAS_PRODUCT_PICKUPONLY_SHARING_CLASSES, features)
        println("HAS_PRODUCT_PICKUPONLY_SHARING_CLASSES,")
    end
    if in(HAS_PRODUCT_DELIVERYONLY_SHARING_CLASSES, features)
        println("HAS_PRODUCT_DELIVERYONLY_SHARING_CLASSES,")
    end
    if in(HAS_PRODUCT_SHIPMENT_SHARING_CLASSES, features)
        println("HAS_PRODUCT_SHIPMENT_SHARING_CLASSES,")
    end
    if in(HAS_PRODUCT_CAPACITY_CONSUMPTIONS, features)
        println("HAS_PRODUCT_CAPACITY_CONSUMPTIONS,")
    end
    if in(HAS_PRODUCT_PROPERTIES_REQUIREMENTS, features)
        println("HAS_PRODUCT_PROPERTIES_REQUIREMENTS,")
    end
    if in(HAS_MULTIPLE_PRODUCT_CAPACITY_CONSUMPTIONS, features)
        println("HAS_MULTIPLE_PRODUCT_CAPACITY_CONSUMPTIONS,")
    end
    if in(HAS_MULTIPLE_PRODUCT_PROPERTIES_REQUIREMENTS, features)
        println("HAS_MULTIPLE_PRODUCT_PROPERTIES_REQUIREMENTS,")
    end

    # Request based features
    if in(HAS_SHIPMENT_REQUESTS, features)
        println("HAS_SHIPMENT_REQUESTS,")
    end
    if in(HAS_PICKUPONLY_REQUESTS, features)
        println("HAS_PICKUPONLY_REQUESTS,")
    end
    if in(HAS_DELIVERYONLY_REQUESTS, features)
        println("HAS_DELIVERYONLY_REQUESTS,")
    end
    if in(HAS_ALTERNATIVE_PICKUP_LOCATIONS, features)
        println("HAS_ALTERNATIVE_PICKUP_LOCATIONS,")
    end
    if in(HAS_ALTERNATIVE_DELIVERY_LOCATIONS, features)
        println("HAS_ALTERNATIVE_DELIVERY_LOCATIONS,")
    end
    if in(HAS_MAX_DURATION, features)
        println("HAS_MAX_DURATION,")
    end
    if in(HAS_DURATION_UNIT_COSTS, features)
        println("HAS_DURATION_UNIT_COSTS,")
    end
    if in(HAS_PICKUP_TIME_WINDOWS, features)
        println("HAS_PICKUP_TIME_WINDOWS,")
    end
    if in(HAS_DELIVERY_TIME_WINDOWS, features)
        println("HAS_DELIVERY_TIME_WINDOWS,")
    end
    if in(HAS_MULTIPLE_PICKUP_TIME_WINDOWS, features)
        println("HAS_MULTIPLE_PICKUP_TIME_WINDOWS,")
    end
    if in(HAS_MULTIPLE_DELIVERY_TIME_WINDOWS, features)
        println("HAS_MULTIPLE_DELIVERY_TIME_WINDOWS,")
    end
    if in(HAS_FLEXIBLE_PICKUP_TIME_WINDOWS, features)
        println("HAS_FLEXIBLE_PICKUP_TIME_WINDOWS,")
    end
    if in(HAS_FLEXIBLE_DELIVERY_TIME_WINDOWS, features)
        println("HAS_FLEXIBLE_DELIVERY_TIME_WINDOWS,")
    end
    if in(HAS_FLEXIBLE_REQUESTS, features)
        println("HAS_FLEXIBLE_REQUESTS,")
    end

    # VehicleCategory based features
    if in(HAS_VEHICLE_CAPACITIES, features)
        println("HAS_VEHICLE_CAPACITIES,")
    end
    if in(HAS_VEHICLE_PROPERTIES, features)
        println("HAS_VEHICLE_PROPERTIES,")
    end
    if in(HAS_COMPARTMENT_CAPACITIES, features)
        println("HAS_COMPARTMENT_CAPACITIES,")
    end
    if in(HAS_COMPARTMENT_PROPERTIES, features)
        println("HAS_COMPARTMENT_PROPERTIES,")
    end
    if in(HAS_MULTIPLE_VEHICLE_CAPACITIES, features)
        println("HAS_MULTIPLE_VEHICLE_CAPACITIES,")
    end
    if in(HAS_MULTIPLE_VEHICLE_PROPERTIES, features)
        println("HAS_MULTIPLE_VEHICLE_PROPERTIES,")
    end
    if in(HAS_MULTIPLE_COMPARTMENT_CAPACITIES, features)
        println("HAS_MULTIPLE_COMPARTMENT_CAPACITIES,")
    end
    if in(HAS_MULTIPLE_COMPARTMENT_PROPERTIES, features)
        println("HAS_MULTIPLE_COMPARTMENT_PROPERTIES,")
    end

    # HomogeneousVehicleSet based features
    if in(HAS_TRAVEL_TIME_UNIT_COST, features)
        println("HAS_TRAVEL_TIME_UNIT_COST,")
    end
    if in(HAS_SERVICE_TIME_UNIT_COST, features)
        println("HAS_SERVICE_TIME_UNIT_COST,")
    end
    if in(HAS_WAITING_TIME_UNIT_COST, features)
        println("HAS_WAITING_TIME_UNIT_COST,")
    end
    if in(HAS_TRAVEL_DISTANCE_UNIT_COST, features)
        println("HAS_TRAVEL_DISTANCE_UNIT_COST,")
    end
    if in(HAS_MAX_WORKING_TIME, features)
        println("HAS_MAX_WORKING_TIME,")
    end
    if in(HAS_MAX_TRAVEL_DISTANCE, features)
        println("HAS_MAX_TRAVEL_DISTANCE,")
    end
    if in(HAS_MIN_NB_VEHICLES, features)
        println("HAS_MIN_NB_VEHICLES,")
    end
    if in(HAS_MAX_NB_VEHICLES, features)
        println("HAS_MAX_NB_VEHICLES,")
    end
    if in(HAS_FLEXIBLE_NB_VEHICLES_RANGE, features)
        println("HAS_FLEXIBLE_NB_VEHICLES_RANGE,")
    end
    if in(HAS_WORKING_TIME_WINDOW, features)
        println("HAS_WORKING_TIME_WINDOW,")
    end
    if in(HAS_FLEXIBLE_WORKING_TIME_WINDOWS, features)
        println("HAS_FLEXIBLE_WORKING_TIME_WINDOWS,")
    end
    if in(HAS_FIXED_COST_PER_VEHICLE, features)
        println("HAS_FIXED_COST_PER_VEHICLE,")
    end

    # Instance based features
    if in(HAS_WORK_PERIODS, features)
        println("HAS_WORK_PERIODS,")
    end
    if in(HAS_MUTLIPLE_TRAVEL_TIME_PERIODS, features)
        println("HAS_MUTLIPLE_TRAVEL_TIME_PERIODS,")
    end
    # if in(HAS_VEHICLE_CATEGORIES, features)
    #     println("HAS_VEHICLE_CATEGORIES,")
    # end
    if in(HAS_MULTIPLE_VEHICLE_CATEGORIES, features)
        println("HAS_MULTIPLE_VEHICLE_CATEGORIES,")
    end
    if in(HAS_MULTIPLE_VEHICLE_SETS, features)
        println("HAS_MULTIPLE_VEHICLE_SETS,")
    end
    if in(HAS_ENERGY_FEATURES, features)
        println("HAS_ENERGY_FEATURES,")
    end
end


#======================================#
#             Random Utils             #
#======================================#

function thinking(_message ::String, _end_message ::String)
  for x in 1:rand(2:6) #amount of times we want the full string dots to show
    for i in 1:4 #the max amount of dots we actually want to show on the screen
      print("$_message $("." ^ (i-1)) \r") # Stack overflow answer SHOULD work
      sleep((rand(0.2:0.3)))
    end
  end
  println("$_end_message")
end

function determine_first_player()
  if rand(1:2) == 1
    print("The first player is you!")
    return 1
  else
    print("The first player is the computer!")
    return 2
  end
end

function display_info(_userCard, _compCard, _userChances, _compChances)
  print(
    """\n\t
    \t#==============#\t#==============#
    \t      You    \t\t     Computer
    \t              \t\t
    \t Health: $(_userCard.Health)   \t\t Health: $(_compCard.Health)
    \t Attack: $(_userCard.Attack)   \t\t Attack: $(_compCard.Attack)
    \t Defence: $(_userCard.Defence)  \t\t Defence: $(_compCard.Defence)
    \t Speed: $(_userCard.Speed)    \t\t Speed: $(_compCard.Speed)
    \t Crit: $(_userCard.Crit)     \t\t Crit: $(_compCard.Crit)
    \t#==============#\t#==============#\n\n
    """)
end


"""
    System{TF, TV<:AbstractVector{TF}, TM<:AbstractMatrix{TF}, TTF<:AbstractVector{TF}}

Contains the system state, residual vector, and jacobian matrices as well as
pointers to be able to access their contents.  Also contains additional storage
needed for time domain simulations.

# Fields:
 - `static`: Flag indicating whether system matrices are only valid for static analyses
 - `x`: State vector
 - `r`: Residual vector
 - `K`: System jacobian matrix with respect to the state variables
 - `M`: System jacobian matrix with respect to the time derivative of the state variables
 - `irow_pt`: Row index of first equilibrium equation for each point
 - `irow_beam`: Row index of first equation for just this beam element
 - `irow_beam1`: Row index of first equation for the left side of each beam
 - `irow_beam2`: Row index of first equation for the right side of each beam
 - `icol_pt`: Row/Column index of first state variable for each point
 - `icol_beam`: Row/Column index of first state variable for each beam element
 - `current_step`: Current time step
 - `udot_init`: `2/dt*u + udot` for each beam element from the previous time step
 - `θdot_init`: `2/dt*θ + θdot` for each beam element from the previous time step
 - `CtCabPdot_init`: `2/dt*C'*Cab*P + C'*Cab*Pdot` for each beam element from the previous time step
 - `CtCabHdot_init`: `2/dt*C'*Cab*H + C'*Cab*Hdot` for each beam element from the previous time step
"""
struct System{TF, TV<:AbstractVector{TF}, TM<:AbstractMatrix{TF}}
    static::Bool
    x::TV
    r::TV
    K::TM
    M::TM
    irow_pt::Vector{Int}
    irow_beam::Vector{Int}
    irow_beam1::Vector{Int}
    irow_beam2::Vector{Int}
    icol_pt::Vector{Int}
    icol_beam::Vector{Int}
    current_step::Ref{Int}
    udot_init::Vector{SVector{3,TF}}
    θdot_init::Vector{SVector{3,TF}}
    CtCabPdot_init::Vector{SVector{3,TF}}
    CtCabHdot_init::Vector{SVector{3,TF}}
end
Base.eltype(::System{TF, TV, TM}) where {TF, TV, TM} = TF

"""
    System([TF=eltype(assembly),] assembly, points, static)

Initialize an object of type `System` which stores the system state, residual vector,
current time function values,and jacobian matrices as well as pointers to
be able to access their contents.

# Arguments:
 - `TF:` (optional) Used to specify floating point type used by resulting `System` object
 - `assembly`: Assembly of rigidly connected nonlinear beam elements
 - `points`: Point indices which should be preserved in the system of equations.
        All points with prescribed conditions should be included.
 - `static`: Flag indicating whether system matrices will be used for static simulations
"""
System(assembly, points, static) = System(eltype(assembly), assembly, points, static)

function System(TF::Type{<:AbstractFloat}, assembly, points, static)

    # get number of beams
    nbeam = length(assembly.elements)

    # get the number of beams connected to each point.
    n_connections = point_connections(assembly)

    # get the size and pointers into the system matrices
    n, irow_pt, irow_beam, irow_beam1, irow_beam2, icol_pt, icol_beam =
        system_indices(assembly, points, n_connections, static)

    # initialize system matrices
    x = zeros(TF, n)
    r = zeros(TF, n)
    K = spzeros(TF, n, n)
    M = spzeros(TF, n, n)

    # initialize pointer to value of current time step
    current_step = Ref(0)

    # initialize storage for time domain simulations
    udot_init = [@SVector zeros(TF, 3) for i = 1:nbeam]
    θdot_init = [@SVector zeros(TF, 3) for i = 1:nbeam]
    CtCabPdot_init = [@SVector zeros(TF, 3) for i = 1:nbeam]
    CtCabHdot_init = [@SVector zeros(TF, 3) for i = 1:nbeam]

    return System(static, x, r, K, M, irow_pt, irow_beam, irow_beam1, irow_beam2,
        icol_pt, icol_beam, current_step, udot_init, θdot_init, CtCabPdot_init,
        CtCabHdot_init)
end

"""
   reset_state!(system)

Reset the state variables in `system` (stored in `system.x`) to zero.
"""
function reset_state!(system)
    system.x .= 0
    return system
end

"""
    point_connections(assembly)

Count the number of beams connected to each point
"""
function point_connections(assembly)

    npt = length(assembly.points)

    n_connections = Array{Int, 1}(undef, npt)

    for ipt = 1:npt
        n_connections[ipt] = count(x -> x == ipt, assembly.start) + count(x -> x == ipt, assembly.stop)
    end

    return n_connections
end

"""
    system_indices(assembly, points, n_connections, static)

Solve for the row indices of the first equilibrium or compatability equations for
each point and side of each beam element.  Also solve for the row/column index of
each point and beam state variable.

Note that this function includes the following logic which reduces the size of
the system of equations where possible (without sacrificing any accuracy):

If only two beams meet at a point, the 6 unknowns associated with that point as
well as the 6 compatability equations are eliminated from the system, except if
specified in the array `points`.  Points for which unknowns have been
eliminated are assigned a column index of -1.  Beams for which the compatability
equations have been eliminated are also assigned an index of -1

# Arguments:
 - `assembly`: Assembly of rigidly connected nonlinear beam elements
 - `points`: Point indices which should be preserved in the system matrices
 - `n_connections`: Number of connections to each point
 - `static`: flag indicating whether analysis is static

# Return Arguments:
 - `n`: total number of equations/unknowns in the system
 - `irow_pt`: Row index of first equilibrium equation for each point
 - `irow_beam`: Row index of first equation for just this beam element
 - `irow_beam1`: Row index of first equation for the left side of each beam
 - `irow_beam2`: Row index of first equation for the right side of each beam
 - `icol_pt`: Column index of first state variable for each point
 - `icol_beam`: Column index of first state variable for each beam element
"""
function system_indices(assembly, points, n_connections, static)

    npt = length(assembly.points)
    nbeam = length(assembly.elements)

    assigned = fill(false, npt)

    irow_pt = Array{Int, 1}(undef, npt)
    irow_beam = Array{Int, 1}(undef, nbeam)
    irow_beam1 = Array{Int, 1}(undef, nbeam)
    irow_beam2 = Array{Int, 1}(undef, nbeam)

    icol_pt = Array{Int, 1}(undef, npt)
    icol_beam = Array{Int, 1}(undef, nbeam)

    irow = 1
    icol = 1
    for ibeam = 1:nbeam
        ipt = assembly.start[ibeam]
        if !assigned[ipt]
            assigned[ipt] = true
            # 6 equilibrium equations + 6 compatability equations
            irow_pt[ipt] = irow
            irow_beam1[ibeam] = irow
            irow += 12
            # add unknowns for each point
            if n_connections[ipt] == 2 && !(ipt in points)
                # no additional unknowns when only two beams meet at a point
                icol_pt[ipt] = -1
            else
                # 6 additional unknowns for each point
                icol_pt[ipt] = icol
                icol += 6
            end
        else
            # add compatability equations
            if n_connections[ipt] == 2 && !(ipt in points)
                # no additional equations when only two beams meet at a point
                irow_beam1[ibeam] = -1
            else
                # 6 additional compatibility equations
                irow_beam1[ibeam] = irow
                irow += 6
            end
        end

        # 12 unknowns for each element
        icol_beam[ibeam] = icol
        icol += 12

        if static
            irow_beam[ibeam] = -1
        else
            # 6 linear and angular momentum residual equations for each element
            irow_beam[ibeam] = irow
            irow += 6
            # 6 additional unknowns for each member for unsteady analyses
            icol += 6
        end

        ipt = assembly.stop[ibeam]
        if !assigned[ipt]
            assigned[ipt] = true
            # 6 equilibrium equations + 6 compatability equations
            irow_pt[ipt] = irow
            irow_beam2[ibeam] = irow
            irow += 12
            # add unknowns for each point
            if n_connections[ipt] == 2 && !(ipt in points)
                # no additional unknowns when only two beams meet at a point
                icol_pt[ipt] = -1
            else
                # 6 additional unknowns for each point
                icol_pt[ipt] = icol
                icol += 6
            end
        else
            if n_connections[ipt] == 2 && !(ipt in points)
                # no additional compatability equations when only two beams meet at a point
                irow_beam2[ibeam] = -1
            else
                # 6 additional compatibility equations
                irow_beam2[ibeam] = irow
                irow += 6
            end
        end
    end

    # number of unknowns/equations
    n = irow - 1

    return n, irow_pt, irow_beam, irow_beam1, irow_beam2, icol_pt, icol_beam
end

"""
    system_residual!(resid, x, assembly, prescribed_conditions, distributed_loads,
        istep, irow_pt, irow_beam, irow_beam1, irow_beam2, icol_pt, icol_beam)
    system_residual!(resid, x, assembly, prescribed_conditions, distributed_loads,
        istep, irow_pt, irow_beam, irow_beam1, irow_beam2, icol_pt, icol_beam, x0, v0, ω0)
    system_residual!(resid, x, assembly, prescribed_conditions, distributed_loads,
        istep, irow_pt, irow_beam, irow_beam1, irow_beam2, icol_pt, icol_beam, x0, v0, ω0,
        u, θ, udot, θdot)
    system_residual!(resid, x, assembly, prescribed_conditions, distributed_loads,
        istep, irow_pt, irow_beam, irow_beam1, irow_beam2, icol_pt, icol_beam, x0, v0, ω0,
        udot, θdot_init, CtCabPdot, CtCabHdot, dt)

Populate the residual vector `resid` with the results of the residual equations
for the system.

There are four implementations corresponding to the following analysis types:
 - Static
 - Dynamic - Steady State
 - Dynamic - Initial Step (for initializing time domain simulations)
 - Dynamic - Time Marching

 See "GEBT: A general-purpose nonlinear analysis tool for composite beams" by
 Wenbin Yu and "Geometrically nonlinear analysis of composite beams using
 Wiener-Milenković parameters" by Qi Wang and Wenbin Yu.

# Arguments
 - `resid`: System residual vector
 - `x`: Current state variables of the system
 - `assembly`: Assembly of rigidly connected nonlinear beam elements
 - `prescribed_conditions`: Dictionary of prescribed conditions at all time steps
 - `distributed_loads`: Dictionary of distributed loads at all time steps
 - `istep`: Current time step
 - `irow_pt`: Row index of first equilibrium equation for each point
 - `irow_beam`: Row index of first equation for just this beam element
 - `irow_beam1`: Row index of first equation for the left side of each beam
 - `irow_beam2`: Row index of first equation for the right side of each beam
 - `icol_pt`: Column index of first state variable for each point
 - `icol_beam`: Column index of first state variable for each beam element

# Additional Arguments for Dynamic Analyses
 - `x0`: Global frame origin (for the current time step)
 - `v0`: Global frame linear velocity (for the current time step)
 - `ω0`: Global frame angular velocity (for the current time step)

# Additional Arguments for Initial Step Analyses
 - `u`: deflection variables for each beam element
 - `θ`: rotation variables for each beam element
 - `udot`: time derivative of u for each beam element
 - `θdot`: time derivative of θ for each beam element

# Additional Arguments for Time Marching Analyses
 - `udot`: `-2/dt*u - udot` for each beam element from the previous time step
 - `θdot_init`: `-2/dt*θ - θdot` for each beam element from the previous time step
 - `CtCabPdot`: `-2/dt*C'*Cab*P - C'*Cab*Pdot` for each beam element from the previous time step
 - `CtCabHdot`: `-2/dt*C'*Cab*H - C'*Cab*Hdot` for each beam element from the previous time step
 - `dt`: time step size
"""
system_residual!

# static
function system_residual!(resid, x, assembly, prescribed_conditions, distributed_loads,
    istep, irow_pt, irow_beam, irow_beam1, irow_beam2, icol_pt, icol_beam)

    npoint = length(assembly.points)
    nbeam = length(assembly.elements)

    # add contributions to residual equations from the elements
    for ibeam = 1:nbeam

        # get pointers for element
        icol = icol_beam[ibeam]
        irow_b = irow_beam[ibeam]
        irow_b1 = irow_beam1[ibeam]
        irow_p1 = irow_pt[assembly.start[ibeam]]
        irow_b2 = irow_beam2[ibeam]
        irow_p2 = irow_pt[assembly.stop[ibeam]]

        resid = element_residual!(resid, x, ibeam, assembly.elements[ibeam],
             distributed_loads, istep, icol, irow_b, irow_b1,
            irow_p1, irow_b2, irow_p2)
    end

    # add contributions to the residual equations from the prescribed point conditions
    for ipoint = 1:npoint

        # skip if the unknowns have been eliminated from the system of equations
        if icol_pt[ipoint] <= 0
            continue
        end

        icol = icol_pt[ipoint]
        irow_p = irow_pt[ipoint]

        resid = point_residual!(resid, x, ipoint, assembly, prescribed_conditions,
            istep, icol, irow_p, irow_beam1, irow_beam2)
    end

    return resid
end

# dynamic
function system_residual!(resid, x, assembly, prescribed_conditions, distributed_loads,
    istep, irow_pt, irow_beam, irow_beam1, irow_beam2, icol_pt, icol_beam,
    x0, v0, ω0)

    npoint = length(assembly.points)
    nbeam = length(assembly.elements)

    # add contributions to residual equations from the elements
    for ibeam = 1:nbeam

        # get pointers for element
        icol = icol_beam[ibeam]
        irow_b = irow_beam[ibeam]
        irow_b1 = irow_beam1[ibeam]
        irow_p1 = irow_pt[assembly.start[ibeam]]
        irow_b2 = irow_beam2[ibeam]
        irow_p2 = irow_pt[assembly.stop[ibeam]]

        resid = element_residual!(resid, x, ibeam, assembly.elements[ibeam],
             distributed_loads, istep, icol, irow_b, irow_b1,
            irow_p1, irow_b2, irow_p2, x0, v0, ω0)
    end

    # add contributions to the residual equations from the prescribed point conditions
    for ipoint = 1:npoint

        # skip if the unknowns have been eliminated from the system of equations
        if icol_pt[ipoint] <= 0
            continue
        end

        icol = icol_pt[ipoint]
        irow_p = irow_pt[ipoint]

        resid = point_residual!(resid, x, ipoint, assembly, prescribed_conditions,
            istep, icol, irow_p, irow_beam1, irow_beam2)
    end

    return resid
end

# initial step
function system_residual!(resid, x, assembly, prescribed_conditions, distributed_loads,
    istep, irow_pt, irow_beam, irow_beam1, irow_beam2, icol_pt, icol_beam,
    x0, v0, ω0, u, θ, udot, θdot)

    npoint = length(assembly.points)
    nbeam = length(assembly.elements)

    # add contributions to residual equations from the elements
    for ibeam = 1:nbeam

        # get pointers for element
        icol = icol_beam[ibeam]
        irow_b = irow_beam[ibeam]
        irow_b1 = irow_beam1[ibeam]
        irow_p1 = irow_pt[assembly.start[ibeam]]
        irow_b2 = irow_beam2[ibeam]
        irow_p2 = irow_pt[assembly.stop[ibeam]]

        resid = element_residual!(resid, x, ibeam, assembly.elements[ibeam],
             distributed_loads, istep, icol, irow_b, irow_b1,
            irow_p1, irow_b2, irow_p2, x0, v0, ω0, u, θ, udot, θdot)
    end

    # add contributions to the residual equations from the prescribed point conditions
    for ipoint = 1:npoint

        # skip if the unknowns have been eliminated from the system of equations
        if icol_pt[ipoint] <= 0
            continue
        end

        icol = icol_pt[ipoint]
        irow_p = irow_pt[ipoint]

        resid = point_residual!(resid, x, ipoint, assembly, prescribed_conditions,
            istep, icol, irow_p, irow_beam1, irow_beam2)
    end

    return resid
end

# time marching
function system_residual!(resid, x, assembly, prescribed_conditions, distributed_loads,
    istep, irow_pt, irow_beam, irow_beam1, irow_beam2, icol_pt, icol_beam,
    x0, v0, ω0, udot, θdot_init, CtCabPdot, CtCabHdot, dt)

    nbeam = length(assembly.elements)
    npoint = length(assembly.points)

    # add contributions to residual equations from the beam elements
    for ibeam = 1:nbeam

        # get pointers for element
        icol = icol_beam[ibeam]
        irow_b = irow_beam[ibeam]
        irow_b1 = irow_beam1[ibeam]
        irow_p1 = irow_pt[assembly.start[ibeam]]
        irow_b2 = irow_beam2[ibeam]
        irow_p2 = irow_pt[assembly.stop[ibeam]]

        resid = element_residual!(resid, x, ibeam, assembly.elements[ibeam],
             distributed_loads, istep, icol, irow_b, irow_b1,
            irow_p1, irow_b2, irow_p2, x0, v0, ω0, udot, θdot_init, CtCabPdot,
            CtCabHdot, dt)
    end

    # add contributions to the residual equations from the prescribed point conditions
    for ipoint = 1:npoint

        # skip if the unknowns have been eliminated from the system of equations
        if icol_pt[ipoint] <= 0
            continue
        end

        icol = icol_pt[ipoint]
        irow_p = irow_pt[ipoint]

        resid = point_residual!(resid, x, ipoint, assembly, prescribed_conditions,
            istep, icol, irow_p, irow_beam1, irow_beam2)
    end

    return resid
end

"""
    system_jacobian!(jacob, x, assembly, prescribed_conditions, distributed_loads,
        istep, irow_pt, irow_beam, irow_beam1, irow_beam2, icol_pt, icol_beam)
    system_jacobian!(jacob, x, assembly, prescribed_conditions, distributed_loads,
        istep, irow_pt, irow_beam, irow_beam1, irow_beam2, icol_pt, icol_beam,
        x0, v0, ω0)
    system_jacobian!(jacob, x, assembly, prescribed_conditions, distributed_loads,
        istep, irow_pt, irow_beam, irow_beam1, irow_beam2, icol_pt, icol_beam,
        x0, v0, ω0, u, θ, udot, θdot)
    system_jacobian!(jacob, x, assembly, prescribed_conditions, distributed_loads,
        istep, irow_pt, irow_beam, irow_beam1, irow_beam2, icol_pt, icol_beam,
        x0, v0, ω0, udot_init, θdot_init, CtCabPdot_init, CtCabHdot_init, dt)

Populate the jacobian matrix `jacob` with the jacobian of the residual vector
with respect to the state variables.

There are four implementations corresponding to the following analysis types:
 - Static
 - Dynamic - Steady State
 - Dynamic - Initial Step (for initializing time domain simulations)
 - Dynamic - Time Marching

 See "GEBT: A general-purpose nonlinear analysis tool for composite beams" by
 Wenbin Yu and "Geometrically nonlinear analysis of composite beams using
 Wiener-Milenković parameters" by Qi Wang and Wenbin Yu.

# Arguments
 - `jacob`: Jacobian matrix
 - `x`: Vector containing current state variables of the system
 - `assembly`: Assembly of rigidly connected nonlinear beam elements
 - `prescribed_conditions`: Dictionary of prescribed conditions at all time steps
 - `distributed_loads`: Dictionary of distributed loads at all time steps
 - `istep`: Current time step
 - `irow_pt`: Row index of first equilibrium equation for each point
 - `irow_beam`: Row index of first equation for just this beam element
 - `irow_beam1`: Row index of first equation for the left side of each beam
 - `irow_beam2`: Row index of first equation for the right side of each beam
 - `icol_pt`: Column index of first state variable for each point
 - `icol_beam`: Column index of first state variable for each beam element

# Additional Arguments for Dynamic Analyses
 - `x0`: Global frame origin (for the current time step)
 - `v0`: Global frame linear velocity (for the current time step)
 - `ω0`: Global frame angular velocity (for the current time step)

# Additional Arguments for Initial Step Analyses
 - `u`: deflection variables for each beam element
 - `θ`: rotation variables for each beam element
 - `udot`: time derivative of u for each beam element
 - `θdot`: time derivative of θ for each beam element

# Additional Arguments for Time Marching Analyses
 - `udot`: `-2/dt*u - udot` for each beam element from the previous time step
 - `θdot_init`: `-2/dt*θ - θdot` for each beam element from the previous time step
 - `CtCabPdot`: `-2/dt*C'*Cab*P - C'*Cab*Pdot` for each beam element from the previous time step
 - `CtCabHdot`: `-2/dt*C'*Cab*H - C'*Cab*Hdot` for each beam element from the previous time step
 - `dt`: time step size
"""
system_jacobian!

# static
function system_jacobian!(jacob, x, assembly, prescribed_conditions, distributed_loads,
    istep, irow_pt, irow_beam, irow_beam1, irow_beam2, icol_pt, icol_beam)

    npoint = length(assembly.points)
    nbeam = length(assembly.elements)

    # add contributions to residual equations from the elements
    for ibeam = 1:nbeam

        icol = icol_beam[ibeam]
        irow_b = irow_beam[ibeam]
        irow_b1 = irow_beam1[ibeam]
        irow_p1 = irow_pt[assembly.start[ibeam]]
        irow_b2 = irow_beam2[ibeam]
        irow_p2 = irow_pt[assembly.stop[ibeam]]

        jacob = element_jacobian!(jacob, x, ibeam, assembly.elements[ibeam],
            distributed_loads, istep, icol, irow_b, irow_b1,
            irow_p1, irow_b2, irow_p2)
    end

    # add contributions to the system jacobian matrix from the prescribed point conditions
    for ipoint = 1:npoint

        # skip if the unknowns have been eliminated from the system of equations
        if icol_pt[ipoint] <= 0
            continue
        end

        icol = icol_pt[ipoint]
        irow_p = irow_pt[ipoint]

        jacob = point_jacobian!(jacob, x, ipoint, assembly, prescribed_conditions,
            istep, icol, irow_p, irow_beam1, irow_beam2)

    end

    return jacob
end

# dynamic
function system_jacobian!(jacob, x, assembly, prescribed_conditions, distributed_loads,
    istep, irow_pt, irow_beam, irow_beam1, irow_beam2, icol_pt, icol_beam,
    x0, v0, ω0)

    npoint = length(assembly.points)
    nbeam = length(assembly.elements)

    # add contributions to residual equations from the elements
    for ibeam = 1:nbeam

        icol = icol_beam[ibeam]
        irow_b = irow_beam[ibeam]
        irow_b1 = irow_beam1[ibeam]
        irow_p1 = irow_pt[assembly.start[ibeam]]
        irow_b2 = irow_beam2[ibeam]
        irow_p2 = irow_pt[assembly.stop[ibeam]]

        jacob = element_jacobian!(jacob, x, ibeam, assembly.elements[ibeam],
            distributed_loads, istep, icol, irow_b, irow_b1,
            irow_p1, irow_b2, irow_p2, x0, v0, ω0)
    end

    # add contributions to the system jacobian matrix from the prescribed point conditions
    for ipoint = 1:npoint

        # skip if the unknowns have been eliminated from the system of equations
        if icol_pt[ipoint] <= 0
            continue
        end

        icol = icol_pt[ipoint]
        irow_p = irow_pt[ipoint]

        jacob = point_jacobian!(jacob, x, ipoint, assembly, prescribed_conditions,
            istep, icol, irow_p, irow_beam1, irow_beam2)
    end

    return jacob
end

# initial step
function system_jacobian!(jacob, x, assembly, prescribed_conditions, distributed_loads,
    istep, irow_pt, irow_beam, irow_beam1, irow_beam2, icol_pt, icol_beam,
    x0, v0, ω0, u, θ, udot, θdot)

    npoint = length(assembly.points)
    nbeam = length(assembly.elements)

    # add contributions to residual equations from the elements
    for ibeam = 1:nbeam

        icol = icol_beam[ibeam]
        irow_b = irow_beam[ibeam]
        irow_b1 = irow_beam1[ibeam]
        irow_p1 = irow_pt[assembly.start[ibeam]]
        irow_b2 = irow_beam2[ibeam]
        irow_p2 = irow_pt[assembly.stop[ibeam]]

        jacob = element_jacobian!(jacob, x, ibeam, assembly.elements[ibeam],
            distributed_loads, icol, irow_b, irow_b1,
            irow_p1, irow_b2, irow_p2, x0, v0, ω0, u, θ, udot, θdot)
    end

    # add contributions to the system jacobian matrix from the prescribed point conditions
    for ipoint = 1:npoint

        # skip if the unknowns have been eliminated from the system of equations
        if icol_pt[ipoint] <= 0
            continue
        end

        icol = icol_pt[ipoint]
        irow_p = irow_pt[ipoint]

        jacob = point_jacobian!(jacob, x, ipoint, assembly, prescribed_conditions,
            istep, icol, irow_p, irow_beam1, irow_beam2)
    end

    return jacob
end

# time marching
function system_jacobian!(jacob, x, assembly, prescribed_conditions, distributed_loads,
    istep, irow_pt, irow_beam, irow_beam1, irow_beam2, icol_pt, icol_beam,
    x0, v0, ω0, udot_init, θdot_init, CtCabPdot_init, CtCabHdot_init, dt)

    npoint = length(assembly.points)
    nbeam = length(assembly.elements)

    # add contributions to residual equations from the elements
    for ibeam = 1:nbeam

        icol = icol_beam[ibeam]
        irow_b = irow_beam[ibeam]
        irow_b1 = irow_beam1[ibeam]
        irow_p1 = irow_pt[assembly.start[ibeam]]
        irow_b2 = irow_beam2[ibeam]
        irow_p2 = irow_pt[assembly.stop[ibeam]]

        jacob = element_jacobian!(jacob, x, ibeam, assembly.elements[ibeam],
            distributed_loads, istep, icol, irow_b, irow_b1,
            irow_p1, irow_b2, irow_p2, x0, v0, ω0, udot_init, θdot_init,
            CtCabPdot_init, CtCabHdot_init, dt)
    end

    # add contributions to the system jacobian matrix from the prescribed point conditions
    for ipoint = 1:npoint

        # skip if the unknowns have been eliminated from the system of equations
        if icol_pt[ipoint] <= 0
            continue
        end

        icol = icol_pt[ipoint]
        irow_p = irow_pt[ipoint]

        jacob = point_jacobian!(jacob, x, ipoint, assembly, prescribed_conditions,
            istep, icol, irow_p, irow_beam1, irow_beam2)
    end

    # zero out near-zero values ( < eps() )
    jacob = droptol!(jacob, eps(eltype(jacob)))

    return jacob
end

"""
    system_mass_matrix!(jacob, x, assembly, irow_pt, irow_beam, irow_beam1,
        irow_beam2, icol_pt, icol_beam)

Populate the system "mass matrix", the jacobian of the residual vector
with respect to the time derivatives of the state variables.

See "GEBT: A general-purpose nonlinear analysis tool for composite beams" by
Wenbin Yu and "Geometrically nonlinear analysis of composite beams using
Wiener-Milenković parameters" by Qi Wang and Wenbin Yu.

# Arguments
 - `jacob`: Jacobian matrix
 - `x`: Vector containing current state variables of the system
 - `assembly`: Assembly of rigidly connected nonlinear beam elements
 - `irow_pt`: Row index of first equilibrium equation for each point
 - `irow_beam`: Row index of first equation for just this beam element
 - `irow_beam1`: Row index of first equation for the left side of each beam
 - `irow_beam2`: Row index of first equation for the right side of each beam
 - `icol_pt`: Column index of first state variable for each point
 - `icol_beam`: Column index of first state variable for each beam element
"""
function system_mass_matrix!(jacob, x, assembly, irow_pt, irow_beam, irow_beam1,
    irow_beam2, icol_pt, icol_beam)

    npoint = length(assembly.points)
    nbeam = length(assembly.elements)

    # add contributions to "mass matrix" from the beam elements
    for ibeam = 1:nbeam

        icol = icol_beam[ibeam]
        irow_b = irow_beam[ibeam]
        irow_b1 = irow_beam1[ibeam]
        irow_p1 = irow_pt[assembly.start[ibeam]]
        irow_b2 = irow_beam2[ibeam]
        irow_p2 = irow_pt[assembly.stop[ibeam]]

        element_mass_matrix!(jacob, x, assembly.elements[ibeam], icol, irow_b,
            irow_b1, irow_p1, irow_b2, irow_p2)
    end

    # no contributions to "mass matrix" from point state variables

    # zero out near-zero values ( < eps() )
    jacob = droptol!(jacob, eps(eltype(jacob)))

    return jacob
end


# This file is a part of JuliaFEM.
# License is MIT: see https://github.com/JuliaFEM/Materials.jl/blob/master/LICENSE

module Materials

using LinearAlgebra, ForwardDiff, Tensors, NLsolve, Parameters

abstract type AbstractMaterial end
abstract type AbstractMaterialState end

@generated function Base.:+(state::T, dstate::T) where {T <: AbstractMaterialState}
   expr = [:(state.$p + dstate.$p) for p in fieldnames(T)]
   return :(T($(expr...)))
end

export AbstractMaterial, AbstractMaterialState

function integrate_material!(material::M) where {M<:AbstractMaterial}
    error("One needs to define how to integrate material $M!")
end

function update_material!(material::M) where {M <: AbstractMaterial}
    material.drivers += material.ddrivers
    material.parameters += material.dparameters
    material.variables = material.variables_new
    reset_material!(material)
    return nothing
end

function reset_material!(material::M) where {M <: AbstractMaterial}
    material.ddrivers = typeof(material.ddrivers)()
    material.dparameters = typeof(material.dparameters)()
    material.variables_new = typeof(material.variables_new)()
    return nothing
end

export integrate_material!, update_material!, reset_material!

function isotropic_elasticity_tensor(lambda, mu)
    delta(i,j) = i==j ? 1.0 : 0.0
    g(i,j,k,l) = lambda*delta(i,j)*delta(k,l) + mu*(delta(i,k)*delta(j,l)+delta(i,l)*delta(j,k))
    jacobian = SymmetricTensor{4, 3, Float64}(g)
    return jacobian
end

include("idealplastic.jl")
export IdealPlastic, IdealPlasticDriverState, IdealPlasticParameterState, IdealPlasticVariableState

include("chaboche.jl")
export Chaboche, ChabocheDriverState, ChabocheParameterState, ChabocheVariableState

# include("viscoplastic.jl")
# export ViscoPlastic

include("uniaxial_increment.jl")
export uniaxial_increment!

include("biaxial_increment.jl")
export biaxial_increment!

include("stress_driven_uniaxial_increment.jl")
export stress_driven_uniaxial_increment!

include("DSA.jl")
export DSA, DSADriverState, DSAParameterState, DSAVariableState

end


################################################################################
#
#	CONCRETE TYPE
#
#   BrillouinZone{R}
#   --> R is the reciprocal unitcell type (<: AbstractReciprocalUnitcell{P,B})
#       --> P is the reciprocal site type (<: AbstractReciprocalPoint{D})
#           --> D is the dimension of embedding space
#       --> B is the bond type (<: AbstractBond{L,N})
#           --> N is the dimension of the Bravais lattice that the bond is located in
#           --> L is the label type of bonds
#
#   FILE CONTAINS
#       - abstract type definition
#       - interface implementation
#
################################################################################

################################################################################
#
#   ABSTRACT TYPE DEFINITION
#
################################################################################
mutable struct BrillouinZone{R} <: AbstractBrillouinZone{R}

    # reciprocal unitcell
    reciprocal_unitcell :: R

    # corners
    corners :: Vector{Vector{Float64}}

    # faces
    faces :: Vector{Vector{Int64}}

end

# export the type
export BrillouinZone

################################################################################
#
#	INTERFACING / ACCESSING BRILLOUIN ZONES
#	(functions that have to be overwritten by concrete types)
#
################################################################################

# constructor
function newBrillouinZone(
            ::Type{BrillouinZone{RU}},
            reciprocal_unitcell :: RU,
            corners             :: Vector{<:Vector{<:Real}},
            faces               :: Vector{<:Vector{<:Integer}}
        ) :: BrillouinZone{RU} where {D,N,L,P<:AbstractReciprocalPoint{D},B<:AbstractBond{L,N},RU<:AbstractReciprocalUnitcell{P,B}}

    # call the constructor
    return BrillouinZone{RU}(reciprocal_unitcell, corners, faces)
end

# export constructor
export newBrillouinZone

# accessing the corner points of the BZ
function corners(
            bz :: BrillouinZone{RU}
        ) :: Vector{Vector{Float64}} where {D,N,L,P<:AbstractReciprocalPoint{D},B<:AbstractBond{L,N},RU<:AbstractReciprocalUnitcell{P,B}}

    # return the corners
    return bz.corners
end

# setting the corner points of the BZ
function corners!(
            bz      :: BrillouinZone{RU},
            corners :: Vector{<:Vector{<:Real}}
        ) where {D,N,L,P<:AbstractReciprocalPoint{D},B<:AbstractBond{L,N},RU<:AbstractReciprocalUnitcell{P,B}}

    # set the corners
    bz.corners = corners
end

# export corner interface
export corners, corners!

# accessing the faces of the BZ
function faces(
            bz :: BrillouinZone{RU}
        ) :: Vector{Vector{Int64}} where {D,N,L,P<:AbstractReciprocalPoint{D},B<:AbstractBond{L,N},RU<:AbstractReciprocalUnitcell{P,B}}

    # return the faces
    return bz.faces
end

# setting the faces of the BZ
function faces!(
            bz    :: BrillouinZone{RU},
            faces :: Vector{<:Vector{<:Integer}}
        ) where {D,N,L,P<:AbstractReciprocalPoint{D},B<:AbstractBond{L,N},RU<:AbstractReciprocalUnitcell{P,B}}

    # set the faces
    bz.faces = faces
end

# export faces interface
export faces, faces!

# accessing the reciprocal unitcell of the BZ
function reciprocalUnitcell(
            bz :: BrillouinZone{RU}
        ) :: RU where {D,N,L,P<:AbstractReciprocalPoint{D},B<:AbstractBond{L,N},RU<:AbstractReciprocalUnitcell{P,B}}

    # return the reciprocal unitcell
    return bz.reciprocal_unitcell
end

# setting the reciprocal unitcell of the BZ
function reciprocalUnitcell!(
            bz  :: BrillouinZone{RU},
            ruc :: RU
        ) where {D,N,L,P<:AbstractReciprocalPoint{D},B<:AbstractBond{L,N},RU<:AbstractReciprocalUnitcell{P,B}}

    # set the reciprocal unitcell
    bz.reciprocal_unitcell = ruc
end

# export reciprocalUnitcell interface
export reciprocalUnitcell, reciprocalUnitcell!


# addprocs(6)

using Caesar, RoME
using TransformUtils
using Rotations, CoordinateTransformations
using PyLCM
using PyCall

using KernelDensityEstimate

using JLD, HDF5

@show lcmtpath = joinpath(dirname(@__FILE__),"lcmtypes")

# println("Compile LCM types")
# run(`lcm-gen -p --ppath $(lcmtpath) $(lcmtpath)/rome_pose_node_t.lcm`)
# run(`lcm-gen -p --ppath $(lcmtpath) $(lcmtpath)/rome_pose_pose_nh_t.lcm`)

println("Adding lcmtypes dir to Python path: $(lcmtpath)")
unshift!(PyVector(pyimport("sys")["path"]),lcmtpath)

@pyimport rome

function whatsthere(channel, msgdata)
  odomsg = rome.pose_pose_nh_t[:decode](msgdata)
  @show odomsg[:utime]
  nothing
end

# MSGDATA = Dict{Symbol, Vector{}}()
# MSGDATA[:pose_node_t] = []
# lastpose = SE3(0)

function lcmposenodes!(vc, slaml::SLAMWrapper, msgdata)
  # push!(MSGDATA[:pose_node_t], msgdata)
  posenode = rome.pose_node_t[:decode](msgdata)
  # @show posenode[:id]
  tA = [posenode[:mean][1];posenode[:mean][2];posenode[:mean][3]]
  EA = Euler(posenode[:mean][6], posenode[:mean][5], posenode[:mean][4])
  q = TransformUtils.convert(Quaternion, EA)
  newpose = SE3(tA, q)
  am = Translation(tA...) ∘ LinearMap(Quat(q.s,q.v...))
  # updaterealtime!(vc,:body, am)
  # visualizerealtime(vc)

  # Dx = slaml.lastpose\newpose # get delta transform between poses
  # copy!(slaml.lastpose.t, newpose.t)
  # copy!(slaml.lastpose.R.R, newpose.R.R)

  @show "here"
  if posenode[:id] == 0
    # first pose prior
    dcovar = [posenode[:covar][1];posenode[:covar][2];posenode[:covar][3];posenode[:covar][4];posenode[:covar][5];posenode[:covar][6]]
    # TODO -- this wont do, must add to existing slaml.fg
    slaml.fg = identitypose6fg(fg=slaml.fg, initpose=newpose, initCov=diagm(dcovar))

    @show length(slaml.fg.g.vertices)
    @show ls(slaml.fg)
    @show lbls, = ls(slaml.fg)
    slaml.usrid2lbl[posenode[:id]] = lbls[1]
    slaml.lbl2usrid[lbls[1]] = posenode[:id]
  end

  rovt( vc, am )
  # change to using pose prior and interpose translation only
  # println("Added node $(v.label)")
  nothing
end

function lcmodomsg!(vc, slaml::SLAMWrapper, msgdata)
  odomsg = rome.pose_pose_nh_t[:decode](msgdata)
  # @show odomsg[:mean_dim]
  # @show odomsg[:node_1_id], odomsg[:node_2_id]
  @show odomsg[:confidence]
  tA = [odomsg[:mean][1];odomsg[:mean][2];odomsg[:mean][3]]
  EA = Euler(odomsg[:mean][6], odomsg[:mean][5], odomsg[:mean][4])
  Dx = SE3(tA, EA)
  dcovar = [odomsg[:covar][1];odomsg[:covar][2];odomsg[:covar][3];odomsg[:covar][4];odomsg[:covar][5];odomsg[:covar][6]]

  if abs(odomsg[:confidence] - 1.0) < 1e-10
    # tA = [odomsg[:mean][1];odomsg[:mean][2];odomsg[:mean][3]]
    # EA = Euler(odomsg[:mean][6], odomsg[:mean][5], odomsg[:mean][4])
    # Dx = SE3(tA, EA)
    # dcovar = [odomsg[:covar][1];odomsg[:covar][2];odomsg[:covar][3];odomsg[:covar][4];odomsg[:covar][5];odomsg[:covar][6]]
    v,f = addOdoFG!(slaml, Pose3Pose3( MvNormal([tA; EA], diagm(dcovar)^2) ) , saveusrid=odomsg[:node_2_id] )
    @show ls(slaml.fg)
  else
    # loop closure case
    println("LOOP CLOSURE")
    @show odomsg[:node_1_id], odomsg[:node_2_id]
    odoc3 = Pose3Pose3NH( MvNormal(veeEuler(Dx), diagm(dcovar)), [0.5;0.5]) # define 50/50% hypothesis
    addFactor!(slam.fg,[Symbol("x$(odomsg[:node_1_id]+1)");Symbol("x$(odomsg[:node_2_id]+1)")],odoc3)
  end
  visualizeallposes!(vc, slaml.fg)

  nothing
end

function runlistener(
      fl::Vector{Bool},
      slam::SLAMWrapper,
      vc;
      until::Symbol=:x999 ) #slam::SLAMWrapper)
  #
  gg = (channel, msg_data) -> lcmposenodes!(vc, slam, msg_data)

  odohdl = (channel, msg_data) -> lcmodomsg!(vc, slam, msg_data)

  print("Preparing LCM...")
  lc = LCM()
  subscribe(lc, "ROME_NODES", gg)
  subscribe(lc, "ROME_FACTORS", odohdl)
  while fl[1]
    handle(lc)
    if slam.lastposesym == until
      break
    end
  end
  println("done")
  nothing
end

function setupSLAMinDB(cloudGraph, addrdict)
  SLAMWrapper(initfg(sessionname=addrdict["session"], cloudgraph=cloudGraph), nothing, 0)
end

function lcmsendpose(lc, vert::Graphs.ExVertex, usrid::Int)
  msg = rome.pose_node_t()
  # p = getBelief(vert)
  p = getKDE(vert)
  mv = getKDEfit(p)
  # pme = getKDEMean(vert)
  msg[:utime] = 0
  msg[:id] = usrid
  msg[:mean_dim] = 6
  msg[:mean] = mv.μ
  msg[:covar_dim] = 6
  msg[:covar] = diag(mv.Σ)

  publish(lc, "MMISAM_POSE_EST_MEAN", msg)
  nothing
end
function lcmsendallposes(lc, slaml::SLAMWrapper)
  for lbluid in slaml.lbl2usrid
    @show lbluid
    lcmsendpose(lc, getVert(slaml.fg, lbluid[1]), lbluid[2])
  end
  nothing
end

vc = startdefaultvisualization()
sleep(3.0)
rovt = loadmodel(:rov)
rovt(vc)

Nparticles = 100
include(joinpath(dirname(@__FILE__),"..","database","blandauthremote.jl"))
addrdict["session"] = "SESSHAUV"
cloudGraph, addrdict = standardcloudgraphsetup(addrdict=addrdict)
# cloudGraph, addrdict = standardcloudgraphsetup()

slam = setupSLAMinDB(cloudGraph, addrdict)

flags = Bool[1]
flags[1] = true

println("LCM listener running...")
runlistener(flags, slam, vc, until=:x5)

# @show 1
#
#
#
# println("AD-HOC DEBUGGING")
#
# lcmposenodes!(vc, slam, MSGDATA[:pose_node_t][1])
# splice!(MSGDATA[:pose_node_t], 2)
#
# @show -1
#
# #
# # # run this to stop the lcm listener loop
# # flags[1] = false
# #
# #
# solve(slam.fg) #, densitymeshes=[:x1;:x33;:x60])
# #
# visualizeallposes!(vc, slam.fg, drawtype=:fit)
#
#
#
#
# lc = LCM()
# lcmsendallposes(lc, slam)
#
#
#
# visualizeDensityMesh!(vc, slam.fg, :x1,meshid=2)

# savefg = deepcopy(slam.fg)
# savefg.registeredModuleFunctions = nothing
# @save joinpath(dirname(@__FILE__),"hauvnh67.jld") savefg

#
# using Gadfly
#
#
# draw(PDF("/home/dehann/Desktop/test.pdf",30cm,20cm),
#         plotKDE( marginal(p60,collect(4:6)),
# 				dimLbls=["x";"y";"z";"phi";"the";"psi"]) )

#


include("lobpcg_sep.jl")

mutable struct Ham
    Ns::Int64
    Ls::Float64
    kmul::Array{Float64,2} # wait until everything is clearer
    dx::Float64
    gridpos::Array{Float64,2}
    posstart::Int64
    posidx::Int64
    H                  # no idea what is this
    rhoa::Array{Float64,2}               # pseudo-charge for atoms (negative)
    rho::Array{Float64,2}                # electron density
    Vhar::Array{Float64,2}               # Hartree potential for both electron and nuclei
    Vtot::Array{Float64,2}               # total potential
    drhoa::Array{Float64,2}              # derivative of the pseudo-charge (on each atom)
    ev::Array{Float64,1}
    psi::Array{Float64,2}
    fermi::Float64
    occ
    nspin::Int64
    Neigs::Int64    # QUESTION: Number of eigenvalues?
    atoms::Atoms
    Eband::Float64              # Total band energy
    Fband::Float64               # Helmholtz band energy
    Ftot::Float64                # Total Helmholtz energy
    YukawaK::Float64             # shift for the potential
    epsil0::Float64
    Tbeta::Float64               # temperature 1beta = 1/T

    function Ham(Lat, Nunit, n_extra, dx, atoms,YukawaK, epsil0,Tbeta)
        Ls = Nunit * Lat;
        Ns = round(Integer, Ls / dx);

        #
        dx = Ls / Ns;
        # defining the grid
        gridpos = zeros(Ns,1) # allocating as a 2D Array
        gridpos[:,1] = transpose(collect(0:Ns-1)).*dx;
        posstart = 0;
        posidx   = 0;

        # Initialize the atom positions
        atoms   = atoms;
        Neigs = sum(atoms.nocc) + n_extra;

        Ls_glb = Ls;
        Ns_glb = Ns;

        # we define the Fourier multipliers as an 2D array
        kx = zeros(Ns,1);
        kx[:,1] = vcat( collect(0:Ns/2-1), collect( -Ns/2:-1) )* 2 * pi / Ls;
        kmul = kx.^2/2;

        rhoa, drhoa = pseudocharge(gridpos, Ls_glb, atoms,YukawaK,epsil0);

        # TODO: we need to figure out the type of each of the fields to properlu
        # initialize them
        H  = []
        rho = zeros(1,1);
        Vhar = zeros(1,1);
        Vtot = zeros(1,1);
        # drhoa = []
        ev = []
        psi = zeros(1,1);
        fermi = 0.0;
        occ = []
        Eband = 0.0
        Fband = 0.0
        Ftot = 0.0
        nspin = 1;

        new(Ns, Ls, kmul, dx, gridpos, posstart, posidx, H, rhoa,
            rho, Vhar, Vtot, drhoa, ev, psi, fermi, occ,nspin, Neigs, atoms,
            Eband, Fband, Ftot, YukawaK, epsil0, Tbeta)
    end
end

# importing the necessary functions to comply with Julia duck typing
using LinearAlgebra
import Base.*
import LinearAlgebra.mul!
import Base.eltype
import Base.size
import LinearAlgebra.issymmetric

function *(H::Ham, x::Array{Float64,1})
    # matvec overloading
    # Function  to  overload the application of the Hamiltonian times avector
    y_lap  = lap(H,x);
    y_vtot = Vtot(H,x);

    return y_lap + y_vtot;
end

# TODO: this should be optimized in order to take advantage of BLAS 3 operations
function *(H::Ham, X::Array{Float64,2})
    # Function  to  overload the application of the Hamiltonian times a matrix
    Y = zeros(size(X));
    mul!(Y, H, X)
    return Y
    # # new version that acts on the full matrices
    # Y_lap  = lap(H,X);
    # Y_vtot = Vtot(H,X);
    #
    # return Y_lap + Y_vtot;
end

function LinearAlgebra.mul!(Y::Array{Float64,2}, H::Ham, V::Array{Float64,2})
    # in place matrix matrix multiplication
    @assert(size(Y) == size(V))
    Y[:,:] = lap(H,V)
    Y += Vtot(H,V)

    return Y
end

# optimized version for eigs (it uses sub arrays to bypass the inference step)
function LinearAlgebra.mul!(Y::SubArray{Float64,1,Array{Float64,1}},
                  H::Ham,
                  V::SubArray{Float64,1,Array{Float64,1}})
    # in place matrix matrix multiplication
    @assert(size(Y) == size(V))
    for ii = 1:size(V,2)
        Y[:,ii] = H*V[:,ii]
    end

end

function size(H::Ham)
    return (H.Ns, H.Ns)
end

function eltype(H::Ham)
    # we work always in real space, the Fourier operations are on ly meant as
    # a pseudo spectral discretization
    return typeof(1.0)
end

function LinearAlgebra.issymmetric(H::Ham)
    return true
end

function update_psi!(H::Ham, eigOpts::eigOptions)
    # we need to add some options to the update
    # functio to solve the eigenvalue problem for a given rho and Vtot

    if eigOpts.eigmethod == "eigs"
        # TODO: make sure that eigs works with overloaded operators
        # TODO: take a look a the interface of eigs in Julia
        (ev,psi,nconv,niter,nmult,resid) = eigs(H,   # Hamiltonian
                                                nev=H.Neigs+4, # number of eigs
                                                which=:SR, # small real part
                                                ritzvec=true, # provide Ritz v
                                                tol=eigOpts.eigstol, # tolerance
                                                maxiter=eigOpts.eigsiter) #maxiter
        #assert(flag == 0);
        # TODO: this has a bug somewhere !!!! fix me!!!!
    elseif  eigOpts.eigmethod == "lobpcg_sep"
        # not working... to be fixed
        X0 = qr(rand(H.Ns, H.Neigs)).Q[:,1:H.Neigs]
        precond(x) = inv_lap(H,x)

        (ev,psi, iter) = lobpcg_sep(H, X0, precond, H.Neigs,
                                    tol=eigOpts.eigstol,
                                    maxiter=eigOpts.eigsiter)

    elseif  eigOpts.eigmethod == "eig"
        # we use a dense diagonalization
        A = create_Hamiltonian(H)
        # checkign that A is symetric
        @assert issymmetric(A)
        (ev, psi) = (eigen(A)...,)

    end

    # sorting the eigenvalues, eigs already providesd them within a vector
    ind = sortperm(ev)[1:H.Neigs];
    # updating the eigenvalues
    H.ev = ev[ind]
    # updating the eigenvectors
    H.psi = psi[:, ind];
end

function update_rho!(H::Ham, nocc::Int64)

    ev = H.ev;
    (occ, fermi) = get_occ(ev, nocc, H.Tbeta);
    occ = occ * H.nspin;
    rho = sum(H.psi.^2*Matrix(Diagonal(occ)),dims=2)/H.dx;

    # Total energy
    E = sum(ev.*occ);

    # Helmholtz free energy
    intg = H.Tbeta*(fermi .- ev);
    ff = zeros(H.Neigs,1);
    for i = 1 : H.Neigs
      if( intg[i] > 30 )  # Avoid numerical problem.
        ff[i] = ev[i]-fermi;
      else
        ff[i] = -1/H.Tbeta * log(1+exp(intg[i]));
      end
    end
    F = sum(ff.*occ) + fermi * nocc * H.nspin;

    H.occ = occ;
    H.fermi = fermi;
    H.Eband = E;
    H.Fband = F;
    H.rho = rho;
end

function update_rhoa!(H::Ham)
    H.rhoa, H.drhoa = pseudocharge(H.gridpos, H.Ls, H.atoms,H.YukawaK,H.epsil0);
end

function create_Hamiltonian(H::Ham)
    # create the matrix version of the Hmailtonian
      A = real(ifft(diagm(0 => H.kmul[:])*fft(Matrix{Float64}(I, length(H.kmul), length(H.kmul)),1),1));
      A += diagm(0 => H.Vtot[:])
      # we symmetrize A
    return 0.5*(A + A')
end

function lap(H::Ham,x::Array{Float64,1})
    # we ask for a vector, given that we will consider the vector to be
    # a nx1 matrix
    # TODO: this can be optimized using rfft
    # TODO: we can surther accelerate this using a in-place multiplication

    ytemp = H.kmul.*fft(x);
    return real(ifft(ytemp))
end

function lap(H::Ham, x::Array{Float64,2})
    # application of the laplacian part to a matrix.
    # TODO: this can be optimized using rfft
    # TODO: we can surther accelerate this using a in-place multiplication

    ytemp = broadcast(*,H.kmul,fft(x,1));
    return real(ifft(ytemp,1))
end

function inv_lap(H::Ham, x::Array{Float64,1})
    # inverse laplacian, to be used as a preconditioner for the
    # lobpcg algorithm

    inv_kmul = zeros(size(H.kmul))
    inv_kmul[1] = 0;
    inv_kmul[2:end] = 1 ./H.kmul[2:end];

    ytemp = inv_kmul.*fft(x);
    return real(ifft(ytemp))
end

function prec(H::Ham, x::Array{Float64,1})
    # preconditioner for lobpcg
## matlab code to translate
#     for ik = 1:nkpts
#     X  = gkincell{ik};
#     Y  = 27.0 + X.*(18.0 + X.*(12.0 + 8.0*X));
#     p{ik}  = Y./(Y + 16.0*X.^4);
# end;

    # inv_kmul = zeros(size(H.kmul))
    # inv_kmul[1] = 0;
    # inv_kmul[2:end] = 1./H.kmul[2:end];

    inv_kmul = zeros(size(H.kmul))
    inv_kmul[1] = 0;
    inv_kmul[2:end] = H.kmul[2:end];

    Y = 27.0 .+ inv_kmul.*(18.0 .+ inv_kmul.*(12.0 .+ 8.0 .*inv_kmul));
    yfft = Y./(Y + 16.0 .*inv_kmul.^4);
    ytemp = yfft.*fft(x);
    return real(ifft(ytemp))
end

function Vtot(H::Ham,x::Array{Float64,1})
    # application of the potential part of the Hamiltonian to a vector
    return (H.Vtot).*x
end

function Vtot(H::Ham, X::Array{Float64,2})
    # application of the potential part of the Hamiltonian to a matric
    return broadcast(*,H.Vtot, X)
end

function hartree_pot_bc(rho, H::Ham)
# computes the Hartree potential and energy in periodic boundary potential
# by solving the 1-d Yakuwa equation.
    # we call the (hopefully compiler optmized version)
    return hartree_pot_bc(rho, H.Ls, H.YukawaK, H.epsil0)
end

function update_vtot!(H::Ham, mixOpts)
    # TODO here we need to implement the andersson mix
    # I added the signature

    (Vtotnew,Verr) = update_pot!(H)

    # TODO: add a swtich to use different kinds of mixing here
    betamix = mixOpts.betamix;
    mixdim = mixOpts.mixdim;
    ymat = mixOpts.ymat;
    smat = mixOpts.smat;
    iter = mixOpts.iter;

    (Vtotmix,ymat,smat) = anderson_mix(H.Vtot,Vtotnew,
        betamix, ymat, smat, iter, mixdim);

    # they are already modified inside the function
    # mixOpts.ymat = ymat;
    # mixOpts.smat = smat;
    mixOpts.iter += 1;

    # updating total potential
    H.Vtot = Vtotmix;
    return Verr
end

function update_vtot!(H::Ham, mixOpts::kerkerMixOptions)
    # TODO here we need to implement the andersson mix
    # I added the signature

    (Vtotnew,Verr) = update_pot!(H)

    # println("using kerker mixing")
    # computign the residual
    res     = H.Vtot - Vtotnew;

    # println("appliying the preconditioner")
    resprec = kerker_mix(res, mixOpts.KerkerB, mixOpts.kx,
                              mixOpts.YukawaK, mixOpts.epsil0);

    # println("performing the linear mixing ")
    Vtotmix =  H.Vtot - mixOpts.betamix * resprec;

    # updating total potential
    # println("saving the total potentail ")
    H.Vtot = Vtotmix;

    # println("returning the error")
    return Verr
end

function update_vtot!(H::Ham, mixOpts::andersonPrecMixOptions)
    # TODO here we need to implement the andersson mix
    # I added the signature

    (Vtotnew,Verr) = update_pot!(H)

    betamix = mixOpts.betamix;
    mixdim = mixOpts.mixdim;
    ymat = mixOpts.ymat;
    smat = mixOpts.smat;
    iter = mixOpts.iter;

    # TODO change this so this is properly modified inside the function
    (Vtotmix,ymat,smat) = prec_anderson_mix(H.Vtot,Vtotnew,
        betamix, ymat, smat, iter, mixdim, mixOpts.prec, mixOpts.precargs)

    # they are already modified inside the function
    # just to be sure we can leave them here
    mixOpts.ymat = ymat;
    mixOpts.smat = smat;
    mixOpts.iter += 1;

    # updating total potential
    H.Vtot = Vtotmix;
    return Verr
end

import Statistics.mean
using FFTW

function init_pot!(H::Ham, nocc::Int64)
    # nocc number of occupied states
    #function to initialize the potential in the Hamiltonian class
    rho  = -H.rhoa;
    rho  = rho / ( sum(rho)*H.dx) * (nocc*H.nspin);
    H.rho = rho;
    H.Vhar = hartree_pot_bc(H.rho+H.rhoa, H);
    H.Vtot = H.Vhar;   # No exchange-correlation
    H.Vtot = H.Vtot .- mean(H.Vtot);# IMPORTANT (zero mean?)
end

function update_pot!(H::Ham)
     # TODO: I dont' know in how many different ways this si updateded
     # computing the hartree potenatial
    H.Vhar = hartree_pot_bc(H.rho+H.rhoa,H);
    # here Vtotnew only considers the
    Vtotnew  = H.Vhar;  # no exchange-correlation so far
    Verr = norm(Vtotnew-H.Vtot)./norm(H.Vtot); # computing the relative error

    # NOTE: H.Fband is only the band energy here.  The real total energy
    # is calculated using the formula below:
    H.Ftot = H.Fband + 1/2 * sum((H.rhoa-H.rho).*H.Vhar)*H.dx;
    return (Vtotnew,Verr) # returns the differnece betwen two consecutive iterations
end

using Arpack

# TODO: add a default setting for the scfOpts
function scf!(H::Ham, scfOpts::scfOptions)

    # vector containing the hostorical of the results
    VtoterrHist = zeros(scfOpts.scfiter)
    eigOpts = scfOpts.eigOpts;
    mixOpts = scfOpts.mixOpts;

    # number of occupied states
    Nocc = round(Integer, sum(H.atoms.nocc) / H.nspin);

    # we test first updating the psi

    for ii = 1:scfOpts.scfiter
        # solving the linear eigenvalues problem
        update_psi!(H, eigOpts);

        # update the electron density
        update_rho!(H,Nocc);

        # update the total potential, and compute the
        # differnce between the potentials
        Verr = update_vtot!(H, mixOpts);

        # save the error
        VtoterrHist[ii] = Verr ;
        # test if the problem had already satiesfied the tolerance
        if scfOpts.SCFtol > Verr
            break
        end
    end

    return VtoterrHist[VtoterrHist.>0]
end

function lap_opt(H::Ham,x::Array{Float64,1})
    # we ask for a 2 vector, given that we will consider the vector to be
    xFourier = rfft(x)
    laplacian_fourier_mult!(xFourier, H.Ls)
    return irfft(xFourier, H.Ns )
end

function lap_opt!(H::Ham,x::Array{Float64,1})
    # we ask for a 2 vector, given that we will consider the vector to be
    xFourier = rfft(x)
    laplacian_fourier_mult!(xFourier, H.Ls)
    x[:] = irfft(xFourier, H.Ns )
end

function laplacian_fourier_mult!(R::Vector{ComplexF64}, Ls::Float64 )
    c = (2 * pi / Ls)^2/2
    @simd for ii = 1:length(R)
        @inbounds R[ii] = (ii-1)^2*c*R[ii]
    end
end

# Function to compute the force and update ham.atoms.force
function get_force!(H::Ham)
    atoms  = H.atoms
    Natoms = atoms.Natoms
    rhotot = H.rho + H.rhoa
    for i=1:Natoms
        # IMPORTANT: derivative is taken w.r.t atom positions, which introduces the minus sign
        dV = -hartree_pot_bc_opt_vec(H.drhoa[:,i], H.Ls, H.YukawaK, H.epsil0)
        # Force is negative gradient
        atoms.force[i] = - sum(dV.*rhotot)*H.dx
    end
end


# Model LMU-40X-NUV  : https://www.thorlabs.com/newgrouppage9.cfm?objectgroup_id=3271

lmu_40x_nuv  = LaserLab.Objective("LMU-40X-NUV", 5.3mm, 5.1mm, 40.0)
lmm40xf_uvv  = LaserLab.Objective("LMM40XF-UVVV", 5.0mm, 5.1mm, 40.0)
df  = LaserLab.load_df_from_csv("/Users/pabloherrero/sabat/LaserLab/data/objectives", "LMU-40X-NUV.csv", LaserLab.enG)
wl  = 200.0:20:800.0
fdf = LaserLab.dftof(wl, df, "Transmission(%)", 0.0, 0.01)

function lmu_40x_nuv_transmission(λ::Unitful.Length)
    lnm = uconvert(nm, λ)
    fdf(lnm/nm)
end

# response of the objective is very flat from 200 nm to 1000 nm, around 85 %
# the approximation of taking the function to be a constant is sufficient for now.
function lmm40xf_uvv_transmission()
    return 0.85
end


####### Single source per instance ##################
# Modeling
@batchdef function time_modeling_azure(_model::BatchFuture, srcG, q, recG, D, _dm, op, mode, opt)
    nsrc = isnothing(srcG) ? length(recG.xloc) : length(srcG.xloc)
    check_njulia(nsrc)
    model = fetch(_model)
    dm = isnothing(_dm) ? _dm : fetch(_dm)
    if nsrc > 1
        argout = time_modeling(model, srcG, q, recG, D, dm, 1:nsrc, op, mode, opt)
    else
        argout = time_modeling(model, srcG, q, recG, D, dm, op, mode, opt)
    end
    rmprocs(workers())
    return argout
end

#FWI
@batchdef function fwi_objective_azure(_model::BatchFuture, q, D, opt)
    check_njulia(q.nsrc)
    model = fetch(_model)
    f, g = fwi_objective(model, q, D; options=opt)
    rmprocs(workers())
    return f, g
end

#LSRTM
@batchdef function lsrtm_objective_azure(_model::BatchFuture, q, D, _dm, opt; nlind=false)
    check_njulia(q.nsrc)
    model = fetch(_model)
    dm = isnothing(_dm) ? _dm : fetch(_dm)
    f, g = lsrtm_objective(model,q, D, dm; options=opt, nlind=nlind)
    rmprocs(workers())
    return f, g
end

### julia distributed setup ###
@batchdef function check_njulia(nsrc::Integer)
    if _nproc_loc == 1 || nsrc == 1
        @info "Only $(nsrc) source or $(_nproc_loc) workers, running serial julia"
        return
    end
    nw = nsrc >= _nproc_loc ? _nproc_loc : nsrc
    @info "Starting julia distributed with $(nw) workers for $(nsrc) sources"
    addprocs(nw)
    eval(macroexpand(Main, quote @everywhere using JUDI end))
end