#!/usr/bin/perl

# part of this code is from: https://github.com/doug1/psip-time/blob/master/psip-time.pl
# This module is free software. You can redistribute it and/or
# modify it under the terms of the Artistic License 2.0.
#
# 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.
#
# on Fedora, this needs the packages perl-Inline, perl-DateTime, and gcc
#
# example /etc/ntp.conf:
# server 127.127.28.0 minpoll 4 maxpoll 4
# fudge 127.127.28.0 time1 0.000 refid GPS stratum 0
#
# to run this program:
# ./ntp-shm </dev/gps0
# Adjust /dev/gps0 to match your device (see dmesg - can be "ttyUSB0").
# This program needs to run as root (shm permissions and realtime priority)

use IPC::SysV qw(IPC_PRIVATE IPC_RMID S_IRUSR S_IWUSR);
use Inline "C";
use DateTime;
use constant {
  NTP_SHMID => 0x4e545030
};

set_rr(32);

my $shmid = shmget(NTP_SHMID, 96, S_IRUSR | S_IWUSR);
die "shmget: $!" if ( $shmid < 0 );

# send the local vs remote time offset to ntp
sub ntp_message {
  my($local,$remote) = @_;

  my $remote_us = $remote->hires_epoch() - $remote->epoch();
  $remote_us = int($remote_us * 1000000);
  my $remote_s = $remote->epoch();

  my $nsamples = 0;
  my $valid = 1;
  my $precision = -13; # 2^-13 = 122 microseconds ~ 125us
  my $leap = 0;
  my $count = 0;
  my $mode = 0;

  my $format = "ll". "ql" . "l" . "ql" . "llll" . "l" . "llllllllll";
  my $message = pack( $format, 
		  $mode, $count,
                  $remote_s, $remote_us,
		  0,
                  $local->[0], $local->[1],
                  $leap,
		  $precision, $nsamples, $valid,
                  0,0,0,0,0,0,0,0,0,0,0);

  my $len = length($message);
  die "wrong message length" unless($len == 96);
  shmwrite($shmid, $message, 0, $len) || die("$!");
}

# parse GPS lock type (none, 2D, 3D) and satellite signal strength info
my($locktype,$satinfo);
sub parse_nmea {
  my($line) = @_;

  if($line =~ /^\$G.GSA,.,([0-3])/) { # GPGSA - GPS, GNGSA - GPS+Glonass, GLGSA - Glonass
    $locktype = $1;
  } elsif($line =~ /^\$G.GSV,[0-9]+,[0-9]+,[0-9]+,(.*)\*[0-9A-F]{2}/) { # GPGSV - GPS, GLGSV - Glonass
    my(@sats) = split(/,/,$1);
    for(my $i = 0; $i < @sats; $i += 4) {
      $satinfo .= "sat ".$sats[$i]."=".$sats[$i+3]." ";
    }
  } elsif($line =~ /^\$G.ZDA/) {
#    print "$locktype $satinfo\n";
    $locktype = $satinfo = undef;
  }
}

my($local_timestamp,$line);
while(my($data,$ts_s,$ts_us) = timeread(fileno(STDIN))) {
  if(defined($ts_s)) { # we found the start of a "TS" string, keep the timestamp and start buffering
    $local_timestamp = [$ts_s, $ts_us];
    $line = $data;
  } elsif($local_timestamp) {
    $line .= $data; # buffer till we get the whole "TS" string
    if($line =~ /^TS([0-9]{4})-([0-9]{2})-([0-9]{2}) @ ([0-9]{2}):([0-9]{2}):([0-9]{2})\(\+0\.([0-9]{6})\) ([AP])M\*[0-9A-F]{2}\r\n(.*)/) {
      my($remote_year,$remote_month,$remote_day,$remote_hour,$remote_minute,$remote_second,$remote_sub_s,$remote_ampm,$newline) = ($1,$2,$3,$4,$5,$6,$7,$8,$9);
      if($remote_ampm eq "P") {
        $remote_hour += 12;
      }
      $remote_sub_s += 86; # add 86us for 10 bits at 115200 baud
      my $remote = DateTime->new(
          year => $remote_year,
          month => $remote_month,
          day => $remote_day,
          hour => $remote_hour,
          minute => $remote_minute,
          second => $remote_second,
          nanosecond => ($remote_sub_s * 1000),
          time_zone => "UTC"
          );
      $line =~ s/\r\n.*//s;
      printf("%0.6f %s\n", $remote->hires_epoch() - ($local_timestamp->[0] + $local_timestamp->[1] / 1000000), $line);
      ntp_message($local_timestamp, $remote);
      $line = $newline;
      $local_timestamp = undef;
    } elsif($line =~ /^TS\*[0-9A-F]{2}\r\n(.*)/) { # we had a "TS" string, but the GPS has no lock
      $line = $1;
      print "TS - no sync\n";
      $local_timestamp = undef;
    }
  } else {
    $line .= $data;
    if($line =~ /^(.*?)\r\n(.*)/s) { # we found a full line
      my $nmea = $1;
      if(length($nmea)) {
        parse_nmea($nmea);
      }
      $line = $2;
    }
  }
}

__END__
__C__
#include <sched.h>
#include <unistd.h>
#include <sys/time.h>
#include <string.h>

int set_rr(int priority) {
  struct sched_param param;

  param.sched_priority = priority;
  return sched_setscheduler(0, SCHED_RR, &param); // set realtime priority (as far as realtime goes on a general OS)
}

// returns: either array (buffer) or array (buffer, seconds, useconds)
//  second return type is for the timestamp of the '$' character in the line "$TS..."
void timeread(int fd) {
  char buf[512];
  int status;
  struct timeval now;
  static int found_newline = 0;
  static struct timeval last_dollar;
  Inline_Stack_Vars;

  ssize_t len = read(fd, buf, sizeof(buf));
  gettimeofday(&now,NULL);
  if(len <= 0) { // error or device gone, return an empty list
    Inline_Stack_Reset;
    Inline_Stack_Done;
    return;
  }

  Inline_Stack_Reset;
  Inline_Stack_Push(sv_2mortal(newSVpvn(buf, len)));

  if(found_newline) {
    if(len == 1 && buf[0] == '$' && found_newline == 1) { // only got one character '$', so we know the timestamp error is bounded at 8ms
      found_newline = 2;
      last_dollar.tv_sec = now.tv_sec;
      last_dollar.tv_usec = now.tv_usec;
    } else {
      if(buf[0] == 'T' && found_newline == 2) { // the line is "$T" so far, it's the timestamp line so return the timestamp of the '$' character
	Inline_Stack_Push(sv_2mortal(newSViv(last_dollar.tv_sec)));
	Inline_Stack_Push(sv_2mortal(newSViv(last_dollar.tv_usec)));
      }
      found_newline = 0;
    }
  }

  char *newline = strchr(buf, '\n');
  if(newline != NULL && newline == (buf+len-1)) {
    found_newline = 1; // last character of the buffer was a '\n'? start looking for the '$'
  }

  Inline_Stack_Done;
}