[gps-watch.git] / src / common / logger.rs

/*
 * Copyright (c) 2017-2020 Tilman Sauerbeck (tilman at code-monkey de)
 *
 * Permission is hereby granted, free of charge, to any person obtaining
 * a copy of this software and associated documentation files (the
 * "Software"), to deal in the Software without restriction, including
 * without limitation the rights to use, copy, modify, merge, publish,
 * distribute, sublicense, and/or sell copies of the Software, and to
 * permit persons to whom the Software is furnished to do so, subject to
 * the following conditions:
 *
 * The above copyright notice and this permission notice shall be
 * included in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
 * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
 * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
 * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 */

use gps::TimeAndPos;
use storage::Storage;
use varint;
use systick::elapsed_ms;
use buffer::Buffer;
use yencode::Yencode;
use fmt::*;
use time::Time;

pub const MEMORY_SIZE: usize = 2 << 20;
const SECTOR_SIZE: usize = 4 << 10;

const NUM_SECTORS: usize = MEMORY_SIZE / SECTOR_SIZE;

#[derive(Clone, Copy, PartialEq, Debug)]
pub enum Error {
    NoSuchRecording,
    StorageError,
}

enum SectorFlag {
    InUse      = 1 << 0,
    DataRecord = 1 << 1,

    // Overrides InUse. The idea is to have erased flash sectors
    // (0xff..ff) be detected as not in use.
    NotInUse   = 1 << 7,
}

#[repr(C)]
#[derive(Clone, Copy)]
struct SectorHeader {
    flags: u16, // Combination of SectorFlag items.
    recording_id: u16, // Zero is considered invalid.
    start_time: u32, // UNIX time. Only present if flag DataRecord isn't set.
}

impl SectorHeader {
    fn new() -> SectorHeader {
        SectorHeader {
            flags: 0,
            recording_id: 0,
            start_time: 0,
        }
    }

    fn is_in_use(&self) -> bool {
        let mask = (SectorFlag::InUse as u16) | (SectorFlag::NotInUse as u16);
        let value = SectorFlag::InUse as u16;

        (self.flags & mask) == value
    }

    fn starts_recording(&self) -> bool {
        let mask = (SectorFlag::InUse as u16)
                 | (SectorFlag::DataRecord as u16)
                 | (SectorFlag::NotInUse as u16);
        let value = SectorFlag::InUse as u16;

        (self.flags & mask) == value
    }

    fn belongs_to(&self, recording_id: u16) -> bool {
        self.is_in_use() && self.recording_id == recording_id
    }
}

#[derive(Clone, Copy)]
struct InFlight {
    d_time_s: u32,
    d_lat: i32,
    d_lon: i32,
}

impl InFlight {
    fn new() -> InFlight {
        InFlight {
            d_time_s: 0,
            d_lat: 0,
            d_lon: 0,
        }
    }
}

pub struct Logger<'a> {
    pub storage: &'a mut dyn Storage,

    recording_id: u16, // Zero is considered invalid.

    // The index of the first sector of the currently running recording.
    // Only written in logger_start_recording.
    first_sector: u16,

    recording_started: u32,

    // The total distance logged of the currently running recording.
    pub total_distance_cm: u32,

    split_distance_cm: u32,
    split_duration_ms: u32,
    pub pace_s: u32,

    // The number of slots filled in num_flight.
    num_in_flight: usize,

    // Deltas not yet written out to write_buffer.
    //
    // Limiting ourselves to 7 items here means we can use
    // 0xff as a padding byte.
    in_flight: [InFlight; 7],

    sector_header: [SectorHeader; NUM_SECTORS],

    sectors_written: u16,

    write_buffer_offset: usize,
    write_buffer: [u8; SECTOR_SIZE],
}

struct SectorHeaderIter<'a> {
    sector_header: &'a [SectorHeader; NUM_SECTORS],
    it_front: usize,
    it_back: usize,
    indices: [u16; NUM_SECTORS],
}

