// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
   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/>.
 */

#include <AP_HAL.h>
#include <AP_Common.h>
#include <AP_Math.h>
#include <GCS_MAVLink.h>
#include <GCS.h>
#include "AP_Terrain.h"

#if HAVE_AP_TERRAIN

#include <assert.h>
#include <stdio.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <errno.h>

#define TERRAIN_DEBUG 0

extern const AP_HAL::HAL& hal;

// table of user settable parameters
const AP_Param::GroupInfo AP_Terrain::var_info[] PROGMEM = {
    // @Param: ENABLE
    // @DisplayName: Terrain following enable
    // @Description: enable terrain following
    // @Values: 0:Disable,1:Enable
    AP_GROUPINFO("ENABLE",    0, AP_Terrain, enable, 0),

    // @Param: SPACING
    // @DisplayName: Terrain grid spacing
    // @Description: distance between terrain grid points in meters
    // @Units: meters
    // @Increment: 1
    AP_GROUPINFO("SPACING",   1, AP_Terrain, grid_spacing, 100),

    AP_GROUPEND
};

// constructor
AP_Terrain::AP_Terrain(AP_AHRS &_ahrs) :
    ahrs(_ahrs),
    last_request_time_ms(0),
    disk_io_state(DiskIoIdle),
    fd(-1),
    timer_setup(false),
    file_lat_degrees(0),
    file_lon_degrees(0),
    io_failure(false),
    directory_created(false)
{
    AP_Param::setup_object_defaults(this, var_info);
}

#if TERRAIN_DEBUG
#define ASSERT_RANGE(v,minv,maxv) assert((v)<=(maxv)&&(v)>=(minv))
#else
#define ASSERT_RANGE(v,minv,maxv)
#endif

/*
  calculate bit number in grid_block bitmap. This corresponds to a
  bit representing a 4x4 mavlink transmitted block
*/
uint8_t AP_Terrain::grid_bitnum(uint8_t idx_x, uint8_t idx_y)
{    
    ASSERT_RANGE(idx_x,0,27);
    ASSERT_RANGE(idx_y,0,31);
    uint8_t subgrid_x = idx_x / TERRAIN_GRID_MAVLINK_SIZE;
    uint8_t subgrid_y = idx_y / TERRAIN_GRID_MAVLINK_SIZE;
    ASSERT_RANGE(subgrid_x,0,TERRAIN_GRID_BLOCK_MUL_X-1);
    ASSERT_RANGE(subgrid_y,0,TERRAIN_GRID_BLOCK_MUL_Y-1);
    return subgrid_y + TERRAIN_GRID_BLOCK_MUL_Y*subgrid_x;
}

/*
  given a grid_info check that a given idx_x/idx_y is available (set
  in the bitmap)
 */
bool AP_Terrain::check_bitmap(const struct grid_block &grid, uint8_t idx_x, uint8_t idx_y)
{
    uint8_t bitnum = grid_bitnum(idx_x, idx_y);
    return (grid.bitmap & (((uint64_t)1U)<<bitnum)) != 0;
}

