/*
   This program is free software: you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation, either version 3 of the License, or
   (at your option) any later version.

   This program is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

//
//

// total up and check overflow
// check size of group var_info

/// @file   AP_Param.cpp
/// @brief  The AP variable store.
#include "AP_Param.h"

#include <cmath>
#include <string.h>

#include <AP_Common/AP_Common.h>
#include <AP_HAL/AP_HAL.h>
#include <AP_Math/AP_Math.h>
#include <GCS_MAVLink/GCS.h>
#include <StorageManager/StorageManager.h>
#include <AP_BoardConfig/AP_BoardConfig.h>
#include <AP_InternalError/AP_InternalError.h>
#include <AP_Filesystem/AP_Filesystem.h>
#include <stdio.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
    #include <SITL/SITL.h>
#endif

extern const AP_HAL::HAL &hal;

uint16_t AP_Param::sentinal_offset;

// singleton instance
AP_Param *AP_Param::_singleton;

#define ENABLE_DEBUG 0

#if ENABLE_DEBUG
 # define Debug(fmt, args ...)  do {::printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0)
#else
 # define Debug(fmt, args ...)
#endif

#if HAL_GCS_ENABLED
#define GCS_SEND_PARAM(name, type, v) gcs().send_parameter_value(name, type, v)
#else
#define GCS_SEND_PARAM(name, type, v)
#endif

// Note about AP_Vector3f handling.
// The code has special cases for AP_Vector3f to allow it to be viewed
// as both a single 3 element vector and as a set of 3 AP_Float
// variables. This is done to make it possible for MAVLink to see
// vectors as parameters, which allows users to save their compass
// offsets in MAVLink parameter files. The code involves quite a few
// special cases which could be generalised to any vector/matrix type
// if we end up needing this behaviour for other than AP_Vector3f


// static member variables for AP_Param.
//

// number of rows in the _var_info[] table
uint16_t AP_Param::_num_vars;

#if AP_PARAM_DYNAMIC_ENABLED
uint16_t AP_Param::_num_vars_base;
AP_Param::Info *AP_Param::_var_info_dynamic;
static const char *_empty_string = "";
uint8_t AP_Param::_dynamic_table_sizes[AP_PARAM_MAX_DYNAMIC];
#endif

// cached parameter count
uint16_t AP_Param::_parameter_count;
uint16_t AP_Param::_count_marker;
uint16_t AP_Param::_count_marker_done;
HAL_Semaphore AP_Param::_count_sem;

// storage and naming information about all types that can be saved
const AP_Param::Info *AP_Param::_var_info;

struct AP_Param::param_override *AP_Param::param_overrides = nullptr;
uint16_t AP_Param::num_param_overrides = 0;
uint16_t AP_Param::num_read_only = 0;

ObjectBuffer_TS<AP_Param::param_save> AP_Param::save_queue{30};
bool AP_Param::registered_save_handler;

AP_Param::defaults_list *AP_Param::default_list;
#if AP_PARAM_MAX_EMBEDDED_PARAM > 0
    AP_Param::defaults_list *AP_Param::embedded_default_list;
#endif

// we need a dummy object for the parameter save callback
static AP_Param save_dummy;

#if AP_PARAM_MAX_EMBEDDED_PARAM > 0
/*
  this holds default parameters in the normal NAME=value form for a
  parameter file. It can be manipulated by apj_tool.py to change the
  defaults on a binary without recompiling
 */
const AP_Param::param_defaults_struct AP_Param::param_defaults_data = {
    "PARMDEF",
    { 0x55, 0x37, 0xf4, 0xa0, 0x38, 0x5d, 0x48, 0x5b },
    AP_PARAM_MAX_EMBEDDED_PARAM,
    0
};
#endif

// storage object
StorageAccess AP_Param::_storage(StorageManager::StorageParam);

// backup storage object
StorageAccess AP_Param::_storage_bak(StorageManager::StorageParamBak);

// flags indicating frame type
uint16_t AP_Param::_frame_type_flags;

// write to EEPROM
void AP_Param::eeprom_write_check(const void *ptr, uint16_t ofs, uint8_t size)
{
    _storage.write_block(ofs, ptr, size);
    _storage_bak.write_block(ofs, ptr, size);
}

bool AP_Param::_hide_disabled_groups = true;

// write a sentinal value at the given offset
void AP_Param::write_sentinal(uint16_t ofs)
{
    struct Param_header phdr;
    phdr.type = _sentinal_type;
    set_key(phdr, _sentinal_key);
    phdr.group_element = _sentinal_group;
    eeprom_write_check(&phdr, ofs, sizeof(phdr));
    sentinal_offset = ofs;
}

// erase all EEPROM variables by re-writing the header and adding
// a sentinal
void AP_Param::erase_all(void)
{
    struct EEPROM_header hdr;

    // write the header
    hdr.magic[0] = k_EEPROM_magic0;
    hdr.magic[1] = k_EEPROM_magic1;
    hdr.revision = k_EEPROM_revision;
    hdr.spare    = 0;
    eeprom_write_check(&hdr, 0, sizeof(hdr));

    // add a sentinal directly after the header
    write_sentinal(sizeof(struct EEPROM_header));
}

/* the 'group_id' of a element of a group is the 18 bit identifier
   used to distinguish between this element of the group and other
   elements of the same group. It is calculated using a bit shift per
   level of nesting, so the first level of nesting gets 6 bits the 2nd
   level gets the next 6 bits, and the 3rd level gets the last 6
   bits. This limits groups to having at most 64 elements.
*/
uint32_t AP_Param::group_id(const struct GroupInfo *grpinfo, uint32_t base, uint8_t i, uint8_t shift)
{
    if (grpinfo[i].idx == 0 && shift != 0 && !(grpinfo[i].flags & AP_PARAM_FLAG_NO_SHIFT)) {
        /*
          this is a special case for a bug in the original design. An
          idx of 0 shifted by n bits is still zero, which makes it
          indistinguishable from a different parameter. This can lead
          to parameter loops. We use index 63 for that case.
        */
        return base + (63U<<shift);
    }
    return base + (grpinfo[i].idx<<shift);
}


/*
  check if a frame type should be included. A frame is included if
  either there are no frame type flags on a parameter or if at least
  one of the flags has been set by set_frame_type_flags()
 */
bool AP_Param::check_frame_type(uint16_t flags)
{
    if (flags & AP_PARAM_FLAG_HIDDEN) {
        // hidden on all frames
        return false;
    }
    uint16_t frame_flags = flags >> AP_PARAM_FRAME_TYPE_SHIFT;
    if (frame_flags == 0) {
        return true;
    }
    return (frame_flags & _frame_type_flags) != 0;
}

// validate a group info table
bool AP_Param::check_group_info(const struct AP_Param::GroupInfo *  group_info,
                                uint16_t *                          total_size,
                                uint8_t                             group_shift,
                                uint8_t                             prefix_length)
{
    uint8_t type;
    uint64_t used_mask = 0;
    for (uint8_t i=0;
         (type=group_info[i].type) != AP_PARAM_NONE;
         i++) {
        uint8_t idx = group_info[i].idx;
        if (idx >= (1<<_group_level_shift)) {
            Debug("idx too large (%u) in %s", idx, group_info[i].name);
            return false;
        }
        if (group_shift != 0 && idx == 0) {
            // treat idx 0 as 63 for duplicates. See group_id()
            idx = 63;
        }
        if (used_mask & (1ULL<<idx)) {
            Debug("Duplicate group idx %u for %s", idx, group_info[i].name);
            return false;
        }
        used_mask |= (1ULL<<idx);
        if (type == AP_PARAM_GROUP) {
            // a nested group
            if (group_shift + _group_level_shift >= _group_bits) {
                Debug("double group nesting in %s", group_info[i].name);
                return false;
            }
            const struct GroupInfo *ginfo = get_group_info(group_info[i]);
            if (ginfo == nullptr) {
                continue;
            }
            if (!check_group_info(ginfo, total_size, group_shift + _group_level_shift, prefix_length + strlen(group_info[i].name))) {
                return false;
            }
            continue;
        }
        uint8_t size = type_size((enum ap_var_type)type);
        if (size == 0) {
            Debug("invalid type in %s", group_info[i].name);
            return false;
        }
        if (prefix_length + strlen(group_info[i].name) > 16) {
            Debug("suffix is too long in %s", group_info[i].name);
            return false;
        }
        (*total_size) += size + sizeof(struct Param_header);
    }
    return true;
}

// check for duplicate key values
bool AP_Param::duplicate_key(uint16_t vindex, uint16_t key)
{
    for (uint16_t i=vindex+1; i<_num_vars; i++) {
        uint16_t key2 = var_info(i).key;
        if (key2 == key) {
            // no duplicate keys allowed
            return true;
        }
    }
    return false;
}

/*
  get group_info pointer for a group
 */
const struct AP_Param::GroupInfo *AP_Param::get_group_info(const struct GroupInfo &ginfo)
{
    if (ginfo.flags & AP_PARAM_FLAG_INFO_POINTER) {
        return *ginfo.group_info_ptr;
    }
    return ginfo.group_info;
}

/*
  get group_info pointer for a group
 */
const struct AP_Param::GroupInfo *AP_Param::get_group_info(const struct Info &info)
{
    if (info.flags & AP_PARAM_FLAG_INFO_POINTER) {
        return *info.group_info_ptr;
    }
    return info.group_info;
}

// validate the _var_info[] table
bool AP_Param::check_var_info(void)
{
    uint16_t total_size = sizeof(struct EEPROM_header);

    for (uint16_t i=0; i<_num_vars; i++) {
        const auto &info = var_info(i);
        uint8_t type = info.type;
        uint16_t key = info.key;
        if (type == AP_PARAM_GROUP) {
            if (i == 0) {
                // first element can't be a group, for first() call
                return false;
            }
            const struct GroupInfo *group_info = get_group_info(info);
            if (group_info == nullptr) {
                continue;
            }
            if (!check_group_info(group_info, &total_size, 0, strlen(info.name))) {
                return false;
            }
        } else {
            uint8_t size = type_size((enum ap_var_type)type);
            if (size == 0) {
                // not a valid type - the top level list can't contain
                // AP_PARAM_NONE
                return false;
            }
            total_size += size + sizeof(struct Param_header);
        }
        if (duplicate_key(i, key)) {
            return false;
        }
        if (type != AP_PARAM_GROUP && (info.flags & AP_PARAM_FLAG_POINTER)) {
            // only groups can be pointers
            return false;
        }
    }

    // we no longer check if total_size is larger than _eeprom_size,
    // as we allow for more variables than could fit, relying on not
    // saving default values

    return true;
}


// setup the _var_info[] table
bool AP_Param::setup(void)
{
    struct EEPROM_header hdr {};
    struct EEPROM_header hdr2 {};

    // check the header
    _storage.read_block(&hdr, 0, sizeof(hdr));
    _storage_bak.read_block(&hdr2, 0, sizeof(hdr2));
    if (hdr.magic[0] != k_EEPROM_magic0 ||
        hdr.magic[1] != k_EEPROM_magic1 ||
        hdr.revision != k_EEPROM_revision) {
        if (hdr2.magic[0] == k_EEPROM_magic0 &&
            hdr2.magic[1] == k_EEPROM_magic1 &&
            hdr2.revision == k_EEPROM_revision &&
            _storage.copy_area(_storage_bak)) {
            // restored from backup
            INTERNAL_ERROR(AP_InternalError::error_t::params_restored);
            return true;
        }
        // header doesn't match. We can't recover any variables. Wipe
        // the header and setup the sentinal directly after the header
        Debug("bad header in setup - erasing");
        erase_all();
    }

    // ensure that backup is in sync with primary
    _storage_bak.copy_area(_storage);

    return true;
}

