common: Store GPS coordinates in radians, too.
[gps-watch.git] / src / common / logger.rs
1 /*
2  * Copyright (c) 2017-2020 Tilman Sauerbeck (tilman at code-monkey de)
3  *
4  * Permission is hereby granted, free of charge, to any person obtaining
5  * a copy of this software and associated documentation files (the
6  * "Software"), to deal in the Software without restriction, including
7  * without limitation the rights to use, copy, modify, merge, publish,
8  * distribute, sublicense, and/or sell copies of the Software, and to
9  * permit persons to whom the Software is furnished to do so, subject to
10  * the following conditions:
11  *
12  * The above copyright notice and this permission notice shall be
13  * included in all copies or substantial portions of the Software.
14  *
15  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
16  * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
17  * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
18  * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
19  * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
20  * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
21  * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
22  */
23
24 use gps::TimeAndPos;
25 use storage::Storage;
26 use varint;
27 use systick::elapsed_ms;
28 use buffer::Buffer;
29 use yencode::Yencode;
30 use fmt::*;
31 use time::Time;
32
33 pub const MEMORY_SIZE: usize = 2 << 20;
34 const SECTOR_SIZE: usize = 4 << 10;
35
36 const NUM_SECTORS: usize = MEMORY_SIZE / SECTOR_SIZE;
37
38 #[derive(Clone, Copy, PartialEq, Debug)]
39 pub enum Error {
40     NoSuchRecording,
41 }
42
43 enum SectorFlag {
44     InUse      = 1 << 0,
45     DataRecord = 1 << 1,
46
47     // Overrides InUse. The idea is to have erased flash sectors
48     // (0xff..ff) be detected as not in use.
49     NotInUse   = 1 << 7,
50 }
51
52 #[repr(C)]
53 #[derive(Clone, Copy)]
54 struct SectorHeader {
55     flags: u16, // Combination of SectorFlag items.
56     recording_id: u16, // Zero is considered invalid.
57     start_time: u32, // UNIX time. Only present if flag DataRecord isn't set.
58 }
59
60 impl SectorHeader {
61     fn new() -> SectorHeader {
62         SectorHeader {
63             flags: 0,
64             recording_id: 0,
65             start_time: 0,
66         }
67     }
68
69     fn is_in_use(&self) -> bool {
70         let mask = (SectorFlag::InUse as u16) | (SectorFlag::NotInUse as u16);
71         let value = SectorFlag::InUse as u16;
72
73         (self.flags & mask) == value
74     }
75
76     fn starts_recording(&self) -> bool {
77         let mask = (SectorFlag::InUse as u16)
78                  | (SectorFlag::DataRecord as u16)
79                  | (SectorFlag::NotInUse as u16);
80         let value = SectorFlag::InUse as u16;
81
82         (self.flags & mask) == value
83     }
84
85     fn belongs_to(&self, recording_id: u16) -> bool {
86         self.is_in_use() && self.recording_id == recording_id
87     }
88 }
89
90 #[derive(Clone, Copy)]
91 struct InFlight {
92     d_time_s: u32,
93     d_lat: i32,
94     d_lon: i32,
95 }
96
97 impl InFlight {
98     fn new() -> InFlight {
99         InFlight {
100             d_time_s: 0,
101             d_lat: 0,
102             d_lon: 0,
103         }
104     }
105 }
106
107 pub struct Logger<'a> {
108     pub storage: &'a mut dyn Storage,
109
110     recording_id: u16, // Zero is considered invalid.
111
112     // The index of the first sector of the currently running recording.
113     // Only written in logger_start_recording.
114     first_sector: u16,
115
116     recording_started: u32,
117
118     // The number of slots filled in num_flight.
119     num_in_flight: usize,
120
121     // Deltas not yet written out to write_buffer.
122     //
123     // Limiting ourselves to 7 items here means we can use
124     // 0xff as a padding byte.
125     in_flight: [InFlight; 7],
126
127     sector_header: [SectorHeader; NUM_SECTORS],
128
129     sectors_written: u16,
130
131     write_buffer_offset: usize,
132     write_buffer: [u8; SECTOR_SIZE],
133 }
134
135 struct SectorHeaderIter<'a> {
136     sector_header: &'a [SectorHeader; NUM_SECTORS],
137     it_front: usize,
138     it_back: usize,
139     indices: [u16; NUM_SECTORS],
140 }
141
142 fn cmp_sector_header_indices(a: u16, b: u16,
143                              sector_header: &[SectorHeader]) -> i32 {
144     let header_a = &sector_header[a as usize];
145     let header_b = &sector_header[b as usize];
146
147     if header_a.starts_recording() && header_b.starts_recording() {
148         // Latest entries come first.
149         if header_a.start_time > header_b.start_time {
150             -1
151         } else if header_a.start_time < header_b.start_time {
152             1
153         } else {
154             0
155         }
156     } else if header_a.starts_recording() {
157         -1
158     } else if header_b.starts_recording() {
159         1
160     } else if a < b {
161         -1
162     } else if a > b {
163         1
164     } else {
165         0
166     }
167 }
168
169 fn downheap(heap: &mut [u16], mut index: usize, num_elts: usize,
170             sector_header: &[SectorHeader]) {
171     let orig = heap[index];
172
173     loop {
174         let mut worker = index * 2;
175
176         if worker < num_elts &&
177            cmp_sector_header_indices(heap[worker], heap[worker + 1], sector_header) < 0 {
178             worker += 1;
179         }
180
181         if worker > num_elts ||
182            cmp_sector_header_indices(orig, heap[worker], sector_header) >= 0 {
183             break;
184         }
185
186         heap[index] = heap[worker];
187         index = worker;
188     }
189
190     heap[index] = orig;
191 }
192
193 impl<'a> SectorHeaderIter<'a> {
194     fn new(logger: &'a Logger) -> SectorHeaderIter<'a> {
195         let mut iter = SectorHeaderIter {
196             sector_header: &logger.sector_header,
197             it_front: 0,
198             it_back: NUM_SECTORS,
199             indices: [0; NUM_SECTORS]
200         };
201
202         for i in 0..NUM_SECTORS {
203             iter.indices[i] = i as u16;
204         }
205
206         iter.sort(NUM_SECTORS);
207
208         // XXX:
209         // Need to handle those sectors that don't start recordings
210         // but that are still used.
211
212         iter
213     }
214
215     fn sort(&mut self, num_elts_to_sort: usize) {
216         for i in (1..((num_elts_to_sort + 1) / 2) + 1).rev() {
217             downheap(&mut self.indices, i - 1, num_elts_to_sort - 1,
218                      self.sector_header);
219         }
220
221         for i in (1..num_elts_to_sort).rev() {
222             self.indices.swap(0, i);
223
224             downheap(&mut self.indices, 0, i - 1, self.sector_header);
225         }
226     }
227 }
228
229 impl<'a> Iterator for SectorHeaderIter<'a> {
230     type Item = usize;
231
232     fn next(&mut self) -> Option<usize> {
233         if self.it_front == self.it_back {
234             None
235         } else {
236             let next_index = self.indices[self.it_front] as usize;
237
238             self.it_front += 1;
239
240             Some(next_index)
241         }
242     }
243 }
244
245 impl<'a> DoubleEndedIterator for SectorHeaderIter<'a> {
246     fn next_back(&mut self) -> Option<usize> {
247         if self.it_back == self.it_front {
248             None
249         } else {
250             self.it_back -= 1;
251
252             let next_index = self.indices[self.it_back] as usize;
253
254             Some(next_index)
255         }
256     }
257 }
258
259 fn normalize_angle(mut angle: i32) -> i32 {
260     let deg90 = 90 * 60 * 10000;
261     let deg180 = deg90 << 1;
262     let deg360 = deg180 << 1;
263
264     while angle >= deg180 {
265         angle -= deg360;
266     }
267
268     while angle <= -deg180 {
269         angle += deg360;
270     }
271
272     angle
273 }
274
275 fn max<T>(a: T, b: T) -> T
276           where T: PartialOrd {
277     if a > b {
278         a
279     } else {
280         b
281     }
282 }
283
284 impl<'a> Logger<'a> {
285     pub fn new(storage: &'a mut dyn Storage) -> Logger {
286         Logger {
287             storage: storage,
288
289             recording_id: 0,
290             first_sector: 0,
291             recording_started: 0,
292             num_in_flight: 0,
293
294             in_flight: [InFlight::new(); 7],
295             sector_header: [SectorHeader::new(); NUM_SECTORS],
296
297             sectors_written: 0,
298
299             write_buffer_offset: 0,
300             write_buffer: [0xff; SECTOR_SIZE],
301         }
302     }
303
304     pub fn init(&mut self) {
305         // Reading the directory entries one by one means
306         // we won't need an as large buffer on the stack.
307         for i in 0..NUM_SECTORS {
308             self.read_sector_header(i);
309         }
310     }
311
312     fn read_sector_header(&mut self, sector_index: usize) {
313         let address = sector_index * SECTOR_SIZE;
314         let mut chunk = [0u8; 8];
315
316         self.storage.read(address, &mut chunk);
317
318         let sector_header_ptr: *mut SectorHeader =
319             &mut self.sector_header[sector_index];
320
321         unsafe {
322             core::ptr::copy(chunk.as_ptr(),
323                             sector_header_ptr as *mut u8,
324                             chunk.len());
325         }
326     }
327
328     fn prepare_write_buffer(&mut self, is_initial: bool) {
329         self.write_buffer = [0xff; SECTOR_SIZE];
330
331         let flags = if is_initial {
332             (SectorFlag::InUse as u16)
333         } else {
334             (SectorFlag::InUse as u16) | (SectorFlag::DataRecord as u16)
335         };
336
337         // Write sector header.
338         self.write_buffer[0..2].copy_from_slice(&flags.to_le_bytes());
339         self.write_buffer[2..4].copy_from_slice(&self.recording_id.to_le_bytes());
340
341         self.write_buffer_offset = 4;
342
343         if is_initial {
344             let start = self.write_buffer_offset;
345             let end = start + 4;
346
347             self.write_buffer[start..end].copy_from_slice(
348                 &self.recording_started.to_le_bytes());
349
350             self.write_buffer_offset += 4;
351         }
352     }
353
354     pub fn start_recording(&mut self, tap: &TimeAndPos) -> u16 {
355         self.find_next_record_slot();
356
357         self.sectors_written = 0;
358         self.recording_started = tap.unix_time;
359         self.num_in_flight = 0;
360
361         self.prepare_write_buffer(true);
362
363         self.write_packet(0, tap.latitude, tap.longitude);
364
365         self.recording_id
366     }
367
368     pub fn log(&mut self, prev_tap: &TimeAndPos, tap: &TimeAndPos) {
369         let d_time_ms = elapsed_ms(tap.system_time, prev_tap.system_time);
370
371         // We know that our hardware cannot deliver updates more often
372         // than once a second. However when there's a delay in evaluating
373         // the hardware's messages, we will end up with intervals like
374         // 1050ms and 950ms (the latter will "make up" for the slowness
375         // in the former). To avoid logging deltas of 0 seconds, we round
376         // the intervals to full seconds.
377         let d_time_s = (d_time_ms + 500) / 1000;
378
379         let d_lat = tap.latitude - prev_tap.latitude;
380         let d_lon = tap.longitude - prev_tap.longitude;
381
382         if self.write_packet(d_time_s, d_lat, d_lon) {
383             self.flush_in_flight(false);
384         }
385     }
386
387     pub fn stop_recording(&mut self, tap: &TimeAndPos) -> u16 {
388         // Mark the end of the points stream.
389         self.write_packet(0xffffffff, 0, 0);
390         self.flush_in_flight(true);
391
392         // Write footer.
393         let duration = (tap.unix_time - self.recording_started) as u16;
394
395         let start = self.write_buffer_offset;
396         let end = start + 2;
397         let dst = &mut self.write_buffer[start..end];
398
399         dst.copy_from_slice(&duration.to_le_bytes());
400
401         let this_sector = self.first_sector + self.sectors_written;
402
403         if self.storage.write(this_sector as usize * SECTOR_SIZE,
404                               &self.write_buffer).is_err() {
405             // XXX
406         }
407
408         self.sectors_written += 1;
409
410         for i in 0..self.sectors_written {
411             self.read_sector_header((self.first_sector + i) as usize);
412         }
413
414         self.sectors_written
415     }
416
417     fn sector_header_iter(&self) -> SectorHeaderIter {
418         SectorHeaderIter::new(self)
419     }
420
421     fn find_next_record_slot(&mut self) {
422         let mut candidate_index = 0;
423         let mut max_recording_id = 0;
424
425         for index in self.sector_header_iter() {
426             candidate_index = index;
427
428             let sector_header = &self.sector_header[index];
429
430             if !sector_header.is_in_use() {
431                 // Due to our sorting we know that there will be no more
432                 // used directory entries following. At this point
433                 // we aren't interested in unused ones, so break the loop.
434                 break;
435             }
436
437             max_recording_id =
438                 max(max_recording_id, sector_header.recording_id);
439         }
440
441         self.first_sector = candidate_index as u16;
442         self.recording_id = max_recording_id.wrapping_add(1);
443     }
444
445     fn write_packet(&mut self, d_time_s: u32, d_lat: i32, d_lon: i32) -> bool {
446         {
447             let in_flight = &mut self.in_flight[self.num_in_flight];
448
449             in_flight.d_time_s = d_time_s;
450             in_flight.d_lat = normalize_angle(d_lat);
451             in_flight.d_lon = normalize_angle(d_lon);
452         }
453
454         self.num_in_flight += 1;
455
456         self.num_in_flight == self.in_flight.len()
457     }
458
459     // Flushes the "in flight" items to the write buffer.
460     //
461     // @param is_final @c true iff this is the final flush in this recording.
462     //
463     // @note May only be called if logger.num_in_flight is greater than zero.
464     fn flush_in_flight(&mut self, is_final: bool) {
465         let mut flags = 0u8;
466
467         // Normally our items will have a time delta of one second.
468         // Mark the ones that differ from that.
469         for i in 0..self.num_in_flight {
470             if self.in_flight[i].d_time_s != 1 {
471                 flags |= 1 << i;
472             }
473         }
474
475         let mut buffer = [0u8; 128];
476         let mut offset = 0;
477
478         buffer[offset] = flags;
479         offset += 1;
480
481         for i in 0..(self.num_in_flight - 1) {
482             let in_flight = &self.in_flight[i];
483
484             // Only write the time delta for the atypical cases.
485             if (flags & (1 << i)) != 0 {
486                 offset +=
487                     varint::write_u32(&mut buffer[offset..], in_flight.d_time_s);
488             }
489
490             offset +=
491                 varint::write_s32(&mut buffer[offset..], in_flight.d_lat);
492
493             offset +=
494                 varint::write_s32(&mut buffer[offset..], in_flight.d_lon);
495         }
496
497         let i = self.num_in_flight - 1;
498         let in_flight = &self.in_flight[i];
499
500         // Only write the time delta for the atypical cases.
501         if (flags & (1 << i)) != 0 {
502             offset +=
503                 varint::write_u32(&mut buffer[offset..], in_flight.d_time_s);
504         }
505
506         // The final point is an end-of-stream marker and doesn't store
507         // d_lat or d_lon.
508         if !is_final {
509             offset +=
510                 varint::write_s32(&mut buffer[offset..], in_flight.d_lat);
511
512             offset +=
513                 varint::write_s32(&mut buffer[offset..], in_flight.d_lon);
514         }
515
516         self.num_in_flight = 0;
517
518         let num_bytes_written = offset;
519
520         let remaining = self.write_buffer.len() - self.write_buffer_offset;
521
522         if remaining < num_bytes_written {
523             // We may use 0xff as padding bytes, since 0xff isn't a valid
524             // first byte in a points batch. prepare_write_buffer() fills
525             // our buffer with 0xff, so we don't need to do anything here.
526             let this_sector = self.first_sector + self.sectors_written;
527
528             if self.storage.write(this_sector as usize * SECTOR_SIZE,
529                                   &self.write_buffer).is_err() {
530                 // XXX
531             }
532
533             self.sectors_written += 1;
534
535             self.prepare_write_buffer(false);
536         }
537
538         let start = self.write_buffer_offset;
539         let end = start + num_bytes_written;
540         let dst = &mut self.write_buffer[start..end];
541
542         dst.copy_from_slice(&buffer[0..num_bytes_written]);
543
544         self.write_buffer_offset += num_bytes_written;
545     }
546
547     ///
548     /// Write a listing of the stored recordings to @p tx_buf.
549     pub fn list_recordings(&mut self, tx_buf: &mut Buffer) {
550         for index in self.sector_header_iter().rev() {
551             let sector_header = &self.sector_header[index as usize];
552
553             if !sector_header.starts_recording() {
554                 continue;
555             }
556
557             let mut num_data_sectors = 0;
558
559             for d in 1..NUM_SECTORS {
560                 let wrapped_index = ((index + d) & (NUM_SECTORS - 1)) as usize;
561                 let other_sector_header = &self.sector_header[wrapped_index];
562
563                 if other_sector_header.belongs_to(sector_header.recording_id) {
564                     num_data_sectors += 1;
565                 }
566             }
567
568             let mut date_time_s = [b' '; 19];
569
570             if let Some(tm) = Time::from_unix_time(sector_header.start_time) {
571                 tm.fmt_date(&mut date_time_s[0..]);
572                 tm.fmt_time(&mut date_time_s[11..]);
573             }
574
575             let recording_size = (num_data_sectors + 1) * (SECTOR_SIZE >> 10);
576
577             let mut recording_size_s = [b'0'; 9];
578             let recording_size_s_len = fmt_u32_pad(&mut recording_size_s,
579                                                    recording_size as u32,
580                                                    8, b' ');
581
582             let mut recording_id_s = [b'0'; 9];
583             let recording_id_s_len =
584                 fmt_u32_pad(&mut recording_id_s,
585                             sector_header.recording_id as u32,
586                             8, b' ');
587
588             tx_buf.write(&date_time_s);
589             tx_buf.write(&recording_size_s[0..recording_size_s_len]);
590             tx_buf.write(b"K");
591             tx_buf.write(&recording_id_s[0..recording_id_s_len]);
592             tx_buf.write(b"\n");
593
594             tx_buf.flush();
595         }
596     }
597
598     ///
599     /// Check whether or not recording @p recording_id exists.
600     pub fn has_recording(&mut self, recording_id: u16) -> bool {
601         if recording_id == 0 {
602             return false;
603         }
604
605         self.sector_header_iter().find(|&index| {
606             let sector_header = &self.sector_header[index as usize];
607
608             sector_header.recording_id == recording_id &&
609             sector_header.starts_recording()
610         }).is_some()
611     }
612
613     ///
614     /// Retrieve recording @p recording_id and
615     /// write it to @p tx_buf in yencoded form.
616     pub fn get_recording(&mut self, recording_id: u16,
617                          tx_buf: &mut Buffer) -> Result<(), Error> {
618         if recording_id == 0 {
619             return Err(Error::NoSuchRecording);
620         }
621
622         if let Some(found_index) = self.sector_header_iter().find(|&index| {
623             let sector_header = &self.sector_header[index as usize];
624
625             sector_header.recording_id == recording_id &&
626             sector_header.starts_recording()
627         }) {
628             let mut filename = [b' '; 29];
629
630             filename[0..].copy_from_slice(b"gps-watch-recording-XXXXX.bin");
631
632             fmt_u32_pad(&mut filename[20..], recording_id as u32, 5, b'0');
633
634             let mut yenc = Yencode::new(tx_buf);
635
636             yenc.start(&filename);
637
638             let format_version = 1u8;
639             yenc.data(&[format_version]);
640
641             let mut next_sector = found_index as usize;
642
643             for _ in 0..NUM_SECTORS {
644                 let address = next_sector * SECTOR_SIZE;
645                 let mut buf = [0u8; SECTOR_SIZE];
646
647                 self.storage.read(address, &mut buf);
648
649                 // Skip flags and recording ID.
650                 yenc.data(&buf[4..]);
651
652                 next_sector += 1;
653                 next_sector &= NUM_SECTORS - 1;
654
655                 if !self.sector_header[next_sector].belongs_to(recording_id) {
656                     break;
657                 }
658             }
659
660             yenc.finish();
661
662             tx_buf.flush();
663
664             Ok(())
665         } else {
666             Err(Error::NoSuchRecording)
667         }
668     }
669 }