/*
  given a location, calculate the 32x28 grid SW corner, plus the
  grid indices
*/
void AP_Terrain::calculate_grid_info(const Location &loc, struct grid_info &info) const
{
    // grids start on integer degrees. This makes storing terrain data
    // on the SD card a bit easier
    info.lat_degrees = (loc.lat<0?(loc.lat-9999999L):loc.lat) / (10*1000*1000L);
    info.lon_degrees = (loc.lng<0?(loc.lng-9999999L):loc.lng) / (10*1000*1000L);

    // create reference position for this rounded degree position
    Location ref;
    ref.lat = info.lat_degrees*10*1000*1000L;
    ref.lng = info.lon_degrees*10*1000*1000L;

    // find offset from reference
    Vector2f offset = location_diff(ref, loc);

    // get indices in terms of grid_spacing elements
    uint32_t idx_x = offset.x / grid_spacing;
    uint32_t idx_y = offset.y / grid_spacing;

    // find indexes into 32*28 grids for this degree reference. Note
    // the use of TERRAIN_GRID_BLOCK_SPACING_{X,Y} which gives a one square
    // overlap between grids
    info.grid_idx_x = idx_x / TERRAIN_GRID_BLOCK_SPACING_X;
    info.grid_idx_y = idx_y / TERRAIN_GRID_BLOCK_SPACING_Y;

    // find the indices within the 32*28 grid
    info.idx_x = idx_x % TERRAIN_GRID_BLOCK_SPACING_X;
    info.idx_y = idx_y % TERRAIN_GRID_BLOCK_SPACING_Y;

    // find the fraction (0..1) within the square
    info.frac_x = (offset.x - idx_x * grid_spacing) / grid_spacing;
    info.frac_y = (offset.y - idx_y * grid_spacing) / grid_spacing;

    // calculate lat/lon of SW corner of 32*28 grid_block
    location_offset(ref, 
                    info.grid_idx_x * TERRAIN_GRID_BLOCK_SPACING_X * (float)grid_spacing,
                    info.grid_idx_y * TERRAIN_GRID_BLOCK_SPACING_Y * (float)grid_spacing);
    info.grid_lat = ref.lat;
    info.grid_lon = ref.lng;

    ASSERT_RANGE(info.idx_x,0,TERRAIN_GRID_BLOCK_SPACING_X-1);
    ASSERT_RANGE(info.idx_y,0,TERRAIN_GRID_BLOCK_SPACING_Y-1);
    ASSERT_RANGE(info.frac_x,0,1);
    ASSERT_RANGE(info.frac_y,0,1);
}


/*
  find a grid structure given a grid_info
 */
AP_Terrain::grid_cache &AP_Terrain::find_grid(const struct grid_info &info)
{
    uint16_t oldest_i = 0;

    // see if we have that grid
    for (uint16_t i=0; i<TERRAIN_GRID_BLOCK_CACHE_SIZE; i++) {
        if (cache[i].grid.lat == info.grid_lat && 
            cache[i].grid.lon == info.grid_lon &&
            cache[i].grid.spacing == grid_spacing) {
            cache[i].last_access_ms = hal.scheduler->millis();
            return cache[i];
        }
        if (cache[i].last_access_ms < cache[oldest_i].last_access_ms) {
            oldest_i = i;
        }
    }

    // Not found. Use the oldest grid and make it this grid,
    // initially unpopulated
    struct grid_cache &grid = cache[oldest_i];
    memset(&grid, 0, sizeof(grid));

    grid.grid.lat = info.grid_lat;
    grid.grid.lon = info.grid_lon;
    grid.grid.spacing = grid_spacing;
    grid.grid.grid_idx_x = info.grid_idx_x;
    grid.grid.grid_idx_y = info.grid_idx_y;
    grid.grid.lat_degrees = info.lat_degrees;
    grid.grid.lon_degrees = info.lon_degrees;
    grid.grid.version = TERRAIN_GRID_FORMAT_VERSION;
    grid.last_access_ms = hal.scheduler->millis();

    // mark as waiting for disk read
    grid.state = GRID_CACHE_DISKWAIT;

    return grid;
}

/*
  return terrain height in meters above average sea level (WGS84) for
  a given position
 */