// check if AP_Param has been initialised
bool AP_Param::initialised(void)
{
    return _var_info != nullptr;
}

/*
  adjust offset of a group element for nested groups and group pointers

  The new_offset variable is relative to the vindex base. This makes
  dealing with pointer groups tricky
 */
bool AP_Param::adjust_group_offset(uint16_t vindex, const struct GroupInfo &group_info, ptrdiff_t &new_offset)
{
    if (group_info.flags & AP_PARAM_FLAG_NESTED_OFFSET) {
        new_offset += group_info.offset;
        return true;
    }
    if (group_info.flags & AP_PARAM_FLAG_POINTER) {
        // group_info.offset refers to a pointer
        ptrdiff_t base;
        if (!get_base(var_info(vindex), base)) {
            // the object is not allocated yet
            return false;
        }
        void **p = (void **)(base + new_offset + group_info.offset);
        if (*p == nullptr) {
            // the object is not allocated yet
            return false;
        }
        // calculate offset that is needed to take base object and adjust for this object
        new_offset = ((ptrdiff_t)*p) - base;
    }
    return true;
}

/*
  get the base pointer for a variable, accounting for AP_PARAM_FLAG_POINTER
 */
bool AP_Param::get_base(const struct Info &info, ptrdiff_t &base)
{
    if (info.flags & AP_PARAM_FLAG_POINTER) {
        base = *(ptrdiff_t *)info.ptr;
        return (base != (ptrdiff_t)0);
    }
    base = (ptrdiff_t)info.ptr;
    return true;
}

namespace AP {

AP_Param *param()
{
    return AP_Param::get_singleton();
}

}

// find the info structure given a header and a group_info table
// return the Info structure and a pointer to the variables storage
const struct AP_Param::Info *AP_Param::find_by_header_group(struct Param_header phdr, void **ptr,
                                                            uint16_t vindex,
                                                            const struct GroupInfo *group_info,
                                                            uint32_t group_base,
                                                            uint8_t group_shift,
                                                            ptrdiff_t group_offset)
{
    uint8_t type;
    for (uint8_t i=0;
         (type=group_info[i].type) != AP_PARAM_NONE;
         i++) {
        if (type == AP_PARAM_GROUP) {
            // a nested group
            if (group_shift + _group_level_shift >= _group_bits) {
                // too deeply nested - this should have been caught by
                // setup() !
                return nullptr;
            }
            const struct GroupInfo *ginfo = get_group_info(group_info[i]);
            if (ginfo == nullptr) {
                continue;
            }
            ptrdiff_t new_offset = group_offset;

            if (!adjust_group_offset(vindex, group_info[i], new_offset)) {
                continue;
            }

            const struct AP_Param::Info *ret = find_by_header_group(phdr, ptr, vindex, ginfo,
                                                                    group_id(group_info, group_base, i, group_shift),
                                                                    group_shift + _group_level_shift, new_offset);
            if (ret != nullptr) {
                return ret;
            }
            continue;
        }
        if (group_id(group_info, group_base, i, group_shift) == phdr.group_element && type == phdr.type) {
            // found a group element
            ptrdiff_t base;
            if (!get_base(var_info(vindex), base)) {
                continue;
            }
            *ptr = (void*)(base + group_info[i].offset + group_offset);
            return &var_info(vindex);
        }
    }
    return nullptr;
}

// find the info structure given a header
// return the Info structure and a pointer to the variables storage
const struct AP_Param::Info *AP_Param::find_by_header(struct Param_header phdr, void **ptr)
{
    // loop over all named variables
    for (uint16_t i=0; i<_num_vars; i++) {
        const auto &info = var_info(i);
        uint8_t type = info.type;
        uint16_t key = info.key;
        if (key != get_key(phdr)) {
            // not the right key
            continue;
        }
        if (type == AP_PARAM_GROUP) {
            const struct GroupInfo *group_info = get_group_info(info);
            if (group_info == nullptr) {
                continue;
            }
            return find_by_header_group(phdr, ptr, i, group_info, 0, 0, 0);
        }
        if (type == phdr.type) {
            // found it
            ptrdiff_t base;
            if (!get_base(info, base)) {
                return nullptr;
            }
            *ptr = (void*)base;
            return &info;
        }
    }
    return nullptr;
}

// find the info structure for a variable in a group
const struct AP_Param::Info *AP_Param::find_var_info_group(const struct GroupInfo * group_info,
                                                           uint16_t                 vindex,
                                                           uint32_t                 group_base,
                                                           uint8_t                  group_shift,
                                                           ptrdiff_t                group_offset,
                                                           uint32_t *               group_element,
                                                           const struct GroupInfo * &group_ret,
                                                           struct GroupNesting      &group_nesting,
                                                           uint8_t *                idx) const
{
    ptrdiff_t base;
    if (!get_base(var_info(vindex), base)) {
        return nullptr;
    }
    uint8_t type;
    for (uint8_t i=0;
         (type=group_info[i].type) != AP_PARAM_NONE;
         i++) {
        ptrdiff_t ofs = group_info[i].offset + group_offset;
        if (type == AP_PARAM_GROUP) {
            const struct GroupInfo *ginfo = get_group_info(group_info[i]);
            if (ginfo == nullptr) {
                continue;
            }
            // a nested group
            if (group_shift + _group_level_shift >= _group_bits) {
                // too deeply nested - this should have been caught by
                // setup() !
                return nullptr;
            }
            const struct AP_Param::Info *info;
            ptrdiff_t new_offset = group_offset;
            if (!adjust_group_offset(vindex, group_info[i], new_offset)) {
                continue;
            }
            if (group_nesting.level >= group_nesting.numlevels) {
                return nullptr;
            }
            group_nesting.group_ret[group_nesting.level++] = &group_info[i];
            info = find_var_info_group(ginfo, vindex,
                                       group_id(group_info, group_base, i, group_shift),
                                       group_shift + _group_level_shift,
                                       new_offset,
                                       group_element,
                                       group_ret,
                                       group_nesting,
                                       idx);
            if (info != nullptr) {
                return info;
            }
            group_nesting.level--;
        } else if ((ptrdiff_t) this == base + ofs) {
            *group_element = group_id(group_info, group_base, i, group_shift);
            group_ret = &group_info[i];
            *idx = 0;
            return &var_info(vindex);
        } else if (type == AP_PARAM_VECTOR3F &&
                   (base+ofs+(ptrdiff_t)sizeof(float) == (ptrdiff_t) this ||
                    base+ofs+2*(ptrdiff_t)sizeof(float) == (ptrdiff_t) this)) {
            // we are inside a Vector3f. We need to work out which
            // element of the vector the current object refers to.
            *idx = (((ptrdiff_t) this) - (base+ofs))/sizeof(float);
            *group_element = group_id(group_info, group_base, i, group_shift);
            group_ret = &group_info[i];
            return &var_info(vindex);
        }
    }
    return nullptr;
}

// find the info structure for a variable
const struct AP_Param::Info *AP_Param::find_var_info(uint32_t *                 group_element,
                                                     const struct GroupInfo *   &group_ret,
                                                     struct GroupNesting        &group_nesting,
                                                     uint8_t *                  idx) const
{
    group_ret = nullptr;
    
    for (uint16_t i=0; i<_num_vars; i++) {
        const auto &info = var_info(i);
        uint8_t type = info.type;
        ptrdiff_t base;
        if (!get_base(info, base)) {
            continue;
        }
        if (type == AP_PARAM_GROUP) {
            const struct GroupInfo *group_info = get_group_info(info);
            if (group_info == nullptr) {
                continue;
            }
            const struct AP_Param::Info *info2;
            info2 = find_var_info_group(group_info, i, 0, 0, 0, group_element, group_ret, group_nesting, idx);
            if (info2 != nullptr) {
                return info2;
            }
        } else if (base == (ptrdiff_t) this) {
            *group_element = 0;
            *idx = 0;
            return &info;
        } else if (type == AP_PARAM_VECTOR3F &&
                   (base+(ptrdiff_t)sizeof(float) == (ptrdiff_t) this ||
                    base+2*(ptrdiff_t)sizeof(float) == (ptrdiff_t) this)) {
            // we are inside a Vector3f. Work out which element we are
            // referring to.
            *idx = (((ptrdiff_t) this) - base)/sizeof(float);
            *group_element = 0;
            return &info;
        }
    }
    return nullptr;
}


// find the info structure for a variable
const struct AP_Param::Info *AP_Param::find_var_info_token(const ParamToken &token,
                                                           uint32_t *                 group_element,
                                                           const struct GroupInfo *   &group_ret,
                                                           struct GroupNesting        &group_nesting,
                                                           uint8_t *                  idx) const
{
    uint16_t i = token.key;
    const auto &info = var_info(i);
    uint8_t type = info.type;
    ptrdiff_t base;
    if (!get_base(info, base)) {
        return nullptr;
    }
    group_ret = nullptr;
    
    if (type == AP_PARAM_GROUP) {
        const struct GroupInfo *group_info = get_group_info(info);
        if (group_info == nullptr) {
            return nullptr;
        }
        const struct AP_Param::Info *info2;
        info2 = find_var_info_group(group_info, i, 0, 0, 0, group_element, group_ret, group_nesting, idx);
        if (info2 != nullptr) {
            return info2;
        }
    } else if (base == (ptrdiff_t) this) {
        *group_element = 0;
        *idx = 0;
        return &info;
    } else if (type == AP_PARAM_VECTOR3F &&
               (base+(ptrdiff_t)sizeof(float) == (ptrdiff_t) this ||
                base+2*(ptrdiff_t)sizeof(float) == (ptrdiff_t) this)) {
        // we are inside a Vector3f. Work out which element we are
        // referring to.
        *idx = (((ptrdiff_t) this) - base)/sizeof(float);
        *group_element = 0;
        return &info;
    }
    return nullptr;
}

// return the storage size for a AP_PARAM_* type
uint8_t AP_Param::type_size(enum ap_var_type type)
{
    switch (type) {
    case AP_PARAM_NONE:
    case AP_PARAM_GROUP:
        return 0;
    case AP_PARAM_INT8:
        return 1;
    case AP_PARAM_INT16:
        return 2;
    case AP_PARAM_INT32:
        return 4;
    case AP_PARAM_FLOAT:
        return 4;
    case AP_PARAM_VECTOR3F:
        return 3*4;
    }
    Debug("unknown type %d\n", type);
    return 0;
}

/*
  extract 9 bit key from Param_header
 */
uint16_t AP_Param::get_key(const Param_header &phdr)
{
    return ((uint16_t)phdr.key_high)<<8 | phdr.key_low;
}

/*
  set 9 bit key in Param_header
 */
void AP_Param::set_key(Param_header &phdr, uint16_t key)
{
    phdr.key_low  = key & 0xFF;
    phdr.key_high = key >> 8;
}

/*
  return true if a header is the end of eeprom sentinal
 */
bool AP_Param::is_sentinal(const Param_header &phdr)
{
    // note that this is an ||, not an && on the key and group, as
    // this makes us more robust to power off while adding a variable
    // to EEPROM
    if (phdr.type == _sentinal_type ||
        get_key(phdr) == _sentinal_key) {
        return true;
    }
    // also check for 0xFFFFFFFF and 0x00000000, which are the fill
    // values for storage. These can appear if power off occurs while
    // writing data
    uint32_t v = *(uint32_t *)&phdr;
    if (v == 0 || v == 0xFFFFFFFF) {
        return true;
    }
    return false;
}

