AC_PolyFence_loader.cpp

#include "AC_PolyFence_loader.h"

#if AP_FENCE_ENABLED

#include <AP_Vehicle/AP_Vehicle_Type.h>
#include <AP_BoardConfig/AP_BoardConfig.h>

#ifndef AC_FENCE_DUMMY_METHODS_ENABLED
#define AC_FENCE_DUMMY_METHODS_ENABLED  (!(APM_BUILD_TYPE(APM_BUILD_Rover) | APM_BUILD_COPTER_OR_HELI | APM_BUILD_TYPE(APM_BUILD_ArduPlane) | APM_BUILD_TYPE(APM_BUILD_ArduSub) | (AP_FENCE_ENABLED == 1)))
#endif

#if !AC_FENCE_DUMMY_METHODS_ENABLED

#include <AP_AHRS/AP_AHRS.h>
#include <GCS_MAVLink/GCS.h>
#include <AP_Logger/AP_Logger.h>
#include <AC_Fence/AC_Fence.h>

#include <stdio.h>

#define POLYFENCE_LOADER_DEBUGGING 0

#if POLYFENCE_LOADER_DEBUGGING
#define Debug(fmt, args ...)  do { GCS_SEND_TEXT(MAV_SEVERITY_INFO, fmt, ## args); } while (0)
#else
#define Debug(fmt, args ...)
#endif

extern const AP_HAL::HAL& hal;

static StorageAccess fence_storage(StorageManager::StorageFence);

#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
#define AC_FENCE_SDCARD_FILENAME "APM/fence.stg"
#else
#define AC_FENCE_SDCARD_FILENAME "fence.stg"
#endif

void AC_PolyFence_loader::init()
{
#if AP_SDCARD_STORAGE_ENABLED
    // check for extra storage on microsd
    const auto *bc = AP::boardConfig();
    if (bc != nullptr) {
        const auto size_kb = bc->get_sdcard_fence_kb();
        if (size_kb > 0) {
            _failed_sdcard_storage = !fence_storage.attach_file(AC_FENCE_SDCARD_FILENAME, size_kb);
            if (_failed_sdcard_storage) {
                // wipe fence if storage not available, but don't
                // save. This allows sdcard error to be fixed and
                // reboot
                _total.set(0);
            }
        }
    }
#endif
    if (!check_indexed()) {
        // tell the user, perhaps?
    }
    _old_total = _total;
}

bool AC_PolyFence_loader::find_index_for_seq(const uint16_t seq, const FenceIndex *&entry, uint16_t &i) const
{
    if (_index == nullptr) {
        return false;
    }

    if (seq > _eeprom_item_count) {
        return false;
    }

    i = 0;
    for (uint16_t j=0; j<_num_fences; j++) {
        entry = &_index[j];
        if (seq < i + entry->count) {
            return true;
        }
        i += entry->count;
    }
    return false;
}

bool AC_PolyFence_loader::find_storage_offset_for_seq(const uint16_t seq, uint16_t &offset, AC_PolyFenceType &type, uint16_t &vertex_count_offset) const
{
    if (_index == nullptr) {
        return false;
    }

    uint16_t i = 0;
    const FenceIndex *entry = nullptr;
    if (!find_index_for_seq(seq, entry, i)) {
        return false;
    }

    if (entry == nullptr) {
        INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
        return false;
    }

    const uint16_t delta = seq - i;

    offset = entry->storage_offset;
    type = entry->type;
    offset++; // skip over type
    switch (type) {
#if AC_POLYFENCE_CIRCLE_INT_SUPPORT_ENABLED
    case AC_PolyFenceType::CIRCLE_INCLUSION_INT:
    case AC_PolyFenceType::CIRCLE_EXCLUSION_INT:
#endif  // AC_POLYFENCE_CIRCLE_INT_SUPPORT_ENABLED
    case AC_PolyFenceType::CIRCLE_INCLUSION:
    case AC_PolyFenceType::CIRCLE_EXCLUSION:
        if (delta != 0) {
            INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
            return false;
        }
        break;
    case AC_PolyFenceType::POLYGON_INCLUSION:
    case AC_PolyFenceType::POLYGON_EXCLUSION:
        vertex_count_offset = offset;
        offset += 1; // the count of points in the fence
        offset += (delta * 8);
        break;
    case AC_PolyFenceType::RETURN_POINT:
        if (delta != 0) {
            INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
            return false;
        }
        break;
    case AC_PolyFenceType::END_OF_STORAGE:
        INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
        return false;
    }
    return true;
}

bool AC_PolyFence_loader::get_item(const uint16_t seq, AC_PolyFenceItem &item)
{
    if (!check_indexed()) {
        return false;
    }

    uint16_t vertex_count_offset = 0; // initialised to make compiler happy
    uint16_t offset;
    AC_PolyFenceType type;
    if (!find_storage_offset_for_seq(seq, offset, type, vertex_count_offset)) {
        return false;
    }

    item.type = type;

    switch (type) {
    case AC_PolyFenceType::CIRCLE_INCLUSION:
    case AC_PolyFenceType::CIRCLE_EXCLUSION:
        if (!read_latlon_from_storage(offset, item.loc)) {
            return false;
        }
        item.radius = fence_storage.read_float(offset);
        break;
#if AC_POLYFENCE_CIRCLE_INT_SUPPORT_ENABLED
    case AC_PolyFenceType::CIRCLE_INCLUSION_INT:
    case AC_PolyFenceType::CIRCLE_EXCLUSION_INT:
        if (!read_latlon_from_storage(offset, item.loc)) {
            return false;
        }
        item.radius = fence_storage.read_uint32(offset);
        // magically change int item into a float item:
        if (type == AC_PolyFenceType::CIRCLE_INCLUSION_INT) {
            item.type = AC_PolyFenceType::CIRCLE_INCLUSION;
        } else {
            item.type = AC_PolyFenceType::CIRCLE_EXCLUSION;
        }
        break;
#endif  // AC_POLYFENCE_CIRCLE_INT_SUPPORT_ENABLED
    case AC_PolyFenceType::POLYGON_INCLUSION:
    case AC_PolyFenceType::POLYGON_EXCLUSION:
        if (!read_latlon_from_storage(offset, item.loc)) {
            return false;
        }
        item.vertex_count = fence_storage.read_uint8(vertex_count_offset);
        break;
    case AC_PolyFenceType::RETURN_POINT:
        if (!read_latlon_from_storage(offset, item.loc)) {
            return false;
        }
        break;
    case AC_PolyFenceType::END_OF_STORAGE:
        // read end-of-storage when I should never do so
        INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
        return false;
    }
    return true;
}

bool AC_PolyFence_loader::write_type_to_storage(uint16_t &offset, const AC_PolyFenceType type)
{
    fence_storage.write_uint8(offset, (uint8_t)type);
    offset++;
    return true;
}

bool AC_PolyFence_loader::write_latlon_to_storage(uint16_t &offset, const Vector2l &latlon)
{
    fence_storage.write_uint32(offset, latlon.x);
    offset += 4;
    fence_storage.write_uint32(offset, latlon.y);
    offset += 4;
    return true;
}

bool AC_PolyFence_loader::read_latlon_from_storage(uint16_t &read_offset, Vector2l &ret) const
{
    ret.x = fence_storage.read_uint32(read_offset);
    read_offset += 4;
    ret.y = fence_storage.read_uint32(read_offset);
    read_offset += 4;
    return true;
}

bool AC_PolyFence_loader::breached() const
{
    Location loc;
    if (!AP::ahrs().get_location(loc)) {
        return false;
    }

    return breached(loc);
}