bool AP_Terrain::height_amsl(const Location &loc, float &height)
{
    if (!enable) {
        return false;
    }

    // quick access for home altitude
    if (loc.lat == home_loc.lat &&
        loc.lng == home_loc.lng) {
        height = home_height;
        return true;
    }

    struct grid_info info;

    calculate_grid_info(loc, info);

    // find the grid
    const struct grid_block &grid = find_grid(info).grid;

    /*
      note that we rely on the one square overlap to ensure these
      calculations don't go past the end of the arrays
     */
    ASSERT_RANGE(info.idx_x, 0, TERRAIN_GRID_BLOCK_SIZE_X-2);
    ASSERT_RANGE(info.idx_y, 0, TERRAIN_GRID_BLOCK_SIZE_Y-2);


    // check we have all 4 required heights
    if (!check_bitmap(grid, info.idx_x,   info.idx_y) ||
        !check_bitmap(grid, info.idx_x,   info.idx_y+1) ||
        !check_bitmap(grid, info.idx_x+1, info.idx_y) ||
        !check_bitmap(grid, info.idx_x+1, info.idx_y+1)) {
        return false;
    }

    // hXY are the heights of the 4 surrounding grid points
    int16_t h00, h01, h10, h11;

    h00 = grid.height[info.idx_x+0][info.idx_y+0];
    h01 = grid.height[info.idx_x+0][info.idx_y+1];
    h10 = grid.height[info.idx_x+1][info.idx_y+0];
    h11 = grid.height[info.idx_x+1][info.idx_y+1];

    float avg1 = (1.0f-info.frac_x) * h00  + info.frac_x * h10;
    float avg2 = (1.0f-info.frac_x) * h01  + info.frac_x * h11;
    float avg  = (1.0f-info.frac_y) * avg1 + info.frac_y * avg2;

    height = avg;

    if (loc.lat == ahrs.get_home().lat &&
        loc.lng == ahrs.get_home().lng) {
        // remember home altitude as a special case
        home_height = height;
        home_loc = loc;
    }

    return true;
}

/*
  request any missing 4x4 grids from a block, given a grid_cache
 */
bool AP_Terrain::request_missing(mavlink_channel_t chan, struct grid_cache &gcache)
{
    struct grid_block &grid = gcache.grid;

    // see if we are waiting for disk read
    if (gcache.state == GRID_CACHE_DISKWAIT) {
        // don't request data from the GCS till we know its not on disk
        return false;
    }

    // see if it is fully populated
    if ((grid.bitmap & bitmap_mask) == bitmap_mask) {
        // it is fully populated, nothing to do
        return false;
    }

    /*
      ask the GCS to send a set of 4x4 grids
     */
    mavlink_msg_terrain_request_send(chan, grid.lat, grid.lon, grid_spacing, bitmap_mask & ~grid.bitmap);
    last_request_time_ms = hal.scheduler->millis();

    return true;
}

/*
  request any missing 4x4 grids from a block
 */
bool AP_Terrain::request_missing(mavlink_channel_t chan, const struct grid_info &info)
{
    // find the grid
    struct grid_cache &gcache = find_grid(info);
    return request_missing(chan, gcache);
}

/*
  send any pending terrain request to the GCS
 */
void AP_Terrain::send_request(mavlink_channel_t chan)
{
    if (enable == 0) {
        // not enabled
        return;
    }

    // see if we need to schedule some disk IO
    schedule_disk_io();

    // did we request recently?
    if (hal.scheduler->millis() - last_request_time_ms < 2000) {
        // too soon to request again
        return;
    }

    Location loc;
    if (!ahrs.get_position(loc)) {
        // we don't know where we are
        return;
    }

    // request any missing 4x4 blocks in the current grid
    struct grid_info info;
    calculate_grid_info(loc, info);

    if (request_missing(chan, info)) {
        return;
    }

    // also request a larger set of up to 9 grids
    for (int8_t x=-1; x<=1; x++) {
        for (int8_t y=-1; y<=1; y++) {
            Location loc2 = loc;
            location_offset(loc2, 
                            x*TERRAIN_GRID_BLOCK_SIZE_X*0.7f*grid_spacing,
                            y*TERRAIN_GRID_BLOCK_SIZE_Y*0.7f*grid_spacing);
            struct grid_info info2;
            calculate_grid_info(loc2, info2);            
            if (request_missing(chan, info2)) {
                return;
            }
        }
    }

    // check cache blocks that may have been setup by a TERRAIN_CHECK
    for (uint16_t i=0; i<TERRAIN_GRID_BLOCK_CACHE_SIZE; i++) {
        if (cache[i].state >= GRID_CACHE_VALID) {
            if (request_missing(chan, cache[i])) {
                return;
            }
        }
    }

    // request the current loc last to ensure it has highest last
    // access time
    if (request_missing(chan, info)) {
        return;
    }

    // nothing to request, send a terrain report
    send_terrain_report(chan, loc);
}