// scan the EEPROM looking for a given variable by header content
// return true if found, along with the offset in the EEPROM where
// the variable is stored
// if not found return the offset of the sentinal
// if the sentinal isn't found either, the offset is set to 0xFFFF
bool AP_Param::scan(const AP_Param::Param_header *target, uint16_t *pofs)
{
    struct Param_header phdr;
    uint16_t ofs = sizeof(AP_Param::EEPROM_header);
    while (ofs < _storage.size()) {
        _storage.read_block(&phdr, ofs, sizeof(phdr));
        if (phdr.type == target->type &&
            get_key(phdr) == get_key(*target) &&
            phdr.group_element == target->group_element) {
            // found it
            *pofs = ofs;
            return true;
        }
        if (is_sentinal(phdr)) {
            // we've reached the sentinal
            *pofs = ofs;
            sentinal_offset = ofs;
            return false;
        }
        ofs += type_size((enum ap_var_type)phdr.type) + sizeof(phdr);
    }
    *pofs = 0xffff;
    Debug("scan past end of eeprom");
    return false;
}

/**
 * add a _X, _Y, _Z suffix to the name of a Vector3f element
 * @param buffer
 * @param buffer_size
 * @param idx Suffix: 0 --> _X; 1 --> _Y; 2 --> _Z; (other --> undefined)
 */
void AP_Param::add_vector3f_suffix(char *buffer, size_t buffer_size, uint8_t idx) const
{
    const size_t len = strnlen(buffer, buffer_size);
    if (len + 2 <= buffer_size) {
        buffer[len] = '_';
        buffer[len + 1] = static_cast<char>('X' + idx);
        if (len + 3 <= buffer_size) {
            buffer[len + 2] = 0;
        }
    }
}

// Copy the variable's whole name to the supplied buffer.
//
// If the variable is a group member, prepend the group name.
//
void AP_Param::copy_name_token(const ParamToken &token, char *buffer, size_t buffer_size, bool force_scalar) const
{
    uint32_t group_element;
    const struct GroupInfo *ginfo;
    struct GroupNesting group_nesting {};
    uint8_t idx;
    const struct AP_Param::Info *info = find_var_info_token(token, &group_element, ginfo, group_nesting, &idx);
    if (info == nullptr) {
        *buffer = 0;
        Debug("no info found");
        return;
    }
    copy_name_info(info, ginfo, group_nesting, idx, buffer, buffer_size, force_scalar);
}

void AP_Param::copy_name_info(const struct AP_Param::Info *info,
                              const struct GroupInfo *ginfo,
                              const struct GroupNesting &group_nesting,
                              uint8_t idx, char *buffer, size_t buffer_size, bool force_scalar) const
{
    strncpy(buffer, info->name, buffer_size);
    for (uint8_t i=0; i<group_nesting.level; i++) {
        uint8_t len = strnlen(buffer, buffer_size);
        if (len < buffer_size) {
            strncpy(&buffer[len], group_nesting.group_ret[i]->name, buffer_size-len);
        }
    }
    if (ginfo != nullptr) {
        uint8_t len = strnlen(buffer, buffer_size);
        if (len < buffer_size) {
            strncpy(&buffer[len], ginfo->name, buffer_size-len);
        }
        if ((force_scalar || idx != 0) && AP_PARAM_VECTOR3F == ginfo->type) {
            // the caller wants a specific element in a Vector3f
            add_vector3f_suffix(buffer, buffer_size, idx);
        }
    } else if ((force_scalar || idx != 0) && AP_PARAM_VECTOR3F == info->type) {
        add_vector3f_suffix(buffer, buffer_size, idx);
    }
}

// Find a variable by name in a group
AP_Param *
AP_Param::find_group(const char *name, uint16_t vindex, ptrdiff_t group_offset,
                     const struct GroupInfo *group_info, enum ap_var_type *ptype)
{
    uint8_t type;
    for (uint8_t i=0;
         (type=group_info[i].type) != AP_PARAM_NONE;
         i++) {
        if (type == AP_PARAM_GROUP) {
            if (strncasecmp(name, group_info[i].name, strlen(group_info[i].name)) != 0) {
                continue;
            }
            const struct GroupInfo *ginfo = get_group_info(group_info[i]);
            if (ginfo == nullptr) {
                continue;
            }
            ptrdiff_t new_offset = group_offset;

            if (!adjust_group_offset(vindex, group_info[i], new_offset)) {
                continue;
            }

            AP_Param *ap = find_group(name+strlen(group_info[i].name), vindex, new_offset, ginfo, ptype);
            if (ap != nullptr) {
                return ap;
            }
        } else if (strcasecmp(name, group_info[i].name) == 0) {
            ptrdiff_t base;
            if (!get_base(var_info(vindex), base)) {
                continue;
            }
            *ptype = (enum ap_var_type)type;
            return (AP_Param *)(base + group_info[i].offset + group_offset);
        } else if (type == AP_PARAM_VECTOR3F) {
            // special case for finding Vector3f elements
            uint8_t suffix_len = strnlen(group_info[i].name, AP_MAX_NAME_SIZE);
            if (strncmp(name, group_info[i].name, suffix_len) == 0 &&
                name[suffix_len] == '_' &&
                (name[suffix_len+1] == 'X' ||
                 name[suffix_len+1] == 'Y' ||
                 name[suffix_len+1] == 'Z')) {
                ptrdiff_t base;
                if (!get_base(var_info(vindex), base)) {
                    continue;
                }
                AP_Float *v = (AP_Float *)(base + group_info[i].offset + group_offset);
                *ptype = AP_PARAM_FLOAT;
                switch (name[suffix_len+1]) {
                case 'X':
                    return (AP_Float *)&v[0];
                case 'Y':
                    return (AP_Float *)&v[1];
                case 'Z':
                    return (AP_Float *)&v[2];
                }
            }
        }
    }
    return nullptr;
}


// Find a variable by name.
//
AP_Param *
AP_Param::find(const char *name, enum ap_var_type *ptype, uint16_t *flags)
{
    for (uint16_t i=0; i<_num_vars; i++) {
        const auto &info = var_info(i);
        uint8_t type = info.type;
        if (type == AP_PARAM_GROUP) {
            uint8_t len = strnlen(info.name, AP_MAX_NAME_SIZE);
            if (strncmp(name, info.name, len) != 0) {
                continue;
            }
            const struct GroupInfo *group_info = get_group_info(info);
            if (group_info == nullptr) {
                continue;
            }
            AP_Param *ap = find_group(name + len, i, 0, group_info, ptype);
            if (ap != nullptr) {
                if (flags != nullptr) {
                    uint32_t group_element = 0;
                    const struct GroupInfo *ginfo;
                    struct GroupNesting group_nesting {};
                    uint8_t idx;
                    ap->find_var_info(&group_element, ginfo, group_nesting, &idx);
                    if (ginfo != nullptr) {
                        *flags = ginfo->flags;
                    }
                }
                return ap;
            }
            // we continue looking as we want to allow top level
            // parameter to have the same prefix name as group
            // parameters, for example CAM_P_G
        } else if (strcasecmp(name, info.name) == 0) {
            *ptype = (enum ap_var_type)type;
            ptrdiff_t base;
            if (!get_base(info, base)) {
                return nullptr;
            }
            return (AP_Param *)base;
        }
    }
    return nullptr;
}

// Find a variable by index. Note that this is quite slow.
//
AP_Param *
AP_Param::find_by_index(uint16_t idx, enum ap_var_type *ptype, ParamToken *token)
{
    AP_Param *ap;
    uint16_t count=0;
    for (ap=AP_Param::first(token, ptype);
         ap && count < idx;
         ap=AP_Param::next_scalar(token, ptype)) {
        count++;
    }
    return ap;    
}

// by-name equivalent of find_by_index()
AP_Param* AP_Param::find_by_name(const char* name, enum ap_var_type *ptype, ParamToken *token)
{
    AP_Param *ap;
    uint16_t count = 0;
    for (ap = AP_Param::first(token, ptype);
         ap && *ptype != AP_PARAM_GROUP && *ptype != AP_PARAM_NONE;
         ap = AP_Param::next_scalar(token, ptype)) {
        int32_t ret = strncasecmp(name, var_info(token->key).name, AP_MAX_NAME_SIZE);
        if (ret >= 0) {
            char buf[AP_MAX_NAME_SIZE];
            ap->copy_name_token(*token, buf, AP_MAX_NAME_SIZE);
            if (strncasecmp(name, buf, AP_MAX_NAME_SIZE) == 0) {
                break;
            }
        }
        count++;
    }
    return ap;
}

/*
  Find a variable by pointer, returning key. This is used for loading pointer variables
*/
bool AP_Param::find_key_by_pointer_group(const void *ptr, uint16_t vindex,
                                         const struct GroupInfo *group_info,
                                         ptrdiff_t offset, uint16_t &key)
{
    for (uint8_t i=0; group_info[i].type != AP_PARAM_NONE; i++) {
        if (group_info[i].type != AP_PARAM_GROUP) {
            continue;
        }
        ptrdiff_t base;
        if (!get_base(var_info(vindex), base)) {
            continue;
        }
        if (group_info[i].flags & AP_PARAM_FLAG_POINTER) {
            if (ptr == *(void **)(base+group_info[i].offset+offset)) {
                key = var_info(vindex).key;
                return true;
            }
        } else if (ptr == (void *)(base+group_info[i].offset+offset)) {
            key = var_info(vindex).key;
            return true;
        }
        ptrdiff_t new_offset = offset;
        if (!adjust_group_offset(vindex, group_info[i], new_offset)) {
            continue;
        }
        const struct GroupInfo *ginfo = get_group_info(group_info[i]);
        if (ginfo == nullptr) {
            continue;
        }
        if (find_key_by_pointer_group(ptr, vindex, ginfo, new_offset, key)) {
            return true;
        }
    }
    return false;
}


/*
  Find a variable by pointer, returning key. This is used for loading pointer variables
*/
bool AP_Param::find_key_by_pointer(const void *ptr, uint16_t &key)
{
    for (uint16_t i=0; i<_num_vars; i++) {
        const auto &info = var_info(i);
        if (info.type != AP_PARAM_GROUP) {
            continue;
        }
        if ((info.flags & AP_PARAM_FLAG_POINTER) &&
            ptr == *(void **)info.ptr) {
            key = info.key;
            return true;
        }
        ptrdiff_t offset = 0;
        const struct GroupInfo *ginfo = get_group_info(info);
        if (ginfo == nullptr) {
            continue;
        }
        if (find_key_by_pointer_group(ptr, i, ginfo, offset, key)) {
            return true;
        }
    }
    return false;
}

/*
  Find key to top level group parameters by pointer
*/
bool AP_Param::find_top_level_key_by_pointer(const void *ptr, uint16_t &key)
{
    for (uint16_t i=0; i<_num_vars; i++) {
        const auto &info = var_info(i);
        if (info.type != AP_PARAM_GROUP) {
            continue;
        }
        if (ptr == (void **)info.ptr) {
            key = info.key;
            return true;
        }
    }
    return false;
}

/*
  fetch a parameter value based on the index within a group. This
  is used to find the old value of a parameter that has been
  removed from an object.
*/
bool AP_Param::get_param_by_index(void *obj_ptr, uint8_t idx, ap_var_type old_ptype, void *pvalue)
{
    uint16_t key;
    if (!find_top_level_key_by_pointer(obj_ptr, key)) {
        return false;
    }
    const ConversionInfo type_info = {key, idx, old_ptype, nullptr };
    return AP_Param::find_old_parameter(&type_info, (AP_Param *)pvalue);
}


// Find a object by name.
//
AP_Param *
AP_Param::find_object(const char *name)
{
    for (uint16_t i=0; i<_num_vars; i++) {
        const auto &info = var_info(i);
        if (strcasecmp(name, info.name) == 0) {
            ptrdiff_t base;
            if (!get_base(info, base)) {
                return nullptr;
            }
            return (AP_Param *)base;
        }
    }
    return nullptr;
}