// check if a position (expressed as lat/lng) is within the boundary
//   returns true if location is outside the boundary
bool AC_PolyFence_loader::breached(const Location& loc, float& distance_outside_fence, Vector2f& fence_direction) const
{
    if (!loaded() || total_fence_count() == 0) {
        return false;
    }

    Vector2f scaled_pos;
    Vector2l pos { loc.lat, loc.lng };
    if (!scale_latlon_from_origin(loaded_origin, pos, scaled_pos)) {
        return false;
    }

    const uint16_t num_inclusion = _num_loaded_circle_inclusion_boundaries + _num_loaded_inclusion_boundaries;
    uint16_t num_inclusion_outside = 0;
    distance_outside_fence = -FLT_MAX;

    // check we are inside each inclusion zone:
    for (uint8_t i=0; i<_num_loaded_inclusion_boundaries; i++) {
        const InclusionBoundary &boundary = _loaded_inclusion_boundary[i];
        bool valid_distance = Polygon_closest_distance_point(boundary.points, boundary.count, scaled_pos, fence_direction);
        float distance = fence_direction.length() * 0.01f; // convert back to meters
        if (Polygon_outside(pos, boundary.points_lla, boundary.count)) {
            num_inclusion_outside++;
            if (valid_distance) {
                if (is_positive(distance_outside_fence)) {
                    distance_outside_fence = MIN(distance_outside_fence, distance);
                } else {
                    distance_outside_fence = distance;
                }
            }
        } else if (valid_distance) {
            distance_outside_fence = MAX(distance_outside_fence, -distance);
        }
    }

    // check we are outside each exclusion zone:
    for (uint8_t i=0; i<_num_loaded_exclusion_boundaries; i++) {
        const ExclusionBoundary &boundary = _loaded_exclusion_boundary[i];
        bool valid_distance = Polygon_closest_distance_point(boundary.points, boundary.count, scaled_pos, fence_direction);
        float distance = fence_direction.length() * 0.01f; // convert back to meters
        if (!Polygon_outside(pos, boundary.points_lla, boundary.count)) {
            if (valid_distance) {
                distance_outside_fence = distance;
            } else {
                distance_outside_fence = 0.0f;
            }
            return true;
        } else if (valid_distance) {
            distance_outside_fence = MAX(distance_outside_fence, -distance);
        }
    }

    for (uint8_t i=0; i<_num_loaded_circle_exclusion_boundaries; i++) {
        const ExclusionCircle &circle = _loaded_circle_exclusion_boundary[i];
        Location circle_center;
        circle_center.lat = circle.point.x;
        circle_center.lng = circle.point.y;
        const float diff_cm = loc.get_distance(circle_center)*100.0f;
        distance_outside_fence = MAX(distance_outside_fence, circle.radius - diff_cm*0.01f);
        if (diff_cm < circle.radius * 100.0f) {
            return true;
        }
    }

    for (uint8_t i=0; i<_num_loaded_circle_inclusion_boundaries; i++) {
        const InclusionCircle &circle = _loaded_circle_inclusion_boundary[i];
        Location circle_center;
        circle_center.lat = circle.point.x;
        circle_center.lng = circle.point.y;
        const float diff_cm = loc.get_distance(circle_center)*100.0f;
        distance_outside_fence = MAX(distance_outside_fence, diff_cm*0.01f - circle.radius);
        if (diff_cm > circle.radius * 100.0f) {
            num_inclusion_outside++;
        }
    }

    if (AC_Fence::option_enabled(AC_Fence::OPTIONS::INCLUSION_UNION, _options)) {
        // using union of inclusion areas, we are outside the fence if
        // there is at least one inclusion areas and we are outside
        // all of them
        if (num_inclusion > 0 && num_inclusion == num_inclusion_outside) {
            return true;
        }
    } else {
        // using intersection of inclusion areas. We are outside if we
        // are outside any of them
        if (num_inclusion_outside > 0) {
            return true;
        }
    }

    // if no value was found, reset to 0
    if (is_equal(distance_outside_fence, -FLT_MAX)) {
        distance_outside_fence = 0.0f;
    }

    // no fence breached
    return false;
}

bool AC_PolyFence_loader::formatted() const
{
    return (fence_storage.read_uint8(0) == new_fence_storage_magic &&
            fence_storage.read_uint8(1) == 0 &&
            fence_storage.read_uint8(2) == 0 &&
            fence_storage.read_uint8(3) == 0);
}

uint16_t AC_PolyFence_loader::max_items() const
{
    // this is 84 items on PixHawk
    return fence_storage.size() / sizeof(Vector2l);
}

bool AC_PolyFence_loader::format()
{
    uint16_t offset = 0;
    fence_storage.write_uint32(offset, 0);
    fence_storage.write_uint8(offset, new_fence_storage_magic);
    offset += 4;
    void_index();
    _eeprom_fence_count = 0;
    _eeprom_item_count = 0;
    return write_eos_to_storage(offset);
}

bool AC_PolyFence_loader::scale_latlon_from_origin(const Location &origin, const Vector2l &point, Vector2f &pos_cm) const
{
    Location tmp_loc;
    tmp_loc.lat = point.x;
    tmp_loc.lng = point.y;
    pos_cm = origin.get_distance_NE(tmp_loc) * 100.0f;
    return true;
}

bool AC_PolyFence_loader::read_polygon_from_storage(const Location &origin, uint16_t &read_offset, const uint8_t vertex_count, Vector2f *&next_storage_point, Vector2l *&next_storage_point_lla)
{
    for (uint8_t i=0; i<vertex_count; i++) {
        // read from storage to lat/lon
        if (!read_latlon_from_storage(read_offset, *next_storage_point_lla)) {
            return false;
        }
        // convert lat/lon to position in cm from origin
        if (!scale_latlon_from_origin(origin, *next_storage_point_lla, *next_storage_point)) {
            return false;
        }
        
        next_storage_point_lla++;
        next_storage_point++;
    }
    return true;
}

bool AC_PolyFence_loader::scan_eeprom(scan_fn_t scan_fn)
{
    uint16_t read_offset = 0; // skipping reserved first 4 bytes
    if (!formatted()) {
        return false;
    }
    read_offset += 4;
    bool all_done = false;
    while (!all_done) {
        if (read_offset > fence_storage.size()) {
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
            AP_HAL::panic("did not find end-of-storage-marker before running out of space");
#endif
            return false;
        }
        const AC_PolyFenceType type = (AC_PolyFenceType)fence_storage.read_uint8(read_offset);
        // validate what we've just pulled back from storage:
        switch (type) {
        case AC_PolyFenceType::END_OF_STORAGE:
        case AC_PolyFenceType::POLYGON_INCLUSION:
        case AC_PolyFenceType::POLYGON_EXCLUSION:
        case AC_PolyFenceType::CIRCLE_INCLUSION:
        case AC_PolyFenceType::CIRCLE_EXCLUSION:
#if AC_POLYFENCE_CIRCLE_INT_SUPPORT_ENABLED
        case AC_PolyFenceType::CIRCLE_INCLUSION_INT:
        case AC_PolyFenceType::CIRCLE_EXCLUSION_INT:
#endif  // AC_POLYFENCE_CIRCLE_INT_SUPPORT_ENABLED
        case AC_PolyFenceType::RETURN_POINT:
            break;
        default:
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
            AP_HAL::panic("Fence corrupt (offset=%u)", read_offset);
#endif
            GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Fence corrupt");
            return false;
        }

        scan_fn(type, read_offset);
        read_offset++;
        switch (type) {
        case AC_PolyFenceType::END_OF_STORAGE:
            _eos_offset = read_offset-1;
            all_done = true;
            break;
        case AC_PolyFenceType::POLYGON_INCLUSION:
        case AC_PolyFenceType::POLYGON_EXCLUSION: {
            const uint8_t vertex_count = fence_storage.read_uint8(read_offset);
            read_offset += 1; // for the count we just read
            read_offset += vertex_count*8;
            break;
        }
#if AC_POLYFENCE_CIRCLE_INT_SUPPORT_ENABLED
        case AC_PolyFenceType::CIRCLE_INCLUSION_INT:
        case AC_PolyFenceType::CIRCLE_EXCLUSION_INT:
#endif  // AC_POLYFENCE_CIRCLE_INT_SUPPORT_ENABLED
        case AC_PolyFenceType::CIRCLE_INCLUSION:
        case AC_PolyFenceType::CIRCLE_EXCLUSION: {
            read_offset += 8; // for latlon
            read_offset += 4; // for radius
            break;
        }
        case AC_PolyFenceType::RETURN_POINT:
            read_offset += 8; // for latlon
            break;
        }
    }
    return true;
}