/*
  count bits in a uint64_t
*/
uint8_t AP_Terrain::bitcount64(uint64_t b)
{
    return __builtin_popcount((unsigned)(b&0xFFFFFFFF)) + __builtin_popcount((unsigned)(b>>32));
}

/*
  get some statistics for TERRAIN_REPORT
*/
void AP_Terrain::get_statistics(uint16_t &pending, uint16_t &loaded)
{
    pending = 0;
    loaded = 0;
    for (uint16_t i=0; i<TERRAIN_GRID_BLOCK_CACHE_SIZE; i++) {
        if (cache[i].state == GRID_CACHE_INVALID) {
            continue;
        }
        uint8_t maskbits = TERRAIN_GRID_BLOCK_MUL_X*TERRAIN_GRID_BLOCK_MUL_Y;
        if (cache[i].state == GRID_CACHE_DISKWAIT) {
            pending += maskbits;
            continue;
        }
        uint8_t bitcount = bitcount64(cache[i].grid.bitmap);
        pending += maskbits - bitcount;
        loaded += bitcount;
    }
}


/* 
   handle terrain messages from GCS
 */
void AP_Terrain::handle_data(mavlink_channel_t chan, mavlink_message_t *msg)
{
    if (msg->msgid == MAVLINK_MSG_ID_TERRAIN_DATA) {
        handle_terrain_data(msg);
    } else if (msg->msgid == MAVLINK_MSG_ID_TERRAIN_CHECK) {
        handle_terrain_check(chan, msg);
    }
}


/* 
   send a TERRAIN_REPORT for a location
 */
void AP_Terrain::send_terrain_report(mavlink_channel_t chan, const Location &loc)
{
    float height = 0;
    uint16_t spacing = 0;
    if (height_amsl(loc, height)) {
        spacing = grid_spacing;
    }
    uint16_t pending, loaded;
    get_statistics(pending, loaded);
    if (comm_get_txspace(chan) >= MAVLINK_NUM_NON_PAYLOAD_BYTES + MAVLINK_MSG_ID_TERRAIN_REPORT_LEN) {
        mavlink_msg_terrain_report_send(chan, loc.lat, loc.lng, spacing, height, pending, loaded);
    }
}

/* 
   handle TERRAIN_CHECK messages from GCS
 */
void AP_Terrain::handle_terrain_check(mavlink_channel_t chan, mavlink_message_t *msg)
{
    mavlink_terrain_check_t packet;
    mavlink_msg_terrain_check_decode(msg, &packet);
    Location loc;
    loc.lat = packet.lat;
    loc.lng = packet.lon;
    send_terrain_report(chan, loc);
}

/* 
   handle TERRAIN_DATA messages from GCS
 */
