#!/bin/bash

endpoints="192.168.111.200"
filename="/home/llh/pingStats/pingStats.json"
downtime=1
gpstopic="/rossettiModem/dji_sdk/gps_position"

echo "pinging: $endpoints"
echo "save to: $filename"
echo "gps topic: $gpstopic"
echo " "

# enable ros commands
source /opt/ros/noetic/setup.bash
source /home/larkin/catkin_ws/devel/setup.bash

ROS_IP=192.168.111.202
ROS_MASTER_URI=http://192.168.111.200:11311

echo ping "${endpoints} every ${downtime:=1} sec, saving to ${filename}"
# for endpoints in "${endpoints[@]}"; do
# done
while true; do

  rawData=$(ping -c 3 -q -w 3 ${endpoints:=localhost})
  newPacketData=$(echo "$rawData" | grep -oP '\d+(?=% packet loss)')
  newLatencyData=$(echo "$rawData" | tail -n 1)

  OLDIFS=$IFS
  IFS=" "
  read bit1 bit2 bit3 bit4 bit5 <<< "$newLatencyData"
  IFS="/"
  read num1 num2 num3 num4 <<< "$bit4"
  IFS=$OLDIFS

  echo "------------------"
  if [[ -z $num4 ]]; then
    # no network data, or all packets lost
    num1="0"
    num2="0"
    num3="0"
    num4="0"
  fi

  # debugging help
  # echo "------------------"
  # echo $num1
  # echo $num2
  # echo $num3
  # echo $num4
  # echo "------------------"
  # echo "$rawData"
  # echo "------------------"
  # echo "$newPacketData"
  # echo "------------------"
  # echo "$newLatencyData"
  # exit

  if test -f "$filename"; then
    false
  else
    touch "$filename"
    echo "{}" > "$filename"
  fi

  # get gps data if present (timeout 5s)
  gpsData=$(timeout 5 rostopic echo -n 1 $gpstopic | grep "latitude:\|longitude:\|altitude:")
  gpsData=$(echo $gpsData | tr '\n' ' ')
  OLDIFS=$IFS
  IFS=" "
  read trash1 latitude trash2 longitude trash3 altitude <<< "$gpsData"
  IFS=$OLDIFS

  # ping can return standard deviation and gps data may not be available
  if [[ -z "$latitude" ]]; then
    # echo "gps data is not there"
    jq --arg timedate $(date +"%H:%M_%F") --arg target $endpoints --argjson packetLoss $newPacketData --argjson rttMin $num1 --argjson rttAvg $num2 --argjson rttMax $num3 --argjson stdDev $num4 '.[$target] += [{"min":$rttMin,"avg":$rttAvg,"max":$rttMax,"std":$stdDev,"timedate":$timedate,"packetLoss":$packetLoss}]' "$filename" > "$filename.tmp" && mv "$filename.tmp" "$filename"
  else
    echo "gps data IS there"
    echo $latitude
    echo $longitude
    echo $altitude
    jq --arg timedate $(date +"%H:%M_%F") --arg target $endpoints --argjson altitude $altitude --argjson longitude $longitude --argjson latitude $latitude --argjson packetLoss $newPacketData --argjson rttMin $num1 --argjson rttAvg $num2 --argjson rttMax $num3 --argjson stdDev $num4 '.[$target] += [{"lat":$latitude,"long":$longitude,"alt":$altitude,"min":$rttMin,"avg":$rttAvg,"max":$rttMax,"std":$stdDev,"timedate":$timedate,"packetLoss":$packetLoss}]' "$filename" > "$filename.tmp" && mv "$filename.tmp" "$filename"
  fi
  sleep ${downtime:=1}
done

if test -f "$filename.tmp"; then
  rm "$filename.tmp"
fi