// notify GCS of current value of parameter
void AP_Param::notify() const {
    uint32_t group_element = 0;
    const struct GroupInfo *ginfo;
    struct GroupNesting group_nesting {};
    uint8_t idx;

    const struct AP_Param::Info *info = find_var_info(&group_element, ginfo, group_nesting, &idx);
    if (info == nullptr) {
        // this is probably very bad
        return;
    }

    char name[AP_MAX_NAME_SIZE+1];
    copy_name_info(info, ginfo, group_nesting, idx, name, sizeof(name), true);

    uint32_t param_header_type;
    if (ginfo != nullptr) {
        param_header_type = ginfo->type;
    } else {
        param_header_type = info->type;
    }

    send_parameter(name, (enum ap_var_type)param_header_type, idx);
}


/*
  Save the variable to HAL storage, synchronous version
*/
void AP_Param::save_sync(bool force_save, bool send_to_gcs)
{
    uint32_t group_element = 0;
    const struct GroupInfo *ginfo;
    struct GroupNesting group_nesting {};
    uint8_t idx;
    const struct AP_Param::Info *info = find_var_info(&group_element, ginfo, group_nesting, &idx);
    const AP_Param *ap;

    if (info == nullptr) {
        // we don't have any info on how to store it
        return;
    }

    struct Param_header phdr;

    // create the header we will use to store the variable
    if (ginfo != nullptr) {
        phdr.type = ginfo->type;
        if (ginfo->flags & AP_PARAM_FLAG_HIDDEN) {
            send_to_gcs = false;
        }
    } else {
        phdr.type = info->type;
        if (info->flags & AP_PARAM_FLAG_HIDDEN) {
            send_to_gcs = false;
        }
    }
    set_key(phdr, info->key);
    phdr.group_element = group_element;

    ap = this;
    if (phdr.type != AP_PARAM_VECTOR3F && idx != 0) {
        // only vector3f can have non-zero idx for now
        return;
    }
    if (idx != 0) {
        ap = (const AP_Param *)((ptrdiff_t)ap) - (idx*sizeof(float));
    }

    if (phdr.type == AP_PARAM_INT8 && ginfo != nullptr && (ginfo->flags & AP_PARAM_FLAG_ENABLE)) {
        // clear cached parameter count
        invalidate_count();
    }
    
    char name[AP_MAX_NAME_SIZE+1];
    copy_name_info(info, ginfo, group_nesting, idx, name, sizeof(name), true);

    // scan EEPROM to find the right location
    uint16_t ofs;
    if (scan(&phdr, &ofs)) {
        // found an existing copy of the variable
        eeprom_write_check(ap, ofs+sizeof(phdr), type_size((enum ap_var_type)phdr.type));
        if (send_to_gcs) {
            send_parameter(name, (enum ap_var_type)phdr.type, idx);
        }
        return;
    }
    if (ofs == (uint16_t) ~0) {
        return;
    }

    // if the value is the default value then don't save
    if (phdr.type <= AP_PARAM_FLOAT) {
        float v1 = cast_to_float((enum ap_var_type)phdr.type);
        float v2;
        if (ginfo != nullptr) {
            v2 = get_default_value(this, &ginfo->def_value);
        } else {
            v2 = get_default_value(this, &info->def_value);
        }
        if (is_equal(v1,v2) && !force_save) {
            if (send_to_gcs) {
                GCS_SEND_PARAM(name, (enum ap_var_type)info->type, v2);
            }
            return;
        }
        if (!force_save &&
            (phdr.type != AP_PARAM_INT32 &&
             (fabsf(v1-v2) < 0.0001f*fabsf(v1)))) {
            // for other than 32 bit integers, we accept values within
            // 0.01 percent of the current value as being the same
            if (send_to_gcs) {
                GCS_SEND_PARAM(name, (enum ap_var_type)info->type, v2);
            }
            return;
        }
    }

    if (ofs+type_size((enum ap_var_type)phdr.type)+2*sizeof(phdr) >= _storage.size()) {
        // we are out of room for saving variables
        DEV_PRINTF("EEPROM full\n");
        return;
    }

    // write a new sentinal, then the data, then the header
    write_sentinal(ofs + sizeof(phdr) + type_size((enum ap_var_type)phdr.type));
    eeprom_write_check(ap, ofs+sizeof(phdr), type_size((enum ap_var_type)phdr.type));
    eeprom_write_check(&phdr, ofs, sizeof(phdr));

    if (send_to_gcs) {
        send_parameter(name, (enum ap_var_type)phdr.type, idx);
    }
}

/*
  put variable into queue to be saved
*/
void AP_Param::save(bool force_save)
{
    struct param_save p, p2;
    p.param = this;
    p.force_save = force_save;
    if (save_queue.peek(p2) &&
        p2.param == this &&
        p2.force_save == force_save) {
        // this one is already at the head of the list to be
        // saved. This check is cheap and catches the case where we
        // are flooding the save queue with one parameter (eg. mission
        // creation, changing MIS_TOTAL)
        return;
    }
    while (!save_queue.push(p)) {
        // if we can't save to the queue
        if (hal.util->get_soft_armed() && hal.scheduler->in_main_thread()) {
            // if we are armed in main thread then don't sleep, instead we lose the
            // parameter save
            return;
        }
        // when we are disarmed then loop waiting for a slot to become
        // available. This guarantees completion for large parameter
        // set loads
        hal.scheduler->expect_delay_ms(1);
        hal.scheduler->delay_microseconds(500);
        hal.scheduler->expect_delay_ms(0);
    }
}

/*
  background function for saving parameters. This runs on the IO thread
 */
void AP_Param::save_io_handler(void)
{
    struct param_save p;
    while (save_queue.pop(p)) {
        p.param->save_sync(p.force_save, true);
    }
    if (hal.scheduler->is_system_initialized()) {
        // pay the cost of parameter counting in the IO thread
        count_parameters();
    }
}

/*
  wait for all parameters to save
*/
void AP_Param::flush(void)
{
    uint16_t counter = 200; // 2 seconds max
    while (counter-- && save_queue.available()) {
        hal.scheduler->expect_delay_ms(10);
        hal.scheduler->delay(10);
        hal.scheduler->expect_delay_ms(0);
    }
}

// Load the variable from EEPROM, if supported
//
bool AP_Param::load(void)
{
    uint32_t group_element = 0;
    const struct GroupInfo *ginfo;
    struct GroupNesting group_nesting {};
    uint8_t idx;
    const struct AP_Param::Info *info = find_var_info(&group_element, ginfo, group_nesting, &idx);
    if (info == nullptr) {
        // we don't have any info on how to load it
        return false;
    }

    struct Param_header phdr;

    // create the header we will use to match the variable
    if (ginfo != nullptr) {
        phdr.type = ginfo->type;
    } else {
        phdr.type = info->type;
    }
    set_key(phdr, info->key);
    phdr.group_element = group_element;

    // scan EEPROM to find the right location
    uint16_t ofs;
    if (!scan(&phdr, &ofs)) {
        // if the value isn't stored in EEPROM then set the default value
        ptrdiff_t base;
        if (!get_base(*info, base)) {
            return false;
        }

        if (ginfo != nullptr) {
            // add in nested group offset
            ptrdiff_t group_offset = 0;
            for (uint8_t i=0; i<group_nesting.level; i++) {
                group_offset += group_nesting.group_ret[i]->offset;
            }
            set_value((enum ap_var_type)phdr.type, (void*)(base + ginfo->offset + group_offset),
                      get_default_value(this, &ginfo->def_value));
        } else {
            set_value((enum ap_var_type)phdr.type, (void*)base, 
                      get_default_value(this, &info->def_value));
        }
        return false;
    }

    if (phdr.type != AP_PARAM_VECTOR3F && idx != 0) {
        // only vector3f can have non-zero idx for now
        return false;
    }

    AP_Param *ap;
    ap = this;
    if (idx != 0) {
        ap = (AP_Param *)((ptrdiff_t)ap) - (idx*sizeof(float));
    }

    // found it
    _storage.read_block(ap, ofs+sizeof(phdr), type_size((enum ap_var_type)phdr.type));
    return true;
}

bool AP_Param::configured_in_storage(void) const
{
    uint32_t group_element = 0;
    const struct GroupInfo *ginfo;
    struct GroupNesting group_nesting {};
    uint8_t idx;
    const struct AP_Param::Info *info = find_var_info(&group_element, ginfo, group_nesting, &idx);
    if (info == nullptr) {
        // we don't have any info on how to load it
        return false;
    }

    struct Param_header phdr;

    // create the header we will use to match the variable
    if (ginfo != nullptr) {
        phdr.type = ginfo->type;
    } else {
        phdr.type = info->type;
    }
    set_key(phdr, info->key);
    phdr.group_element = group_element;

    // scan EEPROM to find the right location
    uint16_t ofs;

    // only vector3f can have non-zero idx for now
    return scan(&phdr, &ofs) && (phdr.type == AP_PARAM_VECTOR3F || idx == 0);
}

bool AP_Param::configured_in_defaults_file(bool &read_only) const
{
    if (num_param_overrides == 0) {
        return false;
    }
    uint32_t group_element = 0;
    const struct GroupInfo *ginfo;
    struct GroupNesting group_nesting {};
    uint8_t idx;
    const struct AP_Param::Info *info = find_var_info(&group_element, ginfo, group_nesting, &idx);
    if (info == nullptr) {
        // we don't have any info on how to load it
        return false;
    }

    for (uint16_t i=0; i<num_param_overrides; i++) {
        if (this == param_overrides[i].object_ptr) {
            read_only = param_overrides[i].read_only;
            return true;
        }
    }

    return false;
}

bool AP_Param::configured(void) const
{
    bool read_only;
    return configured_in_defaults_file(read_only) || configured_in_storage();
}

bool AP_Param::is_read_only(void) const
{
    if (num_read_only == 0) {
        return false;
    }
    bool read_only;
    if (configured_in_defaults_file(read_only)) {
        return read_only;
    }
    return false;
}

// set a AP_Param variable to a specified value
void AP_Param::set_value(enum ap_var_type type, void *ptr, float value)
{
    switch (type) {
    case AP_PARAM_INT8:
        ((AP_Int8 *)ptr)->set(value);
        break;
    case AP_PARAM_INT16:
        ((AP_Int16 *)ptr)->set(value);
        break;
    case AP_PARAM_INT32:
        ((AP_Int32 *)ptr)->set(value);
        break;
    case AP_PARAM_FLOAT:
        ((AP_Float *)ptr)->set(value);
        break;
    default:
        break;
    }
}

// load default values for scalars in a group. This does not recurse
// into other objects. This is a static function that should be called
// in the objects constructor
void AP_Param::setup_object_defaults(const void *object_pointer, const struct GroupInfo *group_info)
{
    ptrdiff_t base = (ptrdiff_t)object_pointer;
    uint8_t type;
    for (uint8_t i=0;
         (type=group_info[i].type) != AP_PARAM_NONE;
         i++) {
        if (type <= AP_PARAM_FLOAT) {
            void *ptr = (void *)(base + group_info[i].offset);
            set_value((enum ap_var_type)type, ptr,
                      get_default_value((const AP_Param *)ptr, &group_info[i].def_value));
        } else if (type == AP_PARAM_VECTOR3F) {
            // Single default for all components
            void *ptr = (void *)(base + group_info[i].offset);
            const float default_val = get_default_value((const AP_Param *)ptr, &group_info[i].def_value);
            ((AP_Vector3f *)ptr)->set(Vector3f{default_val, default_val, default_val});
        }
    }
}