// note read_offset here isn't const and ALSO is not a reference
void AC_PolyFence_loader::scan_eeprom_count_fences(const AC_PolyFenceType type, uint16_t read_offset)
{
    if (type == AC_PolyFenceType::END_OF_STORAGE) {
        return;
    }
    _eeprom_fence_count++;
    switch (type) {
    case AC_PolyFenceType::END_OF_STORAGE:
        INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
        break;
    case AC_PolyFenceType::POLYGON_EXCLUSION:
    case AC_PolyFenceType::POLYGON_INCLUSION: {
        const uint8_t vertex_count = fence_storage.read_uint8(read_offset+1); // skip type
        _eeprom_item_count += vertex_count;
        break;
    }
    case AC_PolyFenceType::CIRCLE_INCLUSION:
    case AC_PolyFenceType::CIRCLE_EXCLUSION:
#if AC_POLYFENCE_CIRCLE_INT_SUPPORT_ENABLED
    case AC_PolyFenceType::CIRCLE_INCLUSION_INT:
    case AC_PolyFenceType::CIRCLE_EXCLUSION_INT:
#endif  // AC_POLYFENCE_CIRCLE_INT_SUPPORT_ENABLED
    case AC_PolyFenceType::RETURN_POINT:
        _eeprom_item_count++;
        break;
    }
}

bool AC_PolyFence_loader::count_eeprom_fences()
{
    _eeprom_fence_count = 0;
    _eeprom_item_count = 0;
    const bool ret = scan_eeprom(FUNCTOR_BIND_MEMBER(&AC_PolyFence_loader::scan_eeprom_count_fences, void, const AC_PolyFenceType, uint16_t));
    return ret;
}

void AC_PolyFence_loader::scan_eeprom_index_fences(const AC_PolyFenceType type, uint16_t read_offset)
{
    if (_index == nullptr) {
        INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
        return;
    }
    if (type == AC_PolyFenceType::END_OF_STORAGE) {
        return;
    }
    FenceIndex &index = _index[_num_fences++];
    index.type = type;
    index.storage_offset = read_offset;
    switch (type) {
    case AC_PolyFenceType::END_OF_STORAGE:
        INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
        break;
    case AC_PolyFenceType::POLYGON_EXCLUSION:
    case AC_PolyFenceType::POLYGON_INCLUSION: {
        const uint8_t vertex_count = fence_storage.read_uint8(read_offset+1);
        index.count = vertex_count;
        break;
    }
#if AC_POLYFENCE_CIRCLE_INT_SUPPORT_ENABLED
    case AC_PolyFenceType::CIRCLE_INCLUSION_INT:
    case AC_PolyFenceType::CIRCLE_EXCLUSION_INT:
#endif  // AC_POLYFENCE_CIRCLE_INT_SUPPORT_ENABLED
    case AC_PolyFenceType::CIRCLE_INCLUSION:
    case AC_PolyFenceType::CIRCLE_EXCLUSION:
        index.count = 1;
        break;
    case AC_PolyFenceType::RETURN_POINT:
        index.count = 1;
        break;
    }
}

bool AC_PolyFence_loader::index_eeprom()
{
    if (!formatted()) {
        if (!format()) {
            return false;
        }
    }

    if (!count_eeprom_fences()) {
        return false;
    }

    void_index();

    if (_eeprom_fence_count == 0) {
        _num_fences = 0;
        _load_attempted = false;
        return true;
    }

    Debug("Fence: Allocating %u bytes for index",
          (unsigned)(_eeprom_fence_count*sizeof(FenceIndex)));
    _index = NEW_NOTHROW FenceIndex[_eeprom_fence_count];
    if (_index == nullptr) {
        return false;
    }

    _num_fences = 0;
    if (!scan_eeprom(FUNCTOR_BIND_MEMBER(&AC_PolyFence_loader::scan_eeprom_index_fences, void, const AC_PolyFenceType, uint16_t))) {
        void_index();
        return false;
    }

#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
    if (_num_fences != _eeprom_fence_count) {
        AP_HAL::panic("indexed fences not equal to eeprom fences");
    }
#endif

    _load_attempted = false;

    return true;
}

bool AC_PolyFence_loader::check_indexed()
{
    if (!_index_attempted) {
        _indexed = index_eeprom();
        _index_attempted = true;
    }
    return _indexed;
}

void AC_PolyFence_loader::unload()
{
    delete[] _loaded_offsets_from_origin;
    _loaded_offsets_from_origin = nullptr;

    delete[] _loaded_points_lla;
    _loaded_points_lla = nullptr;

    delete[] _loaded_inclusion_boundary;
    _loaded_inclusion_boundary = nullptr;
    _num_loaded_inclusion_boundaries = 0;

    delete[] _loaded_exclusion_boundary;
    _loaded_exclusion_boundary = nullptr;
    _num_loaded_exclusion_boundaries = 0;

    delete[] _loaded_circle_inclusion_boundary;
    _loaded_circle_inclusion_boundary = nullptr;
    _num_loaded_circle_inclusion_boundaries = 0;

    delete[] _loaded_circle_exclusion_boundary;
    _loaded_circle_exclusion_boundary = nullptr;
    _num_loaded_circle_exclusion_boundaries = 0;

    _loaded_return_point = nullptr;
    _loaded_return_point_lla = nullptr;
    _load_time_ms = 0;
}

// return the number of fences of type type in the index:
uint16_t AC_PolyFence_loader::index_fence_count(const AC_PolyFenceType type)
{
    uint16_t ret = 0;
    for (uint8_t i=0; i<_eeprom_fence_count; i++) {
        const FenceIndex &index = _index[i];
        if (index.type == type) {
            ret++;
        }
    }
    return ret;
}

uint16_t AC_PolyFence_loader::sum_of_polygon_point_counts_and_returnpoint()
{
    uint16_t ret = 0;
    for (uint8_t i=0; i<_eeprom_fence_count; i++) {
        const FenceIndex &index = _index[i];
        switch (index.type) {
#if AC_POLYFENCE_CIRCLE_INT_SUPPORT_ENABLED
        case AC_PolyFenceType::CIRCLE_INCLUSION_INT:
        case AC_PolyFenceType::CIRCLE_EXCLUSION_INT:
#endif  // AC_POLYFENCE_CIRCLE_INT_SUPPORT_ENABLED
        case AC_PolyFenceType::CIRCLE_INCLUSION:
        case AC_PolyFenceType::CIRCLE_EXCLUSION:
            break;
        case AC_PolyFenceType::RETURN_POINT:
            ret += 1;
            break;
        case AC_PolyFenceType::POLYGON_INCLUSION:
        case AC_PolyFenceType::POLYGON_EXCLUSION:
            ret += index.count;
            break;
        case AC_PolyFenceType::END_OF_STORAGE:
            INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
            break;
        }
    }
    return ret;
}

