#!/usr/bin/perl

use Data::Dumper;
use IO::Socket;
use Math::Trig;
use FindBin;                    # find the script's directory
use lib $FindBin::Bin;          # add that directory to the library path
use xPL::common;


################################################################################
# constants
#
$vendor_id = 'dspc';            # from xplproject.org
$device_id = 'phoneDst';        # max 8 chars
$class_id = 'distance';         # max 8 chars

$separator = '-' x 80;
$indent = ' ' x 2;

#-------------------------------------------------------------------------------
# global variables
#
my $locations_ref = {
  'home' => '46.094341, 7.217851',
  'work' => '46.240586, 7.358861'
};
my %configuration;
$configuration{'gpsPort'} = '11123';
$configuration{'locations'} = $locations_ref;
$configuration{'logFileSpec'} = '/tmp/phoneDistance.log';
$configuration{'logFileLineNb'} = 1E3;


################################################################################
# Input arguments
#
use Getopt::Std;
my %opts;
getopts('hvp:n:t:w:g:l:', \%opts);

die("\n".
    "Usage: $0 [options]\n".
    "\n".
    "Options:\n".
    "${indent}-h        display this help message\n".
    "${indent}-v        verbose\n".
    "${indent}-p port   the base UDP port\n".
    "${indent}-n id     the instance id (max. 12 chars)\n".
    "${indent}-t mins   the heartbeat interval in minutes\n".
    "${indent}-w secs   the startup sleep interval\n".
    "${indent}-g port   the port receiving NMEA location messages\n".
    "${indent}-l file   the log file name\n".
    "\n".
    "Calculates the distance from the mobile phone to different locations.\n".
    "\n".
    "More information with: perldoc $0\n".
    "\n".
    ""
   ) if ($opts{h});
my $verbose = $opts{v};
my $client_base_port = $opts{'p'} || 50000;
my $instance_id = $opts{'n'} || xpl_build_automatic_instance_id;
my $heartbeat_interval = $opts{'t'} || 5;
my $startup_sleep_time = $opts{'w'} || 0;

$configuration{'gpsPort'} = $opts{'g'} || $configuration{'gpsPort'};
$configuration{'logFileSpec'} = $opts{'l'} || $configuration{'logFileSpec'};
if ($configuration{'logFileSpec'} eq '/dev/null') {
  $configuration{'logFileSpec'} = ''
}


################################################################################
# Internal functions
#

#-------------------------------------------------------------------------------
# start an UDP server
#
sub start_gps_server {
  my ($port) = @_;
                                                             # create new socket
  my $server = IO::Socket::INET->new(
    LocalPort => $port,
    Proto     => "udp"
  ) or die "Couldn't start the GPS UDP server on port $port : $@\n";

  return($server)
}

#-------------------------------------------------------------------------------
# get an NMEA message on the UDP server
#
sub get_gps_message {
  my ($socket, $timeout) = @_;
                                                    # read message from UDP port
  my $message;
  eval {
    local $SIG{ALRM} = sub { die "starting alarm time out" };
    alarm $timeout;
    recv($socket, $message, 1500, 0);
    chomp($message);
    alarm 0;
    1;  # return value from eval on normalcy
  } or  $message = '';

  return($message)
}

#-------------------------------------------------------------------------------
# transform NMEA latitude/longitue to angle and decimals
#
sub nema_to_decimals {
  my ($angle) = @_;
                                                              # split into parts
  my ($degrees, $minutes_decimal) = split(/\./, $angle);
  if (length($degrees) == 4) {
    $degrees = '0' . $degrees;
  }
  $degrees = join('.', $degrees =~ /...?/g);
  ($degrees, my $minutes) = split(/\./, $degrees);
                                                           # bring back together
  $angle = $degrees + ($minutes+$minutes_decimal/1000)/60;

  return($angle)
}