// set a value directly in an object. This should only be used by
// example code, not by mainline vehicle code
bool AP_Param::set_object_value(const void *object_pointer,
                                const struct GroupInfo *group_info,
                                const char *name, float value)
{
    ptrdiff_t base = (ptrdiff_t)object_pointer;
    uint8_t type;
    bool found = false;
    for (uint8_t i=0;
         (type=group_info[i].type) != AP_PARAM_NONE;
         i++) {
        if (strcmp(name, group_info[i].name) == 0 && type <= AP_PARAM_FLOAT) {
            void *ptr = (void *)(base + group_info[i].offset);
            set_value((enum ap_var_type)type, ptr, value);
            // return true here ?
            found = true;
        }
    }
    return found;
}


// load default values for all scalars in a sketch. This does not
// recurse into sub-objects
void AP_Param::setup_sketch_defaults(void)
{
    setup();
    for (uint16_t i=0; i<_num_vars; i++) {
        const auto &info = var_info(i);
        uint8_t type = info.type;
        if (type <= AP_PARAM_FLOAT) {
            ptrdiff_t base;
            if (get_base(info, base)) {
                set_value((enum ap_var_type)type, (void*)base,
                          get_default_value((const AP_Param *)base, &info.def_value));
            }
        }
    }
}


// Load all variables from EEPROM
//
bool AP_Param::load_all()
{
    struct Param_header phdr;
    uint16_t ofs = sizeof(AP_Param::EEPROM_header);

    reload_defaults_file(false);

    if (!registered_save_handler) {
        registered_save_handler = true;
        hal.scheduler->register_io_process(FUNCTOR_BIND((&save_dummy), &AP_Param::save_io_handler, void));
    }
    
    while (ofs < _storage.size()) {
        _storage.read_block(&phdr, ofs, sizeof(phdr));
        if (is_sentinal(phdr)) {
            // we've reached the sentinal
            sentinal_offset = ofs;
            return true;
        }

        const struct AP_Param::Info *info;
        void *ptr;

        info = find_by_header(phdr, &ptr);
        if (info != nullptr) {
            _storage.read_block(ptr, ofs+sizeof(phdr), type_size((enum ap_var_type)phdr.type));
        }

        ofs += type_size((enum ap_var_type)phdr.type) + sizeof(phdr);
    }

    // we didn't find the sentinal
    Debug("no sentinal in load_all");
    return false;
}

/*
 * reload from hal.util defaults file or embedded param region
 * @last_pass: if this is the last pass on defaults - unknown parameters are
 *             ignored but if this is set a warning will be emitted
 */
void AP_Param::reload_defaults_file(bool last_pass)
{
#if AP_PARAM_MAX_EMBEDDED_PARAM > 0
    if (param_defaults_data.length != 0) {
        load_embedded_param_defaults(last_pass);
        return;
    }
#endif

#if HAL_OS_POSIX_IO == 1
    /*
      if the HAL specifies a defaults parameter file then override
      defaults using that file
     */
    const char *default_file = hal.util->get_custom_defaults_file();
    if (default_file) {
        if (load_defaults_file(default_file, last_pass)) {
            printf("Loaded defaults from %s\n", default_file);
        } else {
            AP_HAL::panic("Failed to load defaults from %s\n", default_file);
        }
    }
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL && !defined(HAL_BUILD_AP_PERIPH)
    hal.util->set_cmdline_parameters();
#endif
}


/* 
   Load all variables from EEPROM for a particular object. This is
   required for dynamically loaded objects
 */
void AP_Param::load_object_from_eeprom(const void *object_pointer, const struct GroupInfo *group_info)
{
    struct Param_header phdr;
    uint16_t key;

    if (!find_key_by_pointer(object_pointer, key)) {
        DEV_PRINTF("ERROR: Unable to find param pointer\n");
        return;
    }
    
    for (uint8_t i=0; group_info[i].type != AP_PARAM_NONE; i++) {
        if (group_info[i].type == AP_PARAM_GROUP) {
            ptrdiff_t new_offset = 0;
            if (!adjust_group_offset(key, group_info[i], new_offset)) {
                continue;
            }
            const struct GroupInfo *ginfo = get_group_info(group_info[i]);
            if (ginfo != nullptr) {
                load_object_from_eeprom((void *)(((ptrdiff_t)object_pointer)+new_offset), ginfo);
            }
        }
        uint16_t ofs = sizeof(AP_Param::EEPROM_header);
        while (ofs < _storage.size()) {
            _storage.read_block(&phdr, ofs, sizeof(phdr));
            // note that this is an || not an && for robustness
            // against power off while adding a variable
            if (is_sentinal(phdr)) {
                // we've reached the sentinal
                sentinal_offset = ofs;
                break;
            }
            if (get_key(phdr) == key) {
                const struct AP_Param::Info *info;
                void *ptr;
                
                info = find_by_header(phdr, &ptr);
                if (info != nullptr) {
                    if ((ptrdiff_t)ptr == ((ptrdiff_t)object_pointer)+group_info[i].offset) {
                        _storage.read_block(ptr, ofs+sizeof(phdr), type_size((enum ap_var_type)phdr.type));
                        break;
                    }
                }
            }
            ofs += type_size((enum ap_var_type)phdr.type) + sizeof(phdr);
        }
    }

    // reset cached param counter as we may be loading a dynamic var_info
    invalidate_count();
}


// return the first variable in _var_info
AP_Param *AP_Param::first(ParamToken *token, enum ap_var_type *ptype, float *default_val)
{
    token->key = 0;
    token->group_element = 0;
    token->idx = 0;
    if (_num_vars == 0) {
        return nullptr;
    }
    ptrdiff_t base;
    if (!get_base(var_info(0), base)) {
        // should be impossible, first var needs to be non-pointer
        return nullptr;
    }
    if (ptype != nullptr) {
        *ptype = (enum ap_var_type)var_info(0).type;
    }
#if AP_PARAM_DEFAULTS_ENABLED
    if (default_val != nullptr) {
        *default_val = var_info(0).def_value;
    }
    check_default((AP_Param *)base, default_val);
#endif
    return (AP_Param *)base;
}

/// Returns the next variable in a group, recursing into groups
/// as needed
AP_Param *AP_Param::next_group(const uint16_t vindex, const struct GroupInfo *group_info,
                               bool *found_current,
                               const uint32_t group_base,
                               const uint8_t group_shift,
                               const ptrdiff_t group_offset,
                               ParamToken *token,
                               enum ap_var_type *ptype,
                               bool skip_disabled,
                               float *default_val)
{
    enum ap_var_type type;
    for (uint8_t i=0;
         (type=(enum ap_var_type)group_info[i].type) != AP_PARAM_NONE;
         i++) {
        if (!check_frame_type(group_info[i].flags)) {
            continue;
        }
        if (type == AP_PARAM_GROUP) {
            // a nested group
            const struct GroupInfo *ginfo = get_group_info(group_info[i]);
            if (ginfo == nullptr) {
                continue;
            }
            AP_Param *ap;
            ptrdiff_t new_offset = group_offset;

            if (!adjust_group_offset(vindex, group_info[i], new_offset)) {
                continue;
            }

            ap = next_group(vindex, ginfo, found_current, group_id(group_info, group_base, i, group_shift),
                            group_shift + _group_level_shift, new_offset, token, ptype, skip_disabled, default_val);
            if (ap != nullptr) {
                return ap;
            }
        } else {
            if (*found_current) {
                // got a new one
                token->key = vindex;
                token->group_element = group_id(group_info, group_base, i, group_shift);
                token->idx = 0;
                if (ptype != nullptr) {
                    *ptype = type;
                }
                ptrdiff_t base;
                if (!get_base(var_info(vindex), base)) {
                    continue;
                }

                AP_Param *ret = (AP_Param*)(base + group_info[i].offset + group_offset);

                if (skip_disabled &&
                    _hide_disabled_groups &&
                    group_info[i].type == AP_PARAM_INT8 &&
                    (group_info[i].flags & AP_PARAM_FLAG_ENABLE) &&
                    ((AP_Int8 *)ret)->get() == 0) {
                    token->last_disabled = 1;
                }
#if AP_PARAM_DEFAULTS_ENABLED
                if (default_val != nullptr) {
                    *default_val = group_info[i].def_value;
                }
#endif
                return ret;
            }
            if (group_id(group_info, group_base, i, group_shift) == token->group_element) {
                *found_current = true;
                if (token->last_disabled) {
                    token->last_disabled = 0;
                    return nullptr;
                }
                if (type == AP_PARAM_VECTOR3F && token->idx < 3) {
                    // return the next element of the vector as a
                    // float
                    token->idx++;
                    if (ptype != nullptr) {
                        *ptype = AP_PARAM_FLOAT;
                    }
                    ptrdiff_t base;
                    if (!get_base(var_info(vindex), base)) {
                        continue;
                    }
#if AP_PARAM_DEFAULTS_ENABLED
                    if (default_val != nullptr) {
                        *default_val = group_info[i].def_value;
                    }
#endif
                    ptrdiff_t ofs = base + group_info[i].offset + group_offset;
                    ofs += sizeof(float)*(token->idx - 1u);
                    return (AP_Param *)ofs;
                }
            }
        }
    }
    return nullptr;
}

/// Returns the next variable in _var_info, recursing into groups
/// as needed
AP_Param *AP_Param::next(ParamToken *token, enum ap_var_type *ptype, bool skip_disabled, float *default_val)
{
    uint16_t i = token->key;
    bool found_current = false;
    if (i >= _num_vars) {
        // illegal token
        return nullptr;
    }
    enum ap_var_type type = (enum ap_var_type)var_info(i).type;

    // allow Vector3f to be seen as 3 variables. First as a vector,
    // then as 3 separate floats
    if (type == AP_PARAM_VECTOR3F && token->idx < 3) {
        token->idx++;
        if (ptype != nullptr) {
            *ptype = AP_PARAM_FLOAT;
        }
#if AP_PARAM_DEFAULTS_ENABLED
        if (default_val != nullptr) {
            *default_val = var_info(i).def_value;
        }
#endif
        return (AP_Param *)(((token->idx - 1u)*sizeof(float))+(ptrdiff_t)var_info(i).ptr);
    }

    if (type != AP_PARAM_GROUP) {
        i++;
        found_current = true;
    }
    for (; i<_num_vars; i++) {
        const auto &info = var_info(i);
        if (!check_frame_type(info.flags)) {
            continue;
        }
        type = (enum ap_var_type)info.type;
        if (type == AP_PARAM_GROUP) {
            const struct GroupInfo *group_info = get_group_info(info);
            if (group_info == nullptr) {
                continue;
            }
            AP_Param *ap = next_group(i, group_info, &found_current, 0, 0, 0, token, ptype, skip_disabled, default_val);
            if (ap != nullptr) {
                return ap;
            }
        } else {
            // found the next one
            token->key = i;
            token->group_element = 0;
            token->idx = 0;
            if (ptype != nullptr) {
                *ptype = type;
            }
#if AP_PARAM_DEFAULTS_ENABLED
            if (default_val != nullptr) {
                *default_val = info.def_value;
            }
#endif
            return (AP_Param *)(info.ptr);
        }
    }
    return nullptr;
}

/// Returns the next scalar in _var_info, recursing into groups
/// as needed
AP_Param *AP_Param::next_scalar(ParamToken *token, enum ap_var_type *ptype, float *default_val)
{
    AP_Param *ap;
    enum ap_var_type type;
    while ((ap = next(token, &type, true, default_val)) != nullptr && type > AP_PARAM_FLOAT) ;

    if (ap != nullptr) {
        if (ptype != nullptr) {
            *ptype = type;
        }
    }
#if AP_PARAM_DEFAULTS_ENABLED
    check_default(ap, default_val);
#endif
    return ap;
}