bool AC_PolyFence_loader::load_from_storage()
{
    if (!check_indexed()) {
        return false;
    }

    if (_load_attempted) {
        return _load_time_ms != 0;
    }

    if (!AP::ahrs().get_origin(loaded_origin)) {
//        Debug("fence load requires origin");
        return false;
    }

    // find indexes of each fence:
    if (!get_loaded_fence_semaphore().take_nonblocking()) {
        return false;
    }

    _load_attempted = true;

    unload();

    if (_eeprom_item_count == 0) {
        get_loaded_fence_semaphore().give();
        _load_time_ms = AP_HAL::millis();
        return true;
    }

    { // allocate array to hold offsets-from-origin
        const uint16_t count = sum_of_polygon_point_counts_and_returnpoint();
        Debug("Fence: Allocating %u bytes for points",
              (unsigned)(count * sizeof(Vector2f)));
        _loaded_offsets_from_origin = NEW_NOTHROW Vector2f[count];
        _loaded_points_lla = NEW_NOTHROW Vector2l[count];
        if (_loaded_offsets_from_origin == nullptr || _loaded_points_lla == nullptr) {
            unload();
            get_loaded_fence_semaphore().give();
            return false;
        }
    }

    // FIXME: find some way of factoring out all of these allocation routines.

    { // allocate storage for inclusion polyfences:
        const auto count = index_fence_count(AC_PolyFenceType::POLYGON_INCLUSION);
        Debug("Fence: Allocating %u bytes for inc. fences",
              (unsigned)(count * sizeof(InclusionBoundary)));
        _loaded_inclusion_boundary = NEW_NOTHROW InclusionBoundary[count];
        if (_loaded_inclusion_boundary == nullptr) {
            unload();
            get_loaded_fence_semaphore().give();
            return false;
        }
    }

    { // allocate storage for exclusion polyfences:
        const auto count = index_fence_count(AC_PolyFenceType::POLYGON_EXCLUSION);
        Debug("Fence: Allocating %u bytes for exc. fences",
              (unsigned)(count * sizeof(ExclusionBoundary)));
        _loaded_exclusion_boundary = NEW_NOTHROW ExclusionBoundary[count];
        if (_loaded_exclusion_boundary == nullptr) {
            unload();
            get_loaded_fence_semaphore().give();
            return false;
        }
    }

    { // allocate storage for circular inclusion fences:
        uint32_t count = index_fence_count(AC_PolyFenceType::CIRCLE_INCLUSION);
#if AC_POLYFENCE_CIRCLE_INT_SUPPORT_ENABLED
        count += index_fence_count(AC_PolyFenceType::CIRCLE_INCLUSION_INT)
#endif  // AC_POLYFENCE_CIRCLE_INT_SUPPORT_ENABLED
        Debug("Fence: Allocating %u bytes for circ. inc. fences",
              (unsigned)(count * sizeof(InclusionCircle)));
        _loaded_circle_inclusion_boundary = NEW_NOTHROW InclusionCircle[count];
        if (_loaded_circle_inclusion_boundary == nullptr) {
            unload();
            get_loaded_fence_semaphore().give();
            return false;
        }
    }

    { // allocate storage for circular exclusion fences:
        uint32_t count = index_fence_count(AC_PolyFenceType::CIRCLE_EXCLUSION);
#if AC_POLYFENCE_CIRCLE_INT_SUPPORT_ENABLED
        count += index_fence_count(AC_PolyFenceType::CIRCLE_EXCLUSION_INT)
#endif  // AC_POLYFENCE_CIRCLE_INT_SUPPORT_ENABLED
        Debug("Fence: Allocating %u bytes for circ. exc. fences",
              (unsigned)(count * sizeof(ExclusionCircle)));
        _loaded_circle_exclusion_boundary = NEW_NOTHROW ExclusionCircle[count];
        if (_loaded_circle_exclusion_boundary == nullptr) {
            unload();
            get_loaded_fence_semaphore().give();
            return false;
        }
    }

    Vector2f *next_storage_point = _loaded_offsets_from_origin;
    Vector2l *next_storage_point_lla = _loaded_points_lla;

    // use index to load fences from eeprom
    bool storage_valid = true;
    for (uint8_t i=0; i<_eeprom_fence_count; i++) {
        if (!storage_valid) {
            break;
        }
        const FenceIndex &index = _index[i];
        uint16_t storage_offset = index.storage_offset;
        storage_offset += 1; // skip type
        switch (index.type) {
        case AC_PolyFenceType::END_OF_STORAGE:
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
            AP_HAL::panic("indexed end of storage found");
#endif
            storage_valid = false;
            break;
        case AC_PolyFenceType::POLYGON_INCLUSION: {
            // FIXME: consider factoring this with the EXCLUSION case
            InclusionBoundary &boundary = _loaded_inclusion_boundary[_num_loaded_inclusion_boundaries];
            boundary.points = next_storage_point;
            boundary.points_lla = next_storage_point_lla;
            boundary.count = index.count;
            if (index.count < 3) {
                GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "AC_Fence: invalid polygon vertex count %u", index.count);
                storage_valid = false;
                break;
            }
            storage_offset += 1; // skip vertex count
            if (!read_polygon_from_storage(loaded_origin, storage_offset, index.count, next_storage_point, next_storage_point_lla)) {
                GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "AC_Fence: polygon read failed");
                storage_valid = false;
                break;
            }
            _num_loaded_inclusion_boundaries++;
            break;
        }
        case AC_PolyFenceType::POLYGON_EXCLUSION: {
            ExclusionBoundary &boundary = _loaded_exclusion_boundary[_num_loaded_exclusion_boundaries];
            boundary.points = next_storage_point;
            boundary.points_lla = next_storage_point_lla;
            boundary.count = index.count;
            if (index.count < 3) {
                GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "AC_Fence: invalid polygon vertex count %u", index.count);
                storage_valid = false;
                break;
            }
            storage_offset += 1; // skip vertex count
            if (!read_polygon_from_storage(loaded_origin, storage_offset, index.count, next_storage_point, next_storage_point_lla)) {
                GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "AC_Fence: polygon read failed");
                storage_valid = false;
                break;
            }
            _num_loaded_exclusion_boundaries++;
            break;
        }
#if AC_POLYFENCE_CIRCLE_INT_SUPPORT_ENABLED
        case AC_PolyFenceType::CIRCLE_EXCLUSION_INT:
#endif  // AC_POLYFENCE_CIRCLE_INT_SUPPORT_ENABLED
        case AC_PolyFenceType::CIRCLE_EXCLUSION: {
            ExclusionCircle &circle = _loaded_circle_exclusion_boundary[_num_loaded_circle_exclusion_boundaries];
            if (!read_latlon_from_storage(storage_offset, circle.point)) {
                GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "AC_Fence: latlon read failed");
                storage_valid = false;
                break;
            }
            if (!scale_latlon_from_origin(loaded_origin, circle.point, circle.pos_cm)) {
                GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "AC_Fence: latlon read failed");
                storage_valid = false;
                break;
            }
            // now read the radius
#if AC_POLYFENCE_CIRCLE_INT_SUPPORT_ENABLED
            if (index.type == AC_PolyFenceType::CIRCLE_EXCLUSION_INT) {
                circle.radius = fence_storage.read_uint32(storage_offset);
            } else
#endif  // AC_POLYFENCE_CIRCLE_INT_SUPPORT_ENABLED
            {
                circle.radius = fence_storage.read_float(storage_offset);
            }
            if (!is_positive(circle.radius)) {
                GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "AC_Fence: non-positive circle radius");
                storage_valid = false;
                break;
            }
            _num_loaded_circle_exclusion_boundaries++;
            break;
        }
#if AC_POLYFENCE_CIRCLE_INT_SUPPORT_ENABLED
        case AC_PolyFenceType::CIRCLE_INCLUSION_INT:
#endif  // AC_POLYFENCE_CIRCLE_INT_SUPPORT_ENABLED
        case AC_PolyFenceType::CIRCLE_INCLUSION: {
            InclusionCircle &circle = _loaded_circle_inclusion_boundary[_num_loaded_circle_inclusion_boundaries];
            if (!read_latlon_from_storage(storage_offset, circle.point)) {
                GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "AC_Fence: latlon read failed");
                storage_valid = false;
                break;
            }
            if (!scale_latlon_from_origin(loaded_origin, circle.point, circle.pos_cm)){
                GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "AC_Fence: latlon read failed");
                storage_valid = false;
                break;
            }
            // now read the radius
#if AC_POLYFENCE_CIRCLE_INT_SUPPORT_ENABLED
            if (index.type == AC_PolyFenceType::CIRCLE_INCLUSION_INT) {
                circle.radius = fence_storage.read_uint32(storage_offset);
            } else
#endif  // AC_POLYFENCE_CIRCLE_INT_SUPPORT_ENABLED
            {
                circle.radius = fence_storage.read_float(storage_offset);
            }
            if (!is_positive(circle.radius)) {
                GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "AC_Fence: non-positive circle radius");
                storage_valid = false;
                break;
            }
            _num_loaded_circle_inclusion_boundaries++;
            break;
        }
        case AC_PolyFenceType::RETURN_POINT:
            if (_loaded_return_point != nullptr) {
                GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "PolyFence: Multiple return points found");
                storage_valid = false;
                break;
            }
            _loaded_return_point = next_storage_point;
            if (_loaded_return_point_lla != nullptr) {
                GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "PolyFence: Multiple return points found");
                storage_valid = false;
                break;
            }
            _loaded_return_point_lla = next_storage_point_lla;
            // Read the point from storage
            if (!read_latlon_from_storage(storage_offset, *next_storage_point_lla)) {
                storage_valid = false;
                GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "PolyFence: latlon read failed");
                break;
            }
            if (!scale_latlon_from_origin(loaded_origin, *next_storage_point_lla, *next_storage_point)) {
                storage_valid = false;
                GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "PolyFence: latlon read failed");
                break;
            }
            next_storage_point++;
            next_storage_point_lla++;
            break;
        }
    }

    if (!storage_valid) {
        unload();
        get_loaded_fence_semaphore().give();
        return false;
    }

    _load_time_ms = AP_HAL::millis();

    get_loaded_fence_semaphore().give();
    return true;
}

/// returns pointer to array of exclusion polygon points and num_points is filled in with the number of points in the polygon
/// points are offsets in cm from EKF origin in NE frame
Vector2f* AC_PolyFence_loader::get_exclusion_polygon(uint16_t index, uint16_t &num_points) const
{
    if (index >= _num_loaded_exclusion_boundaries) {
        num_points = 0;
        return nullptr;
    }
    const ExclusionBoundary &boundary = _loaded_exclusion_boundary[index];
    num_points = boundary.count;

    return boundary.points;
}

/// returns pointer to array of inclusion polygon points and num_points is filled in with the number of points in the polygon
/// points are offsets in cm from EKF origin in NE frame
Vector2f* AC_PolyFence_loader::get_inclusion_polygon(uint16_t index, uint16_t &num_points) const
{
    if (index >= _num_loaded_inclusion_boundaries) {
        num_points = 0;
        return nullptr;
    }
    const InclusionBoundary &boundary = _loaded_inclusion_boundary[index];
    num_points = boundary.count;

    return boundary.points;
}

/// returns the specified exclusion circle
/// circle center offsets in cm from EKF origin in NE frame, radius is in meters
bool AC_PolyFence_loader::get_exclusion_circle(uint8_t index, Vector2f &center_pos_cm, float &radius) const
{
    if (index >= _num_loaded_circle_exclusion_boundaries) {
        return false;
    }
    center_pos_cm = _loaded_circle_exclusion_boundary[index].pos_cm;
    radius =  _loaded_circle_exclusion_boundary[index].radius;
    return true;
}

/// returns the specified inclusion circle
/// circle centre offsets in cm from EKF origin in NE frame, radius is in meters
bool AC_PolyFence_loader::get_inclusion_circle(uint8_t index, Vector2f &center_pos_cm, float &radius) const
{
    if (index >= _num_loaded_circle_inclusion_boundaries) {
        return false;
    }
    center_pos_cm = _loaded_circle_inclusion_boundary[index].pos_cm;
    radius =  _loaded_circle_inclusion_boundary[index].radius;
    return true;
}

bool AC_PolyFence_loader::check_inclusion_circle_margin(float margin) const
{
    // check circular includes
    for (uint8_t i=0; i<_num_loaded_circle_inclusion_boundaries; i++) {
        const InclusionCircle &circle = _loaded_circle_inclusion_boundary[i];
        if (circle.radius < margin) {
            // circle radius should never be less than margin
            return false;
        } 
    }
    return true;
}

bool AC_PolyFence_loader::validate_fence(const AC_PolyFenceItem *new_items, uint16_t count) const
{
    // validate the fence items...
    AC_PolyFenceType expecting_type = AC_PolyFenceType::END_OF_STORAGE;
    uint16_t expected_type_count = 0;
    uint16_t orig_expected_type_count = 0;
    bool seen_return_point = false;

    for (uint16_t i=0; i<count; i++) {
        bool validate_latlon = false;

        switch (new_items[i].type) {
        case AC_PolyFenceType::END_OF_STORAGE:
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
            AP_HAL::panic("passed in an END_OF_STORAGE");
#endif
            return false;

        case AC_PolyFenceType::POLYGON_INCLUSION:
        case AC_PolyFenceType::POLYGON_EXCLUSION:
            if (new_items[i].vertex_count < 3) {
                GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Invalid vertex count (%u)", new_items[i].vertex_count);
                return false;
            }
            if (expected_type_count == 0) {
                expected_type_count = new_items[i].vertex_count;
                orig_expected_type_count = expected_type_count;
                expecting_type = new_items[i].type;
            } else {
                if (new_items[i].type != expecting_type) {
                    GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Received incorrect vertex type (want=%u got=%u)", (unsigned)expecting_type, (unsigned)new_items[i].type);
                    return false;
                } else if (new_items[i].vertex_count != orig_expected_type_count) {
                    GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Unexpected vertex count want=%u got=%u", orig_expected_type_count, new_items[i].vertex_count);
                    return false;
                }
            }
            expected_type_count--;
            validate_latlon = true;
            break;

#if AC_POLYFENCE_CIRCLE_INT_SUPPORT_ENABLED
        case AC_PolyFenceType::CIRCLE_INCLUSION_INT:
        case AC_PolyFenceType::CIRCLE_EXCLUSION_INT:
            // should never have AC_PolyFenceItems of these types
            INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
            return false;
#endif  // AC_POLYFENCE_CIRCLE_INT_SUPPORT_ENABLED
        case AC_PolyFenceType::CIRCLE_INCLUSION:
        case AC_PolyFenceType::CIRCLE_EXCLUSION:
            if (expected_type_count) {
               GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Received incorrect type (want=%u got=%u)", (unsigned)expecting_type, (unsigned)new_items[i].type);
               return false;
            }
            if (!is_positive(new_items[i].radius)) {
                GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Non-positive circle radius");
                return false;
            }
            validate_latlon = true;
            break;

        case AC_PolyFenceType::RETURN_POINT:
            if (expected_type_count) {
                GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Received incorrect type (want=%u got=%u)", (unsigned)expecting_type, (unsigned)new_items[i].type);
                return false;
            }

            // spec says only one return point allowed
            if (seen_return_point) {
                GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Multiple return points");
                return false;
            }
            seen_return_point = true;
            validate_latlon = true;
            // TODO: ensure return point is within all fences and
            // outside all exclusion zones
            break;
        }

        if (validate_latlon) {
            if (!check_latlng(new_items[i].loc[0], new_items[i].loc[1])) {
                GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Bad lat or lon");
                return false;
            }
        }
    }

    if (expected_type_count) {
        GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Incorrect item count");
        return false;
    }

    return true;
}