#-------------------------------------------------------------------------------
# get information from NMEA message
#
sub get_coordinates {
  my ($message) = @_;
                                                   # split message into elements
  my @message_items = split(/\,/, $message);
  my $time        = $message_items[1];
  my $latitude    = $message_items[2];
  my $north_south = $message_items[3];
  my $longitude   = $message_items[4];
  my $east_west   = $message_items[5];
  my $altitude    = $message_items[9];
                                                             # reformat elements
  $time = join(':', $time =~ /../g);
  $latitude = nema_to_decimals($latitude);
  if ($north_south eq 'S') { $latitude = -$latitude}
  $longitude = nema_to_decimals($longitude);
  if ($north_south eq 'W') { $longitude = -$longitude}

  return($time, $latitude, $longitude, $altitude)
}

#-------------------------------------------------------------------------------
# calculate distances between two coordinates
#
sub calculateDistance {
  my ($latitude1, $longitude1, $latitude2, $longitude2) = @_;
  my $earth_radius = 6371E3;
                                                 # latitudes, degrees to radians
  my $lat1 = $latitude1 * 2*pi / 360;
  my $lat2 = $latitude2 * 2*pi / 360;
                                               # differences, degrees to radians
  my $d_lat = ($latitude2 - $latitude1) * 2*pi / 360;
  my $d_long = ($longitude2 -$longitude1) * 2*pi / 360;
                                                             # haversine formula
  my $haversine_a = (sin($d_lat/2))**2 + cos($lat1) * cos($lat2) * (sin($d_long/2))**2;
  my $haversine_c = 2 * atan2(sqrt($haversine_a), sqrt(1-$haversine_a));
  my $distance = int($earth_radius * $haversine_c + 0.5);
#print "distance: $distance\n";

  return($distance)
}

#-------------------------------------------------------------------------------
# calculate distances from one point to all others
#
sub calculateDistances {
  my ($latitude, $longitude, $locations_ref) = @_;
  my %distances;
                                                             # loop on distances
  my %locations = %$locations_ref;
  foreach my $location (keys(%locations)) {
    my $coordinates = $locations{$location};
    my ($location_latitude, $location_longitude) = split(/\,\s*/, $coordinates);
#print "$location: ($location_latitude, $location_longitude)\n";
    $distances{$location} = calculateDistance(
      $latitude, $longitude,
      $location_latitude, $location_longitude
    );
  }

  return(%distances)
}


################################################################################
# Catch control-C interrupt
#
$SIG{INT} = sub{ $xpl_end++ };


################################################################################
# Main script
#
sleep($startup_sleep_time);
                                                                # xPL parameters
my $xpl_id = xpl_build_id($vendor_id, $device_id, $instance_id);
my $xpl_ip = xpl_find_ip;
                                                             # create xPL socket
my ($client_port, $xpl_socket) = xpl_open_socket($xpl_port, $client_base_port);
                                                             # create GPS socket
my $gps_server = start_gps_server($configuration{'gpsPort'});
                                                    # display working parameters
if ($verbose == 1) {
  system("clear");
  print("$separator\n");
  print("Listening for GPS data on port \"$configuration{'gpsPort'}\".\n");
  print($indent . "class id   : $class_id\n");
  print($indent . "instance id: $instance_id\n");
	print("\n");
}


#-------------------------------------------------------------------------------
# Main loop
#
my $timeout = 1;
my $last_heartbeat_time = 0;