/// cast a variable to a float given its type
float AP_Param::cast_to_float(enum ap_var_type type) const
{
    switch (type) {
    case AP_PARAM_INT8:
        return ((AP_Int8 *)this)->cast_to_float();
    case AP_PARAM_INT16:
        return ((AP_Int16 *)this)->cast_to_float();
    case AP_PARAM_INT32:
        return ((AP_Int32 *)this)->cast_to_float();
    case AP_PARAM_FLOAT:
        return ((AP_Float *)this)->cast_to_float();
    default:
        return NAN;
    }
}

/*
  find an old parameter and return it.
 */
bool AP_Param::find_old_parameter(const struct ConversionInfo *info, AP_Param *value)
{
    // find the old value in EEPROM.
    uint16_t pofs;
    AP_Param::Param_header header;
    header.type = info->type;
    set_key(header, info->old_key);
    header.group_element = info->old_group_element;
    if (!scan(&header, &pofs)) {
        // the old parameter isn't saved in the EEPROM.
        return false;
    }

    // load the old value from EEPROM
    _storage.read_block(value, pofs+sizeof(header), type_size((enum ap_var_type)header.type));
    return true;
}


#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wformat"
// convert one old vehicle parameter to new object parameter
void AP_Param::convert_old_parameter(const struct ConversionInfo *info, float scaler, uint8_t flags)
{
    uint8_t old_value[type_size(info->type)];
    AP_Param *ap = (AP_Param *)&old_value[0];

    if (!find_old_parameter(info, ap)) {
        // the old parameter isn't saved in the EEPROM. It was
        // probably still set to the default value, which isn't stored
        // no need to convert
        return;
    }

    // find the new variable in the variable structures
    enum ap_var_type ptype;
    AP_Param *ap2;
    ap2 = find(&info->new_name[0], &ptype);
    if (ap2 == nullptr) {
        DEV_PRINTF("Unknown conversion '%s'\n", info->new_name);
        return;
    }

    // see if we can load it from EEPROM
    if (!(flags & CONVERT_FLAG_FORCE) && ap2->configured_in_storage()) {
        // the new parameter already has a value set by the user, or
        // has already been converted
        return;
    }

    // see if they are the same type and no scaling applied
    if (ptype == info->type && is_equal(scaler, 1.0f) && flags == 0) {
        // copy the value over only if the new parameter does not already
        // have the old value (via a default).
        if (memcmp(ap2, ap, sizeof(old_value)) != 0) {
            memcpy(ap2, ap, sizeof(old_value));
            // and save
            ap2->save();
        }
    } else if (ptype <= AP_PARAM_FLOAT && info->type <= AP_PARAM_FLOAT) {
        // perform scalar->scalar conversion
        float v = ap->cast_to_float(info->type);
        if (flags & CONVERT_FLAG_REVERSE) {
            // convert a _REV parameter to a _REVERSED parameter
            v = is_equal(v, -1.0f)?1:0;
        }
        if (!is_equal(v,ap2->cast_to_float(ptype))) {
            // the value needs to change
            set_value(ptype, ap2, v * scaler);
            ap2->save();
        }
    } else {
        // can't do vector<->scalar conversion, or different vector types
        DEV_PRINTF("Bad conversion type '%s'\n", info->new_name);
    }
}
#pragma GCC diagnostic pop


// convert old vehicle parameters to new object parametersv
void AP_Param::convert_old_parameters(const struct ConversionInfo *conversion_table, uint8_t table_size, uint8_t flags)
{
    for (uint8_t i=0; i<table_size; i++) {
        convert_old_parameter(&conversion_table[i], 1.0f, flags);
    }
    // we need to flush here to prevent a later set_default_by_name()
    // causing a save to be done on a converted parameter
    flush();
}

// move all parameters from a class to a new location
// is_top_level: Is true if the class had its own top level key, param_key. It is false if the class was a subgroup
void AP_Param::convert_class(uint16_t param_key, void *object_pointer,
                                    const struct AP_Param::GroupInfo *group_info,
                                    uint16_t old_index, uint16_t old_top_element, bool is_top_level)
{
    const uint8_t group_shift = is_top_level ? 0 : 6;

    for (uint8_t i=0; group_info[i].type != AP_PARAM_NONE; i++) {
        struct ConversionInfo info;
        info.old_key = param_key;
        info.type = (ap_var_type)group_info[i].type;
        info.new_name = nullptr;

        uint16_t idx = group_info[i].idx;
        if (group_shift != 0 && idx == 0) {
            // Note: Index 0 is treated as 63 for group bit shifting purposes. See group_id()
            idx = 63;
        }

        info.old_group_element = (idx << group_shift) + old_index;

        uint8_t old_value[type_size(info.type)];
        AP_Param *ap = (AP_Param *)&old_value[0];
        
        if (!AP_Param::find_old_parameter(&info, ap)) {
            // the parameter wasn't set in the old eeprom
            continue;
        }

        AP_Param *ap2 = (AP_Param *)(group_info[i].offset + (uint8_t *)object_pointer);
        if (ap2->configured_in_storage()) {
            // user has already set a value, or previous conversion was done
            continue;
        }
        memcpy(ap2, ap, sizeof(old_value));
        // and save
        ap2->save();
    }

    // we need to flush here to prevent a later set_default_by_name()
    // causing a save to be done on a converted parameter
    flush();
}

/*
 convert width of a parameter, allowing update to wider scalar values
 without changing the parameter indexes
*/
bool AP_Param::convert_parameter_width(ap_var_type old_ptype)
{
    if (configured_in_storage()) {
        // already converted or set by the user
        return false;
    }

    uint32_t group_element = 0;
    const struct GroupInfo *ginfo;
    struct GroupNesting group_nesting {};
    uint8_t idx;
    const struct AP_Param::Info *info = find_var_info(&group_element, ginfo, group_nesting, &idx);

    if (info == nullptr) {
        return false;
    }

    // remember the type
    ap_var_type new_ptype;
    if (ginfo != nullptr) {
        new_ptype = (ap_var_type)ginfo->type;
    } else {
        new_ptype = (ap_var_type)info->type;
    }
    
    // create the header we will use to scan for the variable
    struct Param_header phdr;
    phdr.type = old_ptype;
    set_key(phdr, info->key);
    phdr.group_element = group_element;

    // scan EEPROM to find the right location
    uint16_t pofs;
    if (!scan(&phdr, &pofs)) {
        // it isn't in storage
        return false;
    }

    // load the old value from EEPROM
    uint8_t old_value[type_size(old_ptype)];
    _storage.read_block(old_value, pofs+sizeof(phdr), sizeof(old_value));
    
    AP_Param *old_ap = (AP_Param *)&old_value[0];

    // going via float is safe as the only time we would be converting
    // from AP_Int32 is when converting to float
    float old_float_value = old_ap->cast_to_float(old_ptype);
    set_value(new_ptype, this, old_float_value);

    // force save as the new type
    save(true);

    return true;
}


/*
  set a parameter to a float value
 */
void AP_Param::set_float(float value, enum ap_var_type var_type)
{
    if (isnan(value) || isinf(value)) {
        return;
    }

    // add a small amount before casting parameter values
    // from float to integer to avoid truncating to the
    // next lower integer value.
    float rounding_addition = 0.01f;
        
    // handle variables with standard type IDs
    if (var_type == AP_PARAM_FLOAT) {
        ((AP_Float *)this)->set(value);
    } else if (var_type == AP_PARAM_INT32) {
        if (value < 0) rounding_addition = -rounding_addition;
        float v = value+rounding_addition;
        v = constrain_float(v, INT32_MIN, INT32_MAX);
        ((AP_Int32 *)this)->set(v);
    } else if (var_type == AP_PARAM_INT16) {
        if (value < 0) rounding_addition = -rounding_addition;
        float v = value+rounding_addition;
        v = constrain_float(v, INT16_MIN, INT16_MAX);
        ((AP_Int16 *)this)->set(v);
    } else if (var_type == AP_PARAM_INT8) {
        if (value < 0) rounding_addition = -rounding_addition;
        float v = value+rounding_addition;
        v = constrain_float(v, INT8_MIN, INT8_MAX);
        ((AP_Int8 *)this)->set(v);
    }
}


/*
  parse a parameter file line
 */
bool AP_Param::parse_param_line(char *line, char **vname, float &value, bool &read_only)
{
    if (line[0] == '#') {
        return false;
    }
    char *saveptr = nullptr;
    /*
      note that we need the \r\n as delimiters to prevent us getting
      strings with line termination in the results
     */
    char *pname = strtok_r(line, ", =\t\r\n", &saveptr);
    if (pname == nullptr) {
        return false;
    }
    if (strlen(pname) > AP_MAX_NAME_SIZE) {
        return false;
    }

#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
    // Workaround to prevent FORMAT_VERSION in param file resulting in invalid
    // EEPROM. For details, see: https://github.com/ArduPilot/ardupilot/issues/15579
    if (strcmp(pname, "FORMAT_VERSION") == 0) {
        ::printf("Warning: Ignoring FORMAT_VERSION in param file\n");
        return false;
    }
#endif

    const char *value_s = strtok_r(nullptr, ", =\t\r\n", &saveptr);
    if (value_s == nullptr) {
        return false;
    }
    value = strtof(value_s, NULL);
    *vname = pname;

    const char *flags_s = strtok_r(nullptr, ", =\t\r\n", &saveptr);
    if (flags_s && strcmp(flags_s, "@READONLY") == 0) {
        read_only = true;
    } else {
        read_only = false;
    }

    return true;
}


#if HAVE_FILESYSTEM_SUPPORT

// increments num_defaults for each default found in filename
bool AP_Param::count_defaults_in_file(const char *filename, uint16_t &num_defaults)
{
    // try opening the file both in the posix filesystem and using AP::FS
    int file_apfs = AP::FS().open(filename, O_RDONLY, true);
    if (file_apfs == -1) {
        return false;
    }
    char line[100];

    /*
      work out how many parameter default structures to allocate
     */
    while (AP::FS().fgets(line, sizeof(line)-1, file_apfs)) {
        char *pname;
        float value;
        bool read_only;
        if (!parse_param_line(line, &pname, value, read_only)) {
            continue;
        }
        enum ap_var_type var_type;
        if (!find(pname, &var_type)) {
            continue;
        }
        num_defaults++;
    }
    AP::FS().close(file_apfs);

    return true;
}

bool AP_Param::read_param_defaults_file(const char *filename, bool last_pass)
{
    // try opening the file both in the posix filesystem and using AP::FS
    int file_apfs = AP::FS().open(filename, O_RDONLY, true);
    if (file_apfs == -1) {
        AP_HAL::panic("AP_Param: Failed to re-open defaults file");
        return false;
    }

    uint16_t idx = 0;
    char line[100];
    while (AP::FS().fgets(line, sizeof(line)-1, file_apfs)) {
        char *pname;
        float value;
        bool read_only;
        if (!parse_param_line(line, &pname, value, read_only)) {
            continue;
        }
        enum ap_var_type var_type;
        AP_Param *vp = find(pname, &var_type);
        if (!vp) {
            if (last_pass) {
#if ENABLE_DEBUG
                ::printf("Ignored unknown param %s in defaults file %s\n",
                         pname, filename);
                hal.console->printf(
                         "Ignored unknown param %s in defaults file %s\n",
                         pname, filename);
#endif
            }
            continue;
        }
        param_overrides[idx].object_ptr = vp;
        param_overrides[idx].value = value;
        param_overrides[idx].read_only = read_only;
        if (read_only) {
            num_read_only++;
        }
        idx++;
        if (!vp->configured_in_storage()) {
            vp->set_float(value, var_type);
        }
    }
    AP::FS().close(file_apfs);

    return true;
}

/*
  load a default set of parameters from a file
 */