uint16_t AC_PolyFence_loader::fence_storage_space_required(const AC_PolyFenceItem *new_items, uint16_t count)
{
    uint16_t ret = 4; // for the format header
    uint16_t i = 0;
    while (i < count) {
        ret += 1; // one byte for type
        switch (new_items[i].type) {
        case AC_PolyFenceType::POLYGON_INCLUSION:
        case AC_PolyFenceType::POLYGON_EXCLUSION:
            ret += 1 + 8 * new_items[i].vertex_count; // 1 count, 4 lat, 4 lon for each point
            i += new_items[i].vertex_count - 1; // i is incremented down below
            break;
        case AC_PolyFenceType::END_OF_STORAGE:
            INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
            break;
#if AC_POLYFENCE_CIRCLE_INT_SUPPORT_ENABLED
        case AC_PolyFenceType::CIRCLE_INCLUSION_INT:
        case AC_PolyFenceType::CIRCLE_EXCLUSION_INT:
            // should never have AC_PolyFenceItems of these types
            INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
            break;
#endif  // AC_POLYFENCE_CIRCLE_INT_SUPPORT_ENABLED
        case AC_PolyFenceType::CIRCLE_INCLUSION:
        case AC_PolyFenceType::CIRCLE_EXCLUSION:
            ret += 12; // 4 radius, 4 lat, 4 lon
            break;
        case AC_PolyFenceType::RETURN_POINT:
            ret += 8; // 4 lat, 4 lon
            break;
        }
        i++;
    }
    return ret;
}

bool AC_PolyFence_loader::write_fence(const AC_PolyFenceItem *new_items, uint16_t count)
{
    if (!validate_fence(new_items, count)) {
        GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Fence validation failed");
        return false;
    }

    if (fence_storage_space_required(new_items, count) > fence_storage.size()) {
        GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Fence exceeds storage size");
        return false;
    }

    if (!format()) {
        return false;
    }

    uint16_t total_vertex_count = 0;
    uint16_t offset = 4; // skipping magic
    uint8_t vertex_count = 0;
    for (uint16_t i=0; i<count; i++) {
        const AC_PolyFenceItem new_item = new_items[i];
        switch (new_item.type) {
        case AC_PolyFenceType::POLYGON_INCLUSION:
        case AC_PolyFenceType::POLYGON_EXCLUSION:
            if (vertex_count == 0) {
                // write out new polygon count
                vertex_count = new_item.vertex_count;
                total_vertex_count += vertex_count;
                if (!write_type_to_storage(offset, new_item.type)) {
                    return false;
                }
                fence_storage.write_uint8(offset, vertex_count);
                offset++;
            }
            vertex_count--;
            if (!write_latlon_to_storage(offset, new_item.loc)) {
                return false;
            }
            break;
        case AC_PolyFenceType::END_OF_STORAGE:
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
            AP_HAL::panic("asked to store end-of-storage marker");
#endif
            return false;
#if AC_POLYFENCE_CIRCLE_INT_SUPPORT_ENABLED
        case AC_PolyFenceType::CIRCLE_INCLUSION_INT:
        case AC_PolyFenceType::CIRCLE_EXCLUSION_INT:
            // should never have AC_PolyFenceItems of these types
            INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
            return false;
#endif  // AC_POLYFENCE_CIRCLE_INT_SUPPORT_ENABLED
        case AC_PolyFenceType::CIRCLE_INCLUSION:
        case AC_PolyFenceType::CIRCLE_EXCLUSION: {
            total_vertex_count++; // useful to make number of lines in QGC file match FENCE_TOTAL
            if (!write_type_to_storage(offset, new_item.type)) {
                return false;
            }
            if (!write_latlon_to_storage(offset, new_item.loc)) {
                return false;
            }
            fence_storage.write_float(offset, new_item.radius);
            offset += 4;
            break;
        }
        case AC_PolyFenceType::RETURN_POINT:
            if (!write_type_to_storage(offset, new_item.type)) {
                return false;
            }
            if (!write_latlon_to_storage(offset, new_item.loc)) {
                return false;
            }
            break;
        }
    }
    if (!write_eos_to_storage(offset)) {
        return false;
    }

#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
    // sanity-check the EEPROM in SITL to make sure we can read what
    // we've just written.
    if (!index_eeprom()) {
        AP_HAL::panic("Failed to index eeprom");
    }
    GCS_SEND_TEXT(MAV_SEVERITY_DEBUG, "Fence Indexed OK");
#endif

#if HAL_LOGGING_ENABLED
    // start logger logging new fence
    AP::logger().Write_Fence();
#endif

    void_index();

    // this may be completely bogus total.  If we are storing an
    // advanced fence then the old protocol which relies on this value
    // will error off if the GCS tries to fetch points.  This number
    // should be correct for a "compatible" fence, however.
    uint16_t new_total = 0;
    if (total_vertex_count >= 3) {
        new_total = total_vertex_count+2;
    }
    _total.set_and_save(new_total);

    return true;
}


bool AC_PolyFence_loader::get_return_point(Vector2l &ret)
{
    if (!check_indexed()) {
        return false;
    }

    const FenceIndex *rp = find_first_fence(AC_PolyFenceType::RETURN_POINT);
    if (rp != nullptr) {
        uint16_t read_offset = rp->storage_offset + 1;
        return read_latlon_from_storage(read_offset, ret);
    }

    const FenceIndex *inc = find_first_fence(AC_PolyFenceType::POLYGON_INCLUSION);
    if (inc == nullptr) {
        return false;
    }

    // we found an inclusion fence but not a return point.  Calculate
    // and return the centroid.  Note that this may not actually be
    // inside all inclusion fences...
    uint16_t offset = inc->storage_offset;
    if ((AC_PolyFenceType)fence_storage.read_uint8(offset) != AC_PolyFenceType::POLYGON_INCLUSION) {
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
        AP_HAL::panic("wrong type at offset");
#endif
        return false;
    }
    offset++;
    const uint8_t count = fence_storage.read_uint8(offset);
    if (count < 3) {
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
        AP_HAL::panic("invalid count found");
#endif
        return false;
    }
    offset++;
    Vector2l min_loc;
    if (!read_latlon_from_storage(offset, min_loc)) {
        return false;
    }
    if (min_loc.is_zero()) {
        return false;
    }
    Vector2l max_loc = min_loc;
    for (uint8_t i=1; i<count; i++) {
        Vector2l new_loc;
        if (!read_latlon_from_storage(offset, new_loc)) {
            return false;
        }
        if (new_loc.is_zero()) {
            return false;
        }
        if (new_loc.x < min_loc.x) {
            min_loc.x = new_loc.x;
        }
        if (new_loc.y < min_loc.y) {
            min_loc.y = new_loc.y;
        }
        if (new_loc.x > max_loc.x) {
            max_loc.x = new_loc.x;
        }
        if (new_loc.y > max_loc.y) {
            max_loc.y = new_loc.y;
        }
    }

    // Potential for int32_t overflow when longitudes are beyond [-107, 107].
    // As a result, the calculated return point's longitude is calculated using overflowed figure.
    // Dividing first before adding avoids the potential overflow.
    ret.x = (min_loc.x / 2) + (max_loc.x / 2);
    ret.y = (min_loc.y / 2) + (max_loc.y / 2);

    return true;
}