fn cmp_sector_header_indices(a: u16, b: u16,
                             sector_header: &[SectorHeader]) -> i32 {
    let header_a = &sector_header[a as usize];
    let header_b = &sector_header[b as usize];

    if header_a.starts_recording() && header_b.starts_recording() {
        // Latest entries come first.
        if header_a.start_time > header_b.start_time {
            -1
        } else if header_a.start_time < header_b.start_time {
            1
        } else {
            0
        }
    } else if header_a.starts_recording() {
        -1
    } else if header_b.starts_recording() {
        1
    } else if a < b {
        -1
    } else if a > b {
        1
    } else {
        0
    }
}

fn downheap(heap: &mut [u16], mut index: usize, num_elts: usize,
            sector_header: &[SectorHeader]) {
    let orig = heap[index];

    loop {
        let mut worker = index * 2;

        if worker < num_elts &&
           cmp_sector_header_indices(heap[worker], heap[worker + 1], sector_header) < 0 {
            worker += 1;
        }

        if worker > num_elts ||
           cmp_sector_header_indices(orig, heap[worker], sector_header) >= 0 {
            break;
        }

        heap[index] = heap[worker];
        index = worker;
    }

    heap[index] = orig;
}

impl<'a> SectorHeaderIter<'a> {
    fn new(logger: &'a Logger) -> SectorHeaderIter<'a> {
        let mut iter = SectorHeaderIter {
            sector_header: &logger.sector_header,
            it_front: 0,
            it_back: NUM_SECTORS,
            indices: [0; NUM_SECTORS]
        };

        for i in 0..NUM_SECTORS {
            iter.indices[i] = i as u16;
        }

        iter.sort(NUM_SECTORS);

        // XXX:
        // Need to handle those sectors that don't start recordings
        // but that are still used.

        iter
    }

    fn sort(&mut self, num_elts_to_sort: usize) {
        for i in (1..((num_elts_to_sort + 1) / 2) + 1).rev() {
            downheap(&mut self.indices, i - 1, num_elts_to_sort - 1,
                     self.sector_header);
        }

        for i in (1..num_elts_to_sort).rev() {
            self.indices.swap(0, i);

            downheap(&mut self.indices, 0, i - 1, self.sector_header);
        }
    }
}

impl<'a> Iterator for SectorHeaderIter<'a> {
    type Item = usize;

    fn next(&mut self) -> Option<usize> {
        if self.it_front == self.it_back {
            None
        } else {
            let next_index = self.indices[self.it_front] as usize;

            self.it_front += 1;

            Some(next_index)
        }
    }
}

impl<'a> DoubleEndedIterator for SectorHeaderIter<'a> {
    fn next_back(&mut self) -> Option<usize> {
        if self.it_back == self.it_front {
            None
        } else {
            self.it_back -= 1;

            let next_index = self.indices[self.it_back] as usize;

            Some(next_index)
        }
    }
}

fn normalize_angle(mut angle: i32) -> i32 {
    let deg90 = 90 * 60 * 10000;
    let deg180 = deg90 << 1;
    let deg360 = deg180 << 1;

    while angle >= deg180 {
        angle -= deg360;
    }

    while angle <= -deg180 {
        angle += deg360;
    }

    angle
}

fn max<T>(a: T, b: T) -> T
          where T: PartialOrd {
    if a > b {
        a
    } else {
        b
    }
}

impl<'a> Logger<'a> {
    pub fn new(storage: &'a mut dyn Storage) -> Logger {
        Logger {
            storage: storage,

            recording_id: 0,
            first_sector: 0,
            recording_started: 0,
            total_distance_cm: 0,
            split_distance_cm: 0,
            split_duration_ms: 0,
            pace_s: 0,
            num_in_flight: 0,

            in_flight: [InFlight::new(); 7],
            sector_header: [SectorHeader::new(); NUM_SECTORS],

            sectors_written: 0,

            write_buffer_offset: 0,
            write_buffer: [0xff; SECTOR_SIZE],
        }
    }

    pub fn init(&mut self) {
        // Reading the directory entries one by one means
        // we won't need an as large buffer on the stack.
        for i in 0..NUM_SECTORS {
            self.read_sector_header(i);
        }
    }