while ( (defined($xpl_socket)) && ($xpl_end == 0) ) {
                                                 # check time and send heartbeat
  $last_heartbeat_time = xpl_send_heartbeat(
    $xpl_socket, $xpl_id, $xpl_ip, $client_port,
    $heartbeat_interval, $last_heartbeat_time
  );
                                              # get GPS UDP message with timeout
  my $gps_message = get_gps_message($gps_server, $timeout);
  if ($gps_message ne '') {
#print "$gps_message\n";
                                                    # get position and distances
    my ($time, $latitude, $longitude, $altitude) = get_coordinates($gps_message);
#print $indent . "time: $time\n";
#print $indent . "latitude: $latitude\n";
#print $indent . "longitude: $longitude\n";
#print $indent . "altitude: $altitude\n";
    my %distances = calculateDistances(
      $latitude, $longitude, $configuration{'locations'}
    );
                                                              # send xPL message
    $latitude = sprintf('%.5f', $latitude);
    $longitude = sprintf('%.5f', $longitude);
    my %status = (
      'time'      => $time,
      'latitude'  => $latitude,
      'longitude' => $longitude,
      'altitude'  => $altitude,
    );
    if ($verbose == 1) {
      print "\n";
      print "at $time, location is ($latitude, $longitude), altutude is $altitude\n";
    }
    foreach my $location (keys(%distances)) {
      $status{$location} = $distances{$location};
      if ($verbose == 1) {
        my $distance_km = int($distances{$location} / 1E3 + 0.5);
        print "${indent}distance to $location is $distance_km km\n";
      }
    }
    xpl_send_message(
      $xpl_socket, $xpl_port,
      'xpl-trig', $xpl_id, '*', "$class_id.status",
      %status
    );
                                                    # log position and distances
    if ($configuration{'logFileSpec'} ne '') {
      my $log_line = "$time lat=$latitude long=$longitude alt=$altitude";
      foreach my $location (keys(%distances)) {
        $log_line .= " $location=$distances{$location}";
      }
#print "$log_line\n";
      open(LOG_FILE, "<$configuration{'logFileSpec'}");
      my @log_file_content = <LOG_FILE>;
      close(LOG_FILE);
      my $log_file_length = scalar(@log_file_content);
      open(LOG_FILE, ">$configuration{'logFileSpec'}");
      if ($log_file_length < $configuration{'logFileLineNb'}) {
        print LOG_FILE @log_file_content;
      }
      else {
        print LOG_FILE @log_file_content[
          $log_file_length-$configuration{'logFileLineNb'}+1 ..
          $log_file_length-1
        ];
      }
      print LOG_FILE "$log_line\n";
      close(LOG_FILE);
    }
  }
}

xpl_disconnect($xpl_socket, $xpl_id, $xpl_ip, $client_port);


################################################################################
# Documentation (access it with: perldoc <scriptname>)
#
__END__

=head1 NAME

xpl-phoneDistance.pl - Calculates the distance from the mobile phone to different locations

=head1 SYNOPSIS

xpl-phoneDistance.pl [options]

=head1 DESCRIPTION

This xPL client waits for NMEA GGA sentence on a specified UDP port.
It decodes the inlying location information and sends it via xPL.
It also calculates the distance between the received location and a set of
specified locations.
This is also sent within the same xPL message.

The script also logs the location in order to allow a graphic presentation.

=head1 OPTIONS

=over 8

=item B<-h>

Display a help message.

=item B<-v>

Be verbose.

=item B<-p port>

Specify the base port from which the client searches for a free xPL port.
If not specified, the client will take a default value.

=item B<-n id>

Specify the instance id (name).
The id is limited to 12 characters.
If not specified, it is constructed from the host name.

=item B<-t mins>

Specify the number of minutes between two heartbeat messages.

=item B<-w secs>

Specify the number of seconds before sending the first heartbeat.
This allows to start the client after the hub,
thus eliminating an prospective startup delay of one heartbeat interval.

=item B<-g port>

Specify UDP port receiving NMEA messages.

=item B<-l file>

Specify the name of the log file.

=back

=head1 TEST

Make sure you have an C<xpl-hub> running on the machine.

Start the distance calculator:
C<xpl-phoneDistance.pl -vn iPhone>.

Start the mobile phone application which sends the GPS data to the UDP port.

Start C<xpl-monitor -v> in another terminal window.

=head1 AUTHOR

Francois Corthay, DSPC

=head1 VERSION

1.0, 2015

=cut