AC_PolyFence_loader::FenceIndex *AC_PolyFence_loader::find_first_fence(const AC_PolyFenceType type) const
{
    if (_index == nullptr) {
        return nullptr;
    }
    for (uint8_t i=0; i<_num_fences; i++) {
        if (_index[i].type == type) {
            return &_index[i];
        }
    }
    return nullptr;
}

#if AC_POLYFENCE_FENCE_POINT_PROTOCOL_SUPPORT
void AC_PolyFence_loader::handle_msg_fetch_fence_point(GCS_MAVLINK &link, const mavlink_message_t& msg)
{
    if (!check_indexed()) {
        return;
    }
    if (!contains_compatible_fence()) {
        link.send_text(MAV_SEVERITY_WARNING, "Vehicle contains advanced fences");
        return;
    }

    if (_total != 0 && _total < 5) {
        link.send_text(MAV_SEVERITY_WARNING, "Invalid FENCE_TOTAL");
        return;
    }

    mavlink_fence_fetch_point_t packet;
    mavlink_msg_fence_fetch_point_decode(&msg, &packet);

    if (packet.idx >= _total) {
        link.send_text(MAV_SEVERITY_WARNING, "Invalid fence point, index past total(%u >= %u)", packet.idx, _total.get());
        return;
    }

    mavlink_fence_point_t ret_packet{};
    ret_packet.target_system = msg.sysid;
    ret_packet.target_component = msg.compid;
    ret_packet.idx = packet.idx;
    ret_packet.count = _total;

    if (packet.idx == 0) {
        // return point
        Vector2l ret;
        if (get_return_point(ret)) {
            ret_packet.lat = ret.x * 1.0e-7f;
            ret_packet.lng = ret.y * 1.0e-7f;
        } else {
            link.send_text(MAV_SEVERITY_WARNING, "Failed to get return point");
        }
    } else {
        // find the inclusion fence:
        const FenceIndex *inclusion_fence = find_first_fence(AC_PolyFenceType::POLYGON_INCLUSION);
        if (inclusion_fence == nullptr) {
            // nothing stored yet; just send back zeroes
            ret_packet.lat = 0;
            ret_packet.lng = 0;
        } else {
            uint8_t fencepoint_offset; // 1st idx is return point
            if (packet.idx == _total-1) {
                // the is the loop closure point - send the first point again
                fencepoint_offset = 0;
            } else {
                fencepoint_offset = packet.idx - 1;
            }
            if (fencepoint_offset >= inclusion_fence->count) {
                // we haven't been given a value for this item yet; we will return zeroes
            } else {
                uint16_t storage_offset = inclusion_fence->storage_offset;
                storage_offset++; // skip over type
                storage_offset++; // skip over count
                storage_offset += 8*fencepoint_offset; // move to point we're interested in
                Vector2l bob;
                if (!read_latlon_from_storage(storage_offset, bob)) {
                    link.send_text(MAV_SEVERITY_WARNING, "Fence read failed");
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
                    AP_HAL::panic("read failure");
#endif
                    return;
                }
                ret_packet.lat = bob[0] * 1.0e-7f;
                ret_packet.lng = bob[1] * 1.0e-7f;
            }
        }
    }

    link.send_message(MAVLINK_MSG_ID_FENCE_POINT, (const char*)&ret_packet);
}

AC_PolyFence_loader::FenceIndex *AC_PolyFence_loader::get_or_create_return_point()
{
    if (!check_indexed()) {
        return nullptr;
    }
    FenceIndex *return_point = find_first_fence(AC_PolyFenceType::RETURN_POINT);
    if (return_point != nullptr) {
        return return_point;
    }

    // if the inclusion fence exists we will move it in storage to
    // avoid having to continually shift the return point forward as
    // we receive fence points
    uint16_t offset;
    const FenceIndex *inclusion_fence = find_first_fence(AC_PolyFenceType::POLYGON_INCLUSION);
    if (inclusion_fence != nullptr) {
        offset = inclusion_fence->storage_offset;
        // the "9"s below represent the size of a return point in storage
        for (uint8_t i=0; i<inclusion_fence->count; i++) {
            // we are shifting the last fence point first - so 'i=0'
            // means the last point stored.
            const uint16_t point_storage_offset = offset + 2 + (inclusion_fence->count-1-i) * 8;
            Vector2l latlon;
            uint16_t tmp_read_offs = point_storage_offset;
            if (!read_latlon_from_storage(tmp_read_offs, latlon)) {
                return nullptr;
            }
            uint16_t write_offset = point_storage_offset + 9;
            if (!write_latlon_to_storage(write_offset, latlon)) {
                return nullptr;
            }
        }
        // read/write the count:
        const uint8_t count = fence_storage.read_uint8(inclusion_fence->storage_offset+1);
        fence_storage.write_uint8(inclusion_fence->storage_offset + 1 + 9, count);
        // read/write the type:
        const uint8_t t = fence_storage.read_uint8(inclusion_fence->storage_offset);
        fence_storage.write_uint8(inclusion_fence->storage_offset + 9, t);

        uint16_t write_offset = offset + 2 + 8*inclusion_fence->count + 9;
        if (!write_eos_to_storage(write_offset)) {
            return nullptr;
        }
    } else {
        if (fence_storage.read_uint8(_eos_offset) != (uint8_t)AC_PolyFenceType::END_OF_STORAGE) {
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
            AP_HAL::panic("Expected end-of-storage marker at offset=%u",
                          _eos_offset);
#endif
            return nullptr;
        }
        offset = _eos_offset;
    }

    if (!write_type_to_storage(offset, AC_PolyFenceType::RETURN_POINT)) {
        return nullptr;
    }
    if (!write_latlon_to_storage(offset, Vector2l{0, 0})) {
        return nullptr;
    }
    if (inclusion_fence == nullptr) {
        if (!write_eos_to_storage(offset)) {
            return nullptr;
        }
    }

    if (!index_eeprom()) {
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
        AP_HAL::panic("Failed to index eeprom after moving inclusion fence for return point");
#endif
        return nullptr;
    }

    return_point = find_first_fence(AC_PolyFenceType::RETURN_POINT);
    if (return_point == nullptr) {
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
        AP_HAL::panic("Failed to get return point after indexing");
#endif
    }
    return return_point;
}

AC_PolyFence_loader::FenceIndex *AC_PolyFence_loader::get_or_create_include_fence()
{
    if (!check_indexed()) {
        return nullptr;
    }
    FenceIndex *inclusion = find_first_fence(AC_PolyFenceType::POLYGON_INCLUSION);
    if (inclusion != nullptr) {
        return inclusion;
    }
    if (_total < 5) {
        return nullptr;
    }
    if (!write_type_to_storage(_eos_offset, AC_PolyFenceType::POLYGON_INCLUSION)) {
        return nullptr;
    }
    fence_storage.write_uint8(_eos_offset, 0);
    _eos_offset++;
    if (!write_eos_to_storage(_eos_offset)) {
        return nullptr;
    }

    if (!index_eeprom()) {
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
        AP_HAL::panic("Failed to index eeprom after creating fence");
#endif
        return nullptr;
    }
    AC_PolyFence_loader::FenceIndex *ret = find_first_fence(AC_PolyFenceType::POLYGON_INCLUSION);
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
    if (ret == nullptr) {
        AP_HAL::panic("Failed to index eeprom after creating fence");
    }
#endif
    return ret;
}