    fn read_sector_header(&mut self, sector_index: usize) {
        let address = sector_index * SECTOR_SIZE;
        let mut chunk = [0u8; 8];

        self.storage.read(address, &mut chunk);

        let sector_header_ptr: *mut SectorHeader =
            &mut self.sector_header[sector_index];

        unsafe {
            core::ptr::copy(chunk.as_ptr(),
                            sector_header_ptr as *mut u8,
                            chunk.len());
        }
    }

    fn prepare_write_buffer(&mut self, is_initial: bool) {
        self.write_buffer = [0xff; SECTOR_SIZE];

        let flags = if is_initial {
            (SectorFlag::InUse as u16)
        } else {
            (SectorFlag::InUse as u16) | (SectorFlag::DataRecord as u16)
        };

        // Write sector header.
        self.write_buffer[0..2].copy_from_slice(&flags.to_le_bytes());
        self.write_buffer[2..4].copy_from_slice(&self.recording_id.to_le_bytes());

        self.write_buffer_offset = 4;

        if is_initial {
            let start = self.write_buffer_offset;
            let end = start + 4;

            self.write_buffer[start..end].copy_from_slice(
                &self.recording_started.to_le_bytes());

            self.write_buffer_offset += 4;
        }
    }

    pub fn start_recording(&mut self, tap: &TimeAndPos) -> u16 {
        self.find_next_record_slot();

        self.sectors_written = 0;
        self.recording_started = tap.unix_time;
        self.total_distance_cm = 0;
        self.split_distance_cm = 0;
        self.split_duration_ms = 0;
        self.pace_s = 0;
        self.num_in_flight = 0;

        self.prepare_write_buffer(true);

        self.write_packet(0, tap.latitude_deg, tap.longitude_deg);

        self.recording_id
    }

    pub fn log(&mut self, prev_tap: &TimeAndPos, tap: &TimeAndPos) {
        let d_time_ms = elapsed_ms(tap.system_time, prev_tap.system_time);

        // We know that our hardware cannot deliver updates more often
        // than once a second. However when there's a delay in evaluating
        // the hardware's messages, we will end up with intervals like
        // 1050ms and 950ms (the latter will "make up" for the slowness
        // in the former). To avoid logging deltas of 0 seconds, we round
        // the intervals to full seconds.
        let d_time_s = (d_time_ms + 500) / 1000;

        let d_lat = tap.latitude_deg - prev_tap.latitude_deg;
        let d_lon = tap.longitude_deg - prev_tap.longitude_deg;

        if self.write_packet(d_time_s, d_lat, d_lon) {
            self.flush_in_flight(false);
        }

        let distance_cm = tap.distance_cm(&prev_tap) as u32;

        self.total_distance_cm += distance_cm;
        self.split_distance_cm += distance_cm;

        self.split_duration_ms += d_time_ms;

        if self.split_distance_cm >= 100_000 {
            self.split_distance_cm -= 100_000;

            self.pace_s = self.split_duration_ms / 1000;
            self.split_duration_ms = 0;
        }
    }

    pub fn stop_recording(&mut self, tap: &TimeAndPos) -> u16 {
        // Mark the end of the points stream.
        self.write_packet(0xffffffff, 0, 0);
        self.flush_in_flight(true);

        // Write footer.
        let duration = (tap.unix_time - self.recording_started) as u16;

        let start = self.write_buffer_offset;
        let end = start + 2;
        let dst = &mut self.write_buffer[start..end];

        dst.copy_from_slice(&duration.to_le_bytes());

        let this_sector = self.first_sector + self.sectors_written;

        if self.storage.write(this_sector as usize * SECTOR_SIZE,
                              &self.write_buffer).is_err() {
            // XXX
        }

        self.sectors_written += 1;

        for i in 0..self.sectors_written {
            self.read_sector_header((self.first_sector + i) as usize);
        }

        self.sectors_written
    }

    fn sector_header_iter(&self) -> SectorHeaderIter {
        SectorHeaderIter::new(self)
    }