void AP_Terrain::handle_terrain_data(mavlink_message_t *msg)
{
    mavlink_terrain_data_t packet;
    mavlink_msg_terrain_data_decode(msg, &packet);

    uint16_t i;
    for (i=0; i<TERRAIN_GRID_BLOCK_CACHE_SIZE; i++) {
        if (cache[i].grid.lat == packet.lat && 
            cache[i].grid.lon == packet.lon && 
            cache[i].grid.spacing == packet.grid_spacing &&
            packet.gridbit < 56) {
            break;
        }
    }
    if (i == TERRAIN_GRID_BLOCK_CACHE_SIZE) {
        // we don't have that grid, ignore data
        return;
    }
    struct grid_cache &gcache = cache[i];
    struct grid_block &grid = gcache.grid;
    uint8_t idx_x = (packet.gridbit / TERRAIN_GRID_BLOCK_MUL_Y) * TERRAIN_GRID_MAVLINK_SIZE;
    uint8_t idx_y = (packet.gridbit % TERRAIN_GRID_BLOCK_MUL_Y) * TERRAIN_GRID_MAVLINK_SIZE;
    ASSERT_RANGE(idx_x,0,(TERRAIN_GRID_BLOCK_MUL_X-1)*TERRAIN_GRID_MAVLINK_SIZE);
    ASSERT_RANGE(idx_y,0,(TERRAIN_GRID_BLOCK_MUL_Y-1)*TERRAIN_GRID_MAVLINK_SIZE);
    for (uint8_t x=0; x<TERRAIN_GRID_MAVLINK_SIZE; x++) {
        for (uint8_t y=0; y<TERRAIN_GRID_MAVLINK_SIZE; y++) {
            grid.height[idx_x+x][idx_y+y] = packet.data[x*TERRAIN_GRID_MAVLINK_SIZE+y];
            ASSERT_RANGE(grid.height[idx_x+x][idx_y+y], 1, 20000);
        }
    }
    gcache.grid.bitmap |= ((uint64_t)1) << packet.gridbit;
    
    // mark dirty for disk IO
    gcache.state = GRID_CACHE_DIRTY;
    
#if TERRAIN_DEBUG
    hal.console->printf("Filled bit %u idx_x=%u idx_y=%u\n", 
                        (unsigned)packet.gridbit, (unsigned)idx_x, (unsigned)idx_y);
    if (gcache.grid.bitmap == bitmap_mask) {
        hal.console->printf("--lat=%12.7f --lon=%12.7f %u\n",
                            grid.lat*1.0e-7f,
                            grid.lon*1.0e-7f,
                            grid.height[0][0]);
        Location loc2;
        loc2.lat = grid.lat;
        loc2.lng = grid.lon;
        location_offset(loc2, 28*grid_spacing, 32*grid_spacing);
        hal.console->printf("--lat=%12.7f --lon=%12.7f %u\n",
                            loc2.lat*1.0e-7f,
                            loc2.lng*1.0e-7f,
                            grid.height[27][31]);            
    }
#endif
    
    // see if we need to schedule some disk IO
    update();
}

/*
  find cache index of disk_block
 */
int16_t AP_Terrain::find_io_idx(enum GridCacheState state)
{
    // try first with given state
    for (uint16_t i=0; i<TERRAIN_GRID_BLOCK_CACHE_SIZE; i++) {
        if (disk_block.block.lat == cache[i].grid.lat &&
            disk_block.block.lon == cache[i].grid.lon && 
            cache[i].state == state) {
            return i;
        }
    }    
    // then any state
    for (uint16_t i=0; i<TERRAIN_GRID_BLOCK_CACHE_SIZE; i++) {
        if (disk_block.block.lat == cache[i].grid.lat &&
            disk_block.block.lon == cache[i].grid.lon) {
            return i;
        }
    }    
    return -1;
}

/*
  check for blocks that need to be read from disk
 */
void AP_Terrain::check_disk_read(void)
{
    for (uint16_t i=0; i<TERRAIN_GRID_BLOCK_CACHE_SIZE; i++) {
        if (cache[i].state == GRID_CACHE_DISKWAIT) {
            disk_block.block = cache[i].grid;
            disk_io_state = DiskIoWaitRead;
            return;
        }
    }    
}

/*
  check for blocks that need to be written to disk
 */
void AP_Terrain::check_disk_write(void)
{
    for (uint16_t i=0; i<TERRAIN_GRID_BLOCK_CACHE_SIZE; i++) {
        if (cache[i].state == GRID_CACHE_DIRTY) {
            disk_block.block = cache[i].grid;
            disk_io_state = DiskIoWaitWrite;
            return;
        }
    }    
}

/*
  Check if we need to do disk IO for grids. 
 */
void AP_Terrain::schedule_disk_io(void)
{
    if (enable == 0) {
        return;
    }

    if (!timer_setup) {
        timer_setup = true;
        hal.scheduler->register_io_process(AP_HAL_MEMBERPROC(&AP_Terrain::io_timer));        
    }

    switch (disk_io_state) {
    case DiskIoIdle:
        // look for a block that needs reading or writing
        check_disk_read();
        if (disk_io_state == DiskIoIdle) {
            // still idle, check for writes
            check_disk_write();            
        }
        break;
        
    case DiskIoDoneRead: {
        // a read has completed
        int16_t cache_idx = find_io_idx(GRID_CACHE_DISKWAIT);
        if (cache_idx != -1) {
            if (disk_block.block.bitmap != 0) {
                // when bitmap is zero we read an empty block
                cache[cache_idx].grid = disk_block.block;
            }
            cache[cache_idx].state = GRID_CACHE_VALID;
            cache[cache_idx].last_access_ms = hal.scheduler->millis();
        }
        disk_io_state = DiskIoIdle;
        break;
    }

    case DiskIoDoneWrite: {
        // a write has completed
        int16_t cache_idx = find_io_idx(GRID_CACHE_DIRTY);
        if (cache_idx != -1) {
            if (cache[cache_idx].grid.bitmap == disk_block.block.bitmap) {
                // only mark valid if more grids haven't been added
                cache[cache_idx].state = GRID_CACHE_VALID;
            }
        }
        disk_io_state = DiskIoIdle;
        break;
    }
        
    case DiskIoWaitWrite:
    case DiskIoWaitRead:
        // waiting for io_timer()
        break;
    }
}

/*
  1Hz update function. This is here to ensure progress is made on disk
  IO even if no MAVLink send_request() operations are called for a
  while.
 */
void AP_Terrain::update(void)
{
    // just schedule any needed disk IO
    schedule_disk_io();
}

/*
  get CRC for a block
 */
uint16_t AP_Terrain::get_block_crc(struct grid_block &block)
{
    uint16_t saved_crc = block.crc;
    block.crc = 0;
    uint16_t ret = crc16_ccitt((const uint8_t *)&block, sizeof(block), 0);
    block.crc = saved_crc;
    return ret;
}


/********************************************************
All the functions below this point run in the IO timer
context, which is a separate thread. The code uses the state
machine controlled by disk_io_state to manage who has 
access to the structures and to prevent race conditions.

The IO timer context owns the data when disk_io_state is 
DiskIoWaitWrite or DiskIoWaitRead. The main thread owns the
data when disk_io_state is DiskIoIdle, DiskIoDoneWrite or
DiskIoDoneRead

All file operations are done by the IO thread.
*********************************************************/


/*
  open the current degree file
 */
void AP_Terrain::open_file(void)
{
    struct grid_block &block = disk_block.block;
    if (fd != -1 && 
        block.lat_degrees == file_lat_degrees &&
        block.lon_degrees == file_lon_degrees) {
        // already open on right file
        return;
    }

    // build the pathname to the degree file
    char path[] = HAL_BOARD_TERRAIN_DIRECTORY "/NxxExxx.DAT";
    char *p = &path[strlen(HAL_BOARD_TERRAIN_DIRECTORY)+1];
    snprintf(p, 12, "%c%02u%c%03u.DAT",
             block.lat_degrees<0?'S':'N',
             abs(block.lat_degrees),
             block.lon_degrees<0?'W':'E',
             abs(block.lon_degrees));

    // create directory if need be
    if (!directory_created) {
        mkdir(HAL_BOARD_TERRAIN_DIRECTORY, 0755);
        directory_created = true;
    }

    if (fd != -1) {
        ::close(fd);
    }
    fd = ::open(path, O_RDWR|O_CREAT, 0644);
    if (fd == -1) {
#if TERRAIN_DEBUG
        hal.console->printf("Open %s failed - %s\n",
                            path, strerror(errno));
#endif
        io_failure = true;
        return;
    }

    file_lat_degrees = block.lat_degrees;
    file_lon_degrees = block.lon_degrees;
}

/*
  seek to the right offset for disk_block
 */