void AC_PolyFence_loader::handle_msg_fence_point(GCS_MAVLINK &link, const mavlink_message_t& msg)
{
    if (!check_indexed()) {
        return;
    }

    mavlink_fence_point_t packet;
    mavlink_msg_fence_point_decode(&msg, &packet);

    if (_total != 0 && _total < 5) {
        link.send_text(MAV_SEVERITY_WARNING, "Invalid FENCE_TOTAL");
        return;
    }

    if (packet.count != _total) {
        link.send_text(MAV_SEVERITY_WARNING, "Invalid fence point, bad count (%u vs %u)", packet.count, _total.get());
        return;
    }

    if (packet.idx >= _total) {
        // this is a protocol failure
        link.send_text(MAV_SEVERITY_WARNING, "Invalid fence point, index past total (%u >= %u)", packet.idx, _total.get());
        return;
    }

    if (!check_latlng(packet.lat, packet.lng)) {
        link.send_text(MAV_SEVERITY_WARNING, "Invalid fence point, bad lat or lng");
        return;
    }

    if (!contains_compatible_fence()) {
        // the GCS has started to upload using the old protocol;
        // ensure we can accept it.  We must be able to index the
        // fence, so it must be valid (minimum number of points)
        if (!format()) {
            return;
        }
    }

    const Vector2l point{
        (int32_t)(packet.lat*1.0e7f),
        (int32_t)(packet.lng*1.0e7f)
    };

    if (packet.idx == 0) {
        // this is the return point; if we have a return point then
        // update it, otherwise create a return point fence thingy
        const FenceIndex *return_point = get_or_create_return_point();
        if (return_point == nullptr) {
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
            AP_HAL::panic("Didn't get return point");
#endif
            return;
        }
        uint16_t offset = return_point->storage_offset;
        offset++; // don't overwrite the type!
        if (!write_latlon_to_storage(offset, point)) {
            link.send_text(MAV_SEVERITY_WARNING, "PolyFence: storage write failed");
            return;
        }
    } else if (packet.idx == _total-1) {
        /* this is the fence closing point. We use this to set the vertex
           count of the inclusion fence
        */
        const FenceIndex *inclusion_fence = get_or_create_include_fence();
        if (inclusion_fence == nullptr) {
            return;
        }
        // write type and length
        fence_storage.write_uint8(inclusion_fence->storage_offset, uint8_t(AC_PolyFenceType::POLYGON_INCLUSION));
        fence_storage.write_uint8(inclusion_fence->storage_offset+1, packet.idx-1);
        // and write end of storage marker
        fence_storage.write_uint8(inclusion_fence->storage_offset+2+(packet.idx-1)*8, uint8_t(AC_PolyFenceType::END_OF_STORAGE));
        void_index();
    } else {
        const FenceIndex *inclusion_fence = get_or_create_include_fence();
        if (inclusion_fence == nullptr) {
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
            AP_HAL::panic("no inclusion fences found");
#endif
            return;
        }
        uint16_t offset = inclusion_fence->storage_offset;
        offset++; // skip type
        if (packet.idx > inclusion_fence->count) {
            // expand the storage space
            fence_storage.write_uint8(offset, packet.idx); // remembering that idx[0] is the return point....
        }
        offset++; // move past number of points
        offset += (packet.idx-1)*8;
        if (!write_latlon_to_storage(offset, point)) {
            link.send_text(MAV_SEVERITY_WARNING, "PolyFence: storage write failed");
            return;
        }
        if (_eos_offset < offset) {
            if (!write_eos_to_storage(offset)) {
                return;
            }
        }
        void_index();
    }
}

bool AC_PolyFence_loader::contains_compatible_fence() const
{
    // must contain a single inclusion fence with an optional return point
    if (_index == nullptr) {
        // this indicates no boundary points present
        return true;
    }
    bool seen_return_point = false;
    bool seen_poly_inclusion = false;
    for (uint16_t i=0; i<_num_fences; i++) {
        switch (_index[i].type) {
        case AC_PolyFenceType::END_OF_STORAGE:
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
            AP_HAL::panic("end-of-storage marker found in loaded list");
#endif
            return false;
        case AC_PolyFenceType::POLYGON_INCLUSION:
            if (seen_poly_inclusion) {
                return false;
            }
            seen_poly_inclusion = true;
            break;
#if AC_POLYFENCE_CIRCLE_INT_SUPPORT_ENABLED
        case AC_PolyFenceType::CIRCLE_INCLUSION_INT:
        case AC_PolyFenceType::CIRCLE_EXCLUSION_INT:
#endif  // AC_POLYFENCE_CIRCLE_INT_SUPPORT_ENABLED
        case AC_PolyFenceType::POLYGON_EXCLUSION:
        case AC_PolyFenceType::CIRCLE_INCLUSION:
        case AC_PolyFenceType::CIRCLE_EXCLUSION:
            return false;
        case AC_PolyFenceType::RETURN_POINT:
            if (seen_return_point) {
                return false;
            }
            seen_return_point = true;
            break;
        }
    }
    return true;
}

#endif // AC_POLYFENCE_FENCE_POINT_PROTOCOL_SUPPORT

bool AC_PolyFence_loader::write_eos_to_storage(uint16_t &offset)
{
    if (!write_type_to_storage(offset, AC_PolyFenceType::END_OF_STORAGE)) {
        return false;
    }
    _eos_offset = offset - 1; // should point to the marker
    return true;
}

/// handler for polygon fence messages with GCS
void AC_PolyFence_loader::handle_msg(GCS_MAVLINK &link, const mavlink_message_t& msg)
{
    switch (msg.msgid) {
#if AC_POLYFENCE_FENCE_POINT_PROTOCOL_SUPPORT
    case MAVLINK_MSG_ID_FENCE_POINT:
        handle_msg_fence_point(link, msg);
        break;
    case MAVLINK_MSG_ID_FENCE_FETCH_POINT:
        handle_msg_fetch_fence_point(link, msg);
        break;
#endif
    default:
        break;
    }
}

void AC_PolyFence_loader::update()
{
#if AC_POLYFENCE_FENCE_POINT_PROTOCOL_SUPPORT
    // if an older GCS sets the fence point count to zero then clear the fence
    if (_old_total != _total) {
        _old_total = _total;
        if (_total == 0 && _eeprom_fence_count) {
            if (!format()) {
                // we are in all sorts of trouble
                return;
            }
        }
    }
#endif
    if (!load_from_storage()) {
        return;
    }
}

#else  // build type is not appropriate; provide a dummy implementation:

void AC_PolyFence_loader::init() {};

bool AC_PolyFence_loader::get_item(const uint16_t seq, AC_PolyFenceItem &item) { return false; }

Vector2f* AC_PolyFence_loader::get_exclusion_polygon(uint16_t index, uint16_t &num_points) const { return nullptr; }
Vector2f* AC_PolyFence_loader::get_inclusion_polygon(uint16_t index, uint16_t &num_points) const { return nullptr; }

bool AC_PolyFence_loader::get_exclusion_circle(uint8_t index, Vector2f &center_pos_cm, float &radius) const { return false; }
bool AC_PolyFence_loader::get_inclusion_circle(uint8_t index, Vector2f &center_pos_cm, float &radius) const { return false; }

void AC_PolyFence_loader::handle_msg(GCS_MAVLINK &link, const mavlink_message_t& msg) {};

bool AC_PolyFence_loader::breached() const { return false; }
bool AC_PolyFence_loader::breached(const Location& loc, float& distance_outside_fence, Vector2f& fence_direction) const { return false; }

uint16_t AC_PolyFence_loader::max_items() const { return 0; }

bool AC_PolyFence_loader::write_fence(const AC_PolyFenceItem *new_items, uint16_t count) { return false; }

void AC_PolyFence_loader::update() {};

#if AC_POLYFENCE_FENCE_POINT_PROTOCOL_SUPPORT
bool AC_PolyFence_loader::get_return_point(Vector2l &ret) { return false; }
#endif

#endif // #if AC_FENCE_DUMMY_METHODS_ENABLED
#endif // AP_FENCE_ENABLED