    fn find_next_record_slot(&mut self) {
        let mut candidate_index = 0;
        let mut max_recording_id = 0;

        for index in self.sector_header_iter() {
            candidate_index = index;

            let sector_header = &self.sector_header[index];

            if !sector_header.is_in_use() {
                // Due to our sorting we know that there will be no more
                // used directory entries following. At this point
                // we aren't interested in unused ones, so break the loop.
                break;
            }

            max_recording_id =
                max(max_recording_id, sector_header.recording_id);
        }

        self.first_sector = candidate_index as u16;
        self.recording_id = max_recording_id.wrapping_add(1);
    }

    fn write_packet(&mut self, d_time_s: u32, d_lat: i32, d_lon: i32) -> bool {
        {
            let in_flight = &mut self.in_flight[self.num_in_flight];

            in_flight.d_time_s = d_time_s;
            in_flight.d_lat = normalize_angle(d_lat);
            in_flight.d_lon = normalize_angle(d_lon);
        }

        self.num_in_flight += 1;

        self.num_in_flight == self.in_flight.len()
    }

    // Flushes the "in flight" items to the write buffer.
    //
    // @param is_final @c true iff this is the final flush in this recording.
    //
    // @note May only be called if logger.num_in_flight is greater than zero.
    fn flush_in_flight(&mut self, is_final: bool) {
        let mut flags = 0u8;

        // Normally our items will have a time delta of one second.
        // Mark the ones that differ from that.
        for i in 0..self.num_in_flight {
            if self.in_flight[i].d_time_s != 1 {
                flags |= 1 << i;
            }
        }

        let mut buffer = [0u8; 128];
        let mut offset = 0;

        buffer[offset] = flags;
        offset += 1;

        for i in 0..(self.num_in_flight - 1) {
            let in_flight = &self.in_flight[i];

            // Only write the time delta for the atypical cases.
            if (flags & (1 << i)) != 0 {
                offset +=
                    varint::write_u32(&mut buffer[offset..], in_flight.d_time_s);
            }

            offset +=
                varint::write_s32(&mut buffer[offset..], in_flight.d_lat);

            offset +=
                varint::write_s32(&mut buffer[offset..], in_flight.d_lon);
        }

        let i = self.num_in_flight - 1;
        let in_flight = &self.in_flight[i];

        // Only write the time delta for the atypical cases.
        if (flags & (1 << i)) != 0 {
            offset +=
                varint::write_u32(&mut buffer[offset..], in_flight.d_time_s);
        }

        // The final point is an end-of-stream marker and doesn't store
        // d_lat or d_lon.
        if !is_final {
            offset +=
                varint::write_s32(&mut buffer[offset..], in_flight.d_lat);

            offset +=
                varint::write_s32(&mut buffer[offset..], in_flight.d_lon);
        }

        self.num_in_flight = 0;

        let num_bytes_written = offset;

        let remaining = self.write_buffer.len() - self.write_buffer_offset;

        if remaining < num_bytes_written {
            // We may use 0xff as padding bytes, since 0xff isn't a valid
            // first byte in a points batch. prepare_write_buffer() fills
            // our buffer with 0xff, so we don't need to do anything here.
            let this_sector = self.first_sector + self.sectors_written;

            if self.storage.write(this_sector as usize * SECTOR_SIZE,
                                  &self.write_buffer).is_err() {
                // XXX
            }

            self.sectors_written += 1;

            self.prepare_write_buffer(false);
        }

        let start = self.write_buffer_offset;
        let end = start + num_bytes_written;
        let dst = &mut self.write_buffer[start..end];

        dst.copy_from_slice(&buffer[0..num_bytes_written]);

        self.write_buffer_offset += num_bytes_written;
    }