bool AP_Param::load_defaults_file(const char *filename, bool last_pass)
{
    if (filename == nullptr) {
        return false;
    }

    char *mutable_filename = strdup(filename);
    if (mutable_filename == nullptr) {
        AP_HAL::panic("AP_Param: Failed to allocate mutable string");
    }

    uint16_t num_defaults = 0;
    char *saveptr = nullptr;
    for (char *pname = strtok_r(mutable_filename, ",", &saveptr);
         pname != nullptr;
         pname = strtok_r(nullptr, ",", &saveptr)) {
        if (!count_defaults_in_file(pname, num_defaults)) {
            free(mutable_filename);
            return false;
        }
    }
    free(mutable_filename);

    delete[] param_overrides;
    num_param_overrides = 0;
    num_read_only = 0;

    param_overrides = new param_override[num_defaults];
    if (param_overrides == nullptr) {
        AP_HAL::panic("AP_Param: Failed to allocate overrides");
        return false;
    }

    if (num_defaults == 0) {
        return true;
    }

    saveptr = nullptr;
    mutable_filename = strdup(filename);
    if (mutable_filename == nullptr) {
        AP_HAL::panic("AP_Param: Failed to allocate mutable string");
    }
    for (char *pname = strtok_r(mutable_filename, ",", &saveptr);
         pname != nullptr;
         pname = strtok_r(nullptr, ",", &saveptr)) {
        if (!read_param_defaults_file(pname, last_pass)) {
            free(mutable_filename);
            return false;
        }
    }
    free(mutable_filename);

    num_param_overrides = num_defaults;

    return true;
}

#endif // HAVE_FILESYSTEM_SUPPORT

#if AP_PARAM_MAX_EMBEDDED_PARAM > 0
/*
  count the number of embedded parameter defaults
 */
bool AP_Param::count_embedded_param_defaults(uint16_t &count)
{
    const volatile char *ptr = param_defaults_data.data;
    uint16_t length = param_defaults_data.length;
    count = 0;
    
    while (length) {
        char line[100];
        char *pname;
        float value;
        bool read_only;
        uint16_t i;
        uint16_t n = MIN(length, sizeof(line)-1);
        for (i=0;i<n;i++) {
            if (ptr[i] == '\n') {
                break;
            }
        }
        if (i == n) {
            // no newline
            break;
        }
        
        memcpy(line, (void *)ptr, i);
        line[i] = 0;

        length -= i+1;
        ptr += i+1;
        
        if (line[0] == '#' || line[0] == 0) {
            continue;
        }

        if (!parse_param_line(line, &pname, value, read_only)) {
            continue;
        }

        enum ap_var_type var_type;
        if (!find(pname, &var_type)) {
            continue;
        }

        count++;
    }
    return true;
}

/*
 * load a default set of parameters from a embedded parameter region
 * @last_pass: if this is the last pass on defaults - unknown parameters are
 *             ignored but if this is set a warning will be emitted
 */
void AP_Param::load_embedded_param_defaults(bool last_pass)
{
    delete[] param_overrides;
    param_overrides = nullptr;
    num_param_overrides = 0;
    num_read_only = 0;

    uint16_t num_defaults = 0;
    if (!count_embedded_param_defaults(num_defaults)) {
        return;
    }

    param_overrides = new param_override[num_defaults];
    if (param_overrides == nullptr) {
        AP_HAL::panic("AP_Param: Failed to allocate overrides");
        return;
    }

    const volatile char *ptr = param_defaults_data.data;
    uint16_t length = param_defaults_data.length;
    uint16_t idx = 0;
    
    while (idx < num_defaults && length) {
        char line[100];
        char *pname;
        float value;
        bool read_only;
        uint16_t i;
        uint16_t n = MIN(length, sizeof(line)-1);
        for (i=0;i<n;i++) {
            if (ptr[i] == '\n') {
                break;
            }
        }
        if (i == n) {
            // no newline
            break;
        }
        memcpy(line, (void*)ptr, i);
        line[i] = 0;
        
        length -= i+1;
        ptr += i+1;

        if (line[0] == '#' || line[0] == 0) {
            continue;
        }
        
        if (!parse_param_line(line, &pname, value, read_only)) {
            continue;
        }
        enum ap_var_type var_type;
        AP_Param *vp = find(pname, &var_type);
        if (!vp) {
            if (last_pass) {
#if ENABLE_DEBUG
                ::printf("Ignored unknown param %s from embedded region (offset=%u)\n",
                         pname, unsigned(ptr - param_defaults_data.data));
                hal.console->printf(
                         "Ignored unknown param %s from embedded region (offset=%u)\n",
                         pname, unsigned(ptr - param_defaults_data.data));
#endif
            }
            continue;
        }
        param_overrides[idx].object_ptr = vp;
        param_overrides[idx].value = value;
        param_overrides[idx].read_only = read_only;
        if (read_only) {
            num_read_only++;
        }
        idx++;
#if AP_PARAM_DEFAULTS_ENABLED
        add_default(vp, value, embedded_default_list);
#endif
        if (!vp->configured_in_storage()) {
            vp->set_float(value, var_type);
        }
    }
    num_param_overrides = num_defaults;
}
#endif // AP_PARAM_MAX_EMBEDDED_PARAM > 0

/* 
   find a default value given a pointer to a default value in flash
 */
float AP_Param::get_default_value(const AP_Param *vp, const float *def_value_ptr)
{
    for (uint16_t i=0; i<num_param_overrides; i++) {
        if (vp == param_overrides[i].object_ptr) {
            return param_overrides[i].value;
        }
    }
    return *def_value_ptr;
}


void AP_Param::send_parameter(const char *name, enum ap_var_type var_type, uint8_t idx) const
{
    if (idx != 0 && var_type == AP_PARAM_VECTOR3F) {
        var_type = AP_PARAM_FLOAT;
    }
    if (var_type > AP_PARAM_VECTOR3F) {
        // invalid
        return;
    }
    if (var_type != AP_PARAM_VECTOR3F) {
        // nice and simple for scalar types
        GCS_SEND_PARAM(name, var_type, cast_to_float(var_type));
        return;
    }

    // for vectors we need to send 3 messages. Note that we also come here for the case
    // of a set of the first element of a AP_Vector3f. This happens as the ap->save() call can't
    // distinguish between a vector and scalar save. It means that setting first element of a vector
    // via MAVLink results in sending all 3 elements to the GCS
#if HAL_GCS_ENABLED
    const Vector3f &v = ((AP_Vector3f *)this)->get();
    char name2[AP_MAX_NAME_SIZE+1];
    strncpy(name2, name, AP_MAX_NAME_SIZE);
    name2[AP_MAX_NAME_SIZE] = 0;
    char &name_axis = name2[strlen(name)-1];
    
    name_axis = 'X';
    GCS_SEND_PARAM(name2, AP_PARAM_FLOAT, v.x);
    name_axis = 'Y';
    GCS_SEND_PARAM(name2, AP_PARAM_FLOAT, v.y);
    name_axis = 'Z';
    GCS_SEND_PARAM(name2, AP_PARAM_FLOAT, v.z);
#endif // HAL_GCS_ENABLED
}

/*
  return count of all scalar parameters.
  Note that this function may be called from the IO thread, so needs
  to be thread safe
 */
uint16_t AP_Param::count_parameters(void)
{
    // if we haven't cached the parameter count yet...
    WITH_SEMAPHORE(_count_sem);
    if (_parameter_count != 0 &&
        _count_marker == _count_marker_done) {
        return _parameter_count;
    }
    /*
      cope with another thread invalidating the count while we are
      counting
     */
    uint8_t limit = 4;
    while ((_parameter_count == 0 ||
            _count_marker != _count_marker_done) &&
           limit--) {
        AP_Param  *vp;
        AP_Param::ParamToken token {};
        uint16_t count = 0;
        uint16_t marker = _count_marker;

        for (vp = AP_Param::first(&token, nullptr);
             vp != nullptr;
             vp = AP_Param::next_scalar(&token, nullptr)) {
            count++;
        }
        _parameter_count = count;
        _count_marker_done = marker;
    }
    return _parameter_count;
}

/*
  invalidate parameter count cache
 */
void AP_Param::invalidate_count(void)
{
    // we don't take the semaphore here as we don't want to block. The
    // not-equal test is strong enough to ensure we get the right
    // answer
    _count_marker++;
}

/*
  set a default value by name
 */
bool AP_Param::set_default_by_name(const char *name, float value)
{
    enum ap_var_type vtype;
    AP_Param *vp = find(name, &vtype);
    if (vp == nullptr) {
        return false;
    }
    switch (vtype) {
    case AP_PARAM_INT8:
        ((AP_Int8 *)vp)->set_default(value);
        return true;
    case AP_PARAM_INT16:
        ((AP_Int16 *)vp)->set_default(value);
        return true;
    case AP_PARAM_INT32:
        ((AP_Int32 *)vp)->set_default(value);
        return true;
    case AP_PARAM_FLOAT:
        ((AP_Float *)vp)->set_default(value);
        return true;
    default:
        break;
    }
    // not a supported type
    return false;
}

/*
  set parameter defaults from a defaults_struct table
  sends GCS message and panics (in SITL only) if parameter is not found
 */
void AP_Param::set_defaults_from_table(const struct defaults_table_struct *table, uint8_t count)
{
    for (uint8_t i=0; i<count; i++) {
        if (!AP_Param::set_default_by_name(table[i].name, table[i].value)) {
            AP_BoardConfig::config_error("param deflt fail:%s", table[i].name);
        }
    }
}

/*
  set a value by name
 */
bool AP_Param::set_by_name(const char *name, float value)
{
    enum ap_var_type vtype;
    AP_Param *vp = find(name, &vtype);
    if (vp == nullptr) {
        return false;
    }
    switch (vtype) {
    case AP_PARAM_INT8:
        ((AP_Int8 *)vp)->set(value);
        return true;
    case AP_PARAM_INT16:
        ((AP_Int16 *)vp)->set(value);
        return true;
    case AP_PARAM_INT32:
        ((AP_Int32 *)vp)->set(value);
        return true;
    case AP_PARAM_FLOAT:
        ((AP_Float *)vp)->set(value);
        return true;
    default:
        break;
    }
    // not a supported type
    return false;
}

/*
  get a value by name
 */
bool AP_Param::get(const char *name, float &value)
{
    enum ap_var_type vtype;
    AP_Param *vp = find(name, &vtype);
    if (vp == nullptr) {
        return false;
    }
    switch (vtype) {
    case AP_PARAM_INT8:
        value = ((AP_Int8 *)vp)->get();
        break;
    case AP_PARAM_INT16:
        value = ((AP_Int16 *)vp)->get();
        break;

    case AP_PARAM_INT32:
        value = ((AP_Int32 *)vp)->get();
        break;

    case AP_PARAM_FLOAT:
        value = ((AP_Float *)vp)->get();
        break;

    default:
        // not a supported type
        return false;
    }
    return true;
}

/*
  set and save a value by name
 */
bool AP_Param::set_and_save_by_name(const char *name, float value)
{
    enum ap_var_type vtype;
    AP_Param *vp = find(name, &vtype);
    if (vp == nullptr) {
        return false;
    }
    switch (vtype) {
    case AP_PARAM_INT8:
        ((AP_Int8 *)vp)->set_and_save(value);
        return true;
    case AP_PARAM_INT16:
        ((AP_Int16 *)vp)->set_and_save(value);
        return true;
    case AP_PARAM_INT32:
        ((AP_Int32 *)vp)->set_and_save(value);
        return true;
    case AP_PARAM_FLOAT:
        ((AP_Float *)vp)->set_and_save(value);
        return true;
    default:
        break;
    }
    // not a supported type
    return false;
}

/*
  set and save a value by name
 */