void AP_Terrain::seek_offset(void)
{
    struct grid_block &block = disk_block.block;
    // work out how many longitude blocks there are at this latitude
    Location loc1, loc2;
    loc1.lat = block.lat_degrees*10*1000*1000L;
    loc1.lng = block.lon_degrees*10*1000*1000L;
    loc2.lat = block.lat_degrees*10*1000*1000L;
    loc2.lng = (block.lon_degrees+1)*10*1000*1000L;

    // shift another two blocks east to ensure room is available
    location_offset(loc2, 0, 2*grid_spacing*TERRAIN_GRID_BLOCK_SIZE_Y);
    Vector2f offset = location_diff(loc1, loc2);
    uint16_t east_blocks = offset.y / (grid_spacing*TERRAIN_GRID_BLOCK_SIZE_Y);

    uint32_t file_offset = (east_blocks * block.grid_idx_x + 
                            block.grid_idx_y) * sizeof(union grid_io_block);
    if (::lseek(fd, file_offset, SEEK_SET) != file_offset) {
#if TERRAIN_DEBUG
        hal.console->printf("Seek %lu failed - %s\n",
                            (unsigned long)file_offset, strerror(errno));
#endif
        ::close(fd);
        fd = -1;
        io_failure = true;
    }
}

/*
  write out disk_block
 */
void AP_Terrain::write_block(void)
{
    seek_offset();
    if (io_failure) {
        return;
    }

    disk_block.block.crc = get_block_crc(disk_block.block);

    ssize_t ret = ::write(fd, &disk_block, sizeof(disk_block));
    if (ret  != sizeof(disk_block)) {
#if TERRAIN_DEBUG
        hal.console->printf("write failed - %s\n", strerror(errno));
#endif
        ::close(fd);
        fd = -1;
        io_failure = true;
    } else {
        ::fsync(fd);
#if TERRAIN_DEBUG
        printf("wrote block at %ld %ld ret=%d mask=%07llx\n",
               (long)disk_block.block.lat,
               (long)disk_block.block.lon,
               (int)ret,
               (unsigned long long)disk_block.block.bitmap);
#endif
    }
    disk_io_state = DiskIoDoneWrite;
}

/*
  read in disk_block
 */
void AP_Terrain::read_block(void)
{
    seek_offset();
    if (io_failure) {
        return;
    }
    int32_t lat = disk_block.block.lat;
    int32_t lon = disk_block.block.lon;

    ssize_t ret = ::read(fd, &disk_block, sizeof(disk_block));
    if (ret != sizeof(disk_block) || 
        disk_block.block.lat != lat || 
        disk_block.block.lon != lon ||
        disk_block.block.bitmap == 0 ||
        disk_block.block.spacing != grid_spacing ||
        disk_block.block.version != TERRAIN_GRID_FORMAT_VERSION ||
        disk_block.block.crc != get_block_crc(disk_block.block)) {
#if TERRAIN_DEBUG
        printf("read empty block at %ld %ld ret=%d\n",
               (long)lat,
               (long)lon,
               (int)ret);
#endif
        // a short read or bad data is not an IO failure, just a
        // missing block on disk
        memset(&disk_block, 0, sizeof(disk_block));
        disk_block.block.lat = lat;
        disk_block.block.lon = lon;
        disk_block.block.bitmap = 0;
    } else {
#if TERRAIN_DEBUG
        printf("read block at %ld %ld ret=%d mask=%07llx\n",
               (long)lat,
               (long)lon,
               (int)ret,
               (unsigned long long)disk_block.block.bitmap);
#endif
    }
    disk_io_state = DiskIoDoneRead;
}

/*
  timer called to do disk IO
 */
void AP_Terrain::io_timer(void)
{
    if (io_failure) {
        // don't keep trying io, so we don't thrash the filesystem
        // code while flying
        return;
    }

    switch (disk_io_state) {
    case DiskIoIdle:
    case DiskIoDoneRead:
    case DiskIoDoneWrite:
        // nothing to do
        break;
        
    case DiskIoWaitWrite:
        // need to write out the block
        open_file();
        if (fd == -1) {
            return;
        }
        write_block();
        break;

    case DiskIoWaitRead:
        // need to read in the block
        open_file();
        if (fd == -1) {
            return;
        }
        read_block();
        break;
    }
}

#endif // HAVE_AP_TERRAIN