    ///
    /// Write a listing of the stored recordings to @p tx_buf.
    pub fn list_recordings(&mut self, tx_buf: &mut Buffer) {
        for index in self.sector_header_iter().rev() {
            let sector_header = &self.sector_header[index as usize];

            if !sector_header.starts_recording() {
                continue;
            }

            let mut num_data_sectors = 0;

            for d in 1..NUM_SECTORS {
                let wrapped_index = ((index + d) & (NUM_SECTORS - 1)) as usize;
                let other_sector_header = &self.sector_header[wrapped_index];

                if other_sector_header.belongs_to(sector_header.recording_id) {
                    num_data_sectors += 1;
                }
            }

            let mut date_time_s = [b' '; 19];

            if let Some(tm) = Time::from_unix_time(sector_header.start_time) {
                tm.fmt_date(&mut date_time_s[0..]);
                tm.fmt_time(&mut date_time_s[11..]);
            }

            let recording_size = (num_data_sectors + 1) * (SECTOR_SIZE >> 10);

            let mut recording_size_s = [b'0'; 9];
            let recording_size_s_len = fmt_u32_pad(&mut recording_size_s,
                                                   recording_size as u32,
                                                   8, b' ');

            let mut recording_id_s = [b'0'; 9];
            let recording_id_s_len =
                fmt_u32_pad(&mut recording_id_s,
                            sector_header.recording_id as u32,
                            8, b' ');

            tx_buf.write(&date_time_s);
            tx_buf.write(&recording_size_s[0..recording_size_s_len]);
            tx_buf.write(b"K");
            tx_buf.write(&recording_id_s[0..recording_id_s_len]);
            tx_buf.write(b"\n");

            tx_buf.flush();
        }
    }

    ///
    /// Check whether or not recording @p recording_id exists.
    pub fn has_recording(&mut self, recording_id: u16) -> bool {
        if recording_id == 0 {
            return false;
        }

        self.sector_header_iter().find(|&index| {
            let sector_header = &self.sector_header[index as usize];

            sector_header.recording_id == recording_id &&
            sector_header.starts_recording()
        }).is_some()
    }

    ///
    /// Retrieve recording @p recording_id and
    /// write it to @p tx_buf in yencoded form.
    pub fn get_recording(&mut self, recording_id: u16,
                         tx_buf: &mut Buffer) -> Result<(), Error> {
        if recording_id == 0 {
            return Err(Error::NoSuchRecording);
        }

        if let Some(found_index) = self.sector_header_iter().find(|&index| {
            let sector_header = &self.sector_header[index as usize];

            sector_header.recording_id == recording_id &&
            sector_header.starts_recording()
        }) {
            let mut filename = [b' '; 29];

            filename[0..].copy_from_slice(b"gps-watch-recording-XXXXX.bin");

            fmt_u32_pad(&mut filename[20..], recording_id as u32, 5, b'0');

            let mut yenc = Yencode::new(tx_buf);

            yenc.start(&filename);

            let format_version = 1u8;
            yenc.data(&[format_version]);

            let mut next_sector = found_index as usize;

            for _ in 0..NUM_SECTORS {
                let address = next_sector * SECTOR_SIZE;
                let mut buf = [0u8; SECTOR_SIZE];

                self.storage.read(address, &mut buf);

                // Skip flags and recording ID.
                yenc.data(&buf[4..]);

                next_sector += 1;
                next_sector &= NUM_SECTORS - 1;

                if !self.sector_header[next_sector].belongs_to(recording_id) {
                    break;
                }
            }

            yenc.finish();

            tx_buf.flush();

            Ok(())
        } else {
            Err(Error::NoSuchRecording)
        }
    }

    ///
    /// Remove recording @p recording_id.
    pub fn remove_recording(&mut self, recording_id: u16) -> Result<(), Error> {
        if let Some(found_index) = (0..NUM_SECTORS).find(|&index| {
            let sector_header = &self.sector_header[index as usize];

            sector_header.recording_id == recording_id &&
            sector_header.starts_recording()
        }) {
            let mut next_sector = found_index as usize;

            for _ in 0..NUM_SECTORS {
                let address = next_sector * SECTOR_SIZE;

                if let Err(_) = self.storage.erase(address) {
                    return Err(Error::StorageError);
                }

                // Mark this sector as eligible for the next recording
                // and ensure it won't be picked up by list_recordings().
                self.read_sector_header(next_sector);

                next_sector += 1;
                next_sector &= NUM_SECTORS - 1;

                if !self.sector_header[next_sector].belongs_to(recording_id) {
                    break;
                }
            }

            Ok(())
        } else {
            Err(Error::NoSuchRecording)
        }
    }
}