bool AP_Param::set_and_save_by_name_ifchanged(const char *name, float value)
{
    enum ap_var_type vtype;
    AP_Param *vp = find(name, &vtype);
    if (vp == nullptr) {
        return false;
    }
    switch (vtype) {
    case AP_PARAM_INT8:
        ((AP_Int8 *)vp)->set_and_save_ifchanged(value);
        return true;
    case AP_PARAM_INT16:
        ((AP_Int16 *)vp)->set_and_save_ifchanged(value);
        return true;
    case AP_PARAM_INT32:
        ((AP_Int32 *)vp)->set_and_save_ifchanged(value);
        return true;
    case AP_PARAM_FLOAT:
        ((AP_Float *)vp)->set_and_save_ifchanged(value);
        return true;
    default:
        break;
    }
    // not a supported type
    return false;
}

#if AP_PARAM_DEFAULTS_ENABLED
void AP_Param::check_default(AP_Param *ap, float *default_value)
{
    if (default_value == nullptr || ap == nullptr) {
        return;
    }
#if AP_PARAM_MAX_EMBEDDED_PARAM > 0
    // Check embedded list first
    if (embedded_default_list != nullptr) {
        for (defaults_list *item = embedded_default_list; item; item = item->next) {
            if (item->ap == ap) {
                *default_value = item->val;
                return;
            }
        }
    }
#endif
    if (default_list != nullptr) {
        for (defaults_list *item = default_list; item; item = item->next) {
            if (item->ap == ap) {
                *default_value = item->val;
                return;
            }
        }
    }
}

void AP_Param::add_default(AP_Param *ap, float v, defaults_list*& list)
{
    if (list != nullptr) {
        // check is param is already in list
        for (defaults_list *item = list; item; item = item->next) {
            // update existing entry
            if (item->ap == ap) {
                item->val = v;
                return;
            }
        }
    }

    // add to list
    defaults_list *new_item = new defaults_list;
    if (new_item == nullptr) {
        return;
    }
    new_item->ap = ap;
    new_item->val = v;
    new_item->next = list;
    list = new_item;
}
#endif // AP_PARAM_DEFAULTS_ENABLED


#if AP_PARAM_KEY_DUMP
/*
  do not remove this show_all() code, it is essential for debugging
  and creating conversion tables
 */

// print the value of all variables
void AP_Param::show(const AP_Param *ap, const char *s,
                    enum ap_var_type type, AP_HAL::BetterStream *port)
{
    switch (type) {
    case AP_PARAM_INT8:
        ::printf("%s: %d\n", s, (int)((AP_Int8 *)ap)->get());
        break;
    case AP_PARAM_INT16:
        ::printf("%s: %d\n", s, (int)((AP_Int16 *)ap)->get());
        break;
    case AP_PARAM_INT32:
        ::printf("%s: %ld\n", s, (long)((AP_Int32 *)ap)->get());
        break;
    case AP_PARAM_FLOAT:
        ::printf("%s: %f\n", s, (double)((AP_Float *)ap)->get());
        break;
    default:
        break;
    }
}

// print the value of all variables
void AP_Param::show(const AP_Param *ap, const ParamToken &token,
                    enum ap_var_type type, AP_HAL::BetterStream *port)
{
    char s[AP_MAX_NAME_SIZE+1];
    ap->copy_name_token(token, s, sizeof(s), true);
    s[AP_MAX_NAME_SIZE] = 0;
    show(ap, s, type, port);
}

// print the value of all variables
void AP_Param::show_all(AP_HAL::BetterStream *port, bool showKeyValues)
{
    ParamToken token;
    AP_Param *ap;
    enum ap_var_type type;
    float default_value = nanf("0x4152");  // from logger quiet_nanf

    for (ap=AP_Param::first(&token, &type, &default_value);
         ap;
         ap=AP_Param::next_scalar(&token, &type, &default_value)) {
        if (showKeyValues) {
            ::printf("Key %u: Index %u: GroupElement %u : Default %f  :", (unsigned)var_info(token.key).key, (unsigned)token.idx, (unsigned)token.group_element, default_value);
            default_value = nanf("0x4152");
        }
        show(ap, token, type, port);
        hal.scheduler->delay(1);
    }
}
#endif // AP_PARAM_KEY_DUMP


#if AP_PARAM_DYNAMIC_ENABLED
/*
  allow for dynamically added parameter tables from scripts

  The layout we create is as follows:
    - a top level Info with the given prefix, using one of the 10 possible slots in _var_info_dynamic
    - a dynamically allocated GroupInfo table, never freed, of size (num_params+2)
    - the GroupInfo table has an initial AP_Int32 hidden entry with a 32 bit CRC of the prefix
    - the last GroupInfo is a footer
*/
bool AP_Param::add_table(uint8_t _key, const char *prefix, uint8_t num_params)
{
    // check if the key already exists. We only check base params to allow
    // for scripting reload without a conflict
    uint16_t key = uint16_t(_key) + AP_PARAM_DYNAMIC_KEY_BASE;
    for (uint16_t i=0; i<_num_vars_base; i++) {
        if (var_info(i).key == key) {
            return false;
        }
    }
    if (num_params > 63) {
        return false;
    }

    // we use a crc of the prefix to ensure the table key isn't re-used
    const int32_t crc = int32_t(crc32_small(0, (const uint8_t *)prefix, strlen(prefix)));
    int32_t current_crc;
    if (load_int32(key, 0, current_crc) && current_crc != crc) {
        // crc mismatch, we have a conflict with an existing use of this key
        return false;
    }

    // create the dynamic table if needed. This is never freed
    if (_var_info_dynamic == nullptr) {
        _var_info_dynamic = (Info *)calloc(AP_PARAM_MAX_DYNAMIC, sizeof(struct Info));
        if (_var_info_dynamic == nullptr) {
            return false;
        }
        for (uint8_t i=0; i<AP_PARAM_MAX_DYNAMIC; i++) {
            auto &info = _var_info_dynamic[i];
            info.type = AP_PARAM_NONE;
            info.name = _empty_string;
            info.key = 0xFFFF;
            info.ptr = nullptr;
            info.group_info = nullptr;
            info.flags = 0;
        }
        // make tables available
        _num_vars += AP_PARAM_MAX_DYNAMIC;
    }

    // find existing key (allows for script reload)
    uint8_t i;
    for (i=0; i<AP_PARAM_MAX_DYNAMIC; i++) {
        auto &info = _var_info_dynamic[i];
        if (info.type != AP_PARAM_NONE && info.key == key) {
            if (_dynamic_table_sizes[i] != 0 &&
                num_params > _dynamic_table_sizes[i]) {
                // can't expand the table at runtime
                return false;
            }
            if (strcmp(prefix, info.name) != 0) {
                // prefix has changed, reject as two scripts running
                // with the same key
                return false;
            }
            break;
        }
    }

    if (i == AP_PARAM_MAX_DYNAMIC) {
        // find an unused slot
        for (i=0; i<AP_PARAM_MAX_DYNAMIC; i++) {
            auto &info = _var_info_dynamic[i];
            if (info.type == AP_PARAM_NONE ) {
                break;
            }
        }
    }

    if (i == AP_PARAM_MAX_DYNAMIC) {
        // no empty slots
        return false;
    }

    auto &info = _var_info_dynamic[i];

    // create memory for the array of floats if needed
    // first float is used for the crc
    if (info.ptr == nullptr) {
        info.ptr = calloc(num_params+1, sizeof(float));
        if (info.ptr == nullptr) {
            return false;
        }
    }

    // allocate the name
    if (info.name == _empty_string) {
        info.name = strdup(prefix);
        if (info.name == nullptr) {
            free(const_cast<void*>(info.ptr));
            info.ptr = nullptr;
            info.name = _empty_string;
            return false;
        }
    }

    // if it doesn't exist then create the table
    if (info.group_info == nullptr) {
        info.group_info = (GroupInfo *)calloc(num_params+2, sizeof(GroupInfo));
        if (info.group_info == nullptr) {
            free(const_cast<void*>(info.ptr));
            free(const_cast<char*>(info.name));
            info.ptr = nullptr;
            info.name = _empty_string;
            return false;
        }
        // fill in footer for all entries
        for (uint8_t gi=1; gi<num_params+2; gi++) {
            auto &ginfo = const_cast<GroupInfo*>(info.group_info)[gi];
            ginfo.name = _empty_string;
            ginfo.idx = 0xff;
        }
        // hidden first parameter containing AP_Int32 crc
        auto &hinfo = const_cast<GroupInfo*>(info.group_info)[0];
        hinfo.flags = AP_PARAM_FLAG_HIDDEN;
        hinfo.name = _empty_string;
        hinfo.idx = 0;
        hinfo.offset = 0;
        hinfo.type = AP_PARAM_INT32;
        // fill in default value with the CRC. Relies on sizeof crc == sizeof float
        memcpy((uint8_t *)&hinfo.def_value, (const uint8_t *)&crc, sizeof(crc));
    }

    // remember the table size
    if (_dynamic_table_sizes[i] == 0) {
        _dynamic_table_sizes[i] = num_params;
    }
    
    // make the group active
    info.key = key;
    info.type = AP_PARAM_GROUP;

    invalidate_count();

    // save the CRC
    AP_Int32 *crc_param = const_cast<AP_Int32 *>((AP_Int32 *)info.ptr);
    crc_param->set(crc);
    crc_param->save(true);

    return true;
}

/*
 Load an AP_Int32 variable from EEPROM using top level key and group element. Used to confirm
 a key in add_table()
*/
bool AP_Param::load_int32(uint16_t key, uint32_t group_element, int32_t &value)
{
    struct Param_header phdr;

    phdr.type = AP_PARAM_INT32;
    set_key(phdr, key);
    phdr.group_element = group_element;

    // scan EEPROM to find the right location
    uint16_t ofs;
    if (!scan(&phdr, &ofs)) {
        return false;
    }

    // found it
    _storage.read_block(&value, ofs+sizeof(phdr), type_size(AP_PARAM_INT32));
    return true;
}

/*
  add a parameter to a dynamic table
 */
bool AP_Param::add_param(uint8_t _key, uint8_t param_num, const char *pname, float default_value)
{
    // check for valid values
    if (param_num == 0 || param_num > 63 || strlen(pname) > 16) {
        return false;
    }

    uint16_t key = uint16_t(_key) + AP_PARAM_DYNAMIC_KEY_BASE;
    // find the info
    uint8_t i;
    for (i=0; i<AP_PARAM_MAX_DYNAMIC; i++) {
        auto &info = _var_info_dynamic[i];
        if (info.key == key) {
            break;
        }
    }
    if (i == AP_PARAM_MAX_DYNAMIC) {
        // not found
        return false;
    }

    if (param_num > _dynamic_table_sizes[i]) {
        return false;
    }

    auto &info = _var_info_dynamic[i];
    if (info.ptr == nullptr) {
        return false;
    }

    // check CRC
    auto &hinfo = const_cast<GroupInfo*>(info.group_info)[0];
    const int32_t crc = *(const int32_t *)(&hinfo.def_value);

    int32_t current_crc;
    if (load_int32(key, 0, current_crc) && current_crc != crc) {
        // crc mismatch, we have a conflict with an existing use of this key
        return false;
    }

    auto &ginfo = const_cast<GroupInfo*>(info.group_info)[param_num];

    if (ginfo.name == _empty_string) {
        // we don't allow name change while running
        ginfo.name = strdup(pname);
        if (ginfo.name == nullptr) {
            ginfo.name = _empty_string;
            return false;
        }
    }
    ginfo.offset = param_num*sizeof(float);
    ginfo.idx = param_num;
    float *def_value = const_cast<float *>(&ginfo.def_value);
    *def_value = default_value;
    ginfo.type = AP_PARAM_FLOAT;

    invalidate_count();

    // load from storage if available
    AP_Float *pvalues = const_cast<AP_Float *>((const AP_Float *)info.ptr);
    AP_Float &p = pvalues[param_num];
    p.set_default(default_value);
    p.load();

    return true;
}
#endif