CON
  #0
  IMU_COUNT
  IMU_MAG_X, IMU_MAG_Y, IMU_MAG_Z
  IMU_ACCEL_X, IMU_ACCEL_Y, IMU_ACCEL_Z
  IMU_PITCH, IMU_ROLL, IMU_YAW
  IMU_NUM_VARS

  #0
  GPS_COUNT
  GPS_HOUR, GPS_MINUTE, GPS_SECOND_E3
  GPS_LAT_DEGREES, GPS_LAT_MINUTES_E4
  GPS_LNG_DEGREES, GPS_LNG_MINUTES_E4
  GPS_VALID
  GPS_ALTITUDE_METERS_E1
  GPS_COURSE_DEGREES_E2
  GPS_SPEED_KMH_E2
  GPS_NUM_VARS

  GPS_BUF_SIZE = 80
  IMU_BUF_SIZE = IMU_NUM_VARS * 2


VAR
  long cog
  long stack[32]

  byte imu_buf[IMU_BUF_SIZE]
  byte gps_buf[GPS_BUF_SIZE]
  long gps_buf_len, imu_buf_len

  word imu_data[IMU_NUM_VARS]
  long imu_errors

  long gps_data[GPS_NUM_VARS]
  long gps_errors


OBJ
  imu: "FullDuplexSerial256"
  gps: "FullDuplexSerial256"


PUB start(gps_rx_pin, gps_tx_pin, imu_rx_pin, imu_tx_pin): okay | gps_ok, imu_ok
  stop
  gps_ok := gps.start(gps_rx_pin, gps_tx_pin, 0, 57600)
  imu_ok := imu.start(imu_rx_pin, imu_tx_pin, 0, 115200)
  okay := gps_ok and imu_ok and (cog := cognew(entry, @stack) + 1)
  if not okay
    stop


PUB stop
  if cog
    cogstop(cog~ - 1)
  gps.stop
  imu.stop


PUB get_gps_errors
  return gps_errors


PUB get_gps_data(array)
  longmove(array, @gps_data, GPS_NUM_VARS)
  

PUB get_imu_data(array)
  wordmove(array, @imu_data, IMU_NUM_VARS)


PUB get_imu_errors
  return imu_errors


PRI entry | ch
  imu_buf_len := -1
  gps_buf_len := 0

  repeat
    if (ch := imu.rxcheck) <> -1
      if imu_buf_len < 0
        if ch == "A"
          imu_buf_len := 0
        else
          ++imu_errors
      elseif imu_buf_len == IMU_BUF_SIZE
        if ch == "Z"
          wordmove(@imu_data, @imu_buf, IMU_NUM_VARS)
          imu_buf_len := -1
        else
          ++imu_errors
      else
        imu_buf[imu_buf_len++ ^ 1] := ch  ' byte swap

    if (ch := gps.rxcheck) <> -1
      if ch == 10 or ch == 13
        decode_gps
        gps_buf_len := 0
      elseif gps_buf_len < GPS_BUF_SIZE
        gps_buf[gps_buf_len++] := ch
      else
        ' ++gps_errors
        gps_buf_len := 0


PRI decode_gps | i, checksum
  if gps_buf_len < 6
    return

  if gps_buf[1] == "G" and gps_buf[2] == "P"
    if gps_buf[3] == "G" and gps_buf[4] == "G" and gps_buf[5] == "A"
      decode_gps_gpgga
    elseif gps_buf[3] == "V" and gps_buf[4] == "T" and gps_buf[5] == "G"
      decode_gps_gpvtg


PRI verify_gps_checksum | i, checksum
  if gps_buf_len < 6 or gps_buf[0] <> "$" or gps_buf[gps_buf_len - 3] <> "*"
    return false

  checksum := 0
  repeat i from 1 to gps_buf_len - 4
    checksum ^= gps_buf[i]

  if gps_buf[gps_buf_len - 2] <> hex_digits[checksum >> 4]
    return false
  elseif gps_buf[gps_buf_len - 1] <> hex_digits[checksum & $0F]
    return false
  else
    return true


PRI decode_gps_gpgga | p, latd, latm, lngd, lngm
  if not verify_gps_checksum
    ++gps_errors
    return

  p := 6

  if gps_buf[p+1] <> ","
    gps_data[GPS_HOUR] := 10*(gps_buf[p+1]-"0") + (gps_buf[p+2]-"0")
    gps_data[GPS_MINUTE] := 10*(gps_buf[p+3]-"0") + (gps_buf[p+4]-"0")
    gps_data[GPS_SECOND_E3] := 10000*(gps_buf[p+5]-"0") 
    gps_data[GPS_SECOND_E3] += 1000*(gps_buf[p+6]-"0")
    gps_data[GPS_SECOND_E3] += 100*(gps_buf[p+8]-"0")
    gps_data[GPS_SECOND_E3] += 10*(gps_buf[p+9]-"0") 
    gps_data[GPS_SECOND_E3] += (gps_buf[p+10]-"0")
    p += 10

  if gps_buf[p+2] <> ","
    latd := 10*(gps_buf[p+2]-"0") + (gps_buf[p+3]-"0")
    latm := 100000*(gps_buf[p+4]-"0")
    latm += 10000*(gps_buf[p+5]-"0")
    latm += 1000*(gps_buf[p+7]-"0")
    latm += 100*(gps_buf[p+8]-"0")
    latm += 10*(gps_buf[p+9]-"0")
    latm += (gps_buf[p+10]-"0")
    p += 9

  if gps_buf[p+3] == "S"
    -latd
  if gps_buf[p+3] <> ","
    p += 1

  if gps_buf[p+4] <> ","
    lngd := 100*(gps_buf[p+4]-"0") + 10*(gps_buf[p+5]-"0") + (gps_buf[p+6]-"0")
    lngm := 100000*(gps_buf[p+7]-"0")
    lngm += 10000*(gps_buf[p+8]-"0")
    lngm += 1000*(gps_buf[p+10]-"0")
    lngm += 100*(gps_buf[p+11]-"0")
    lngm += 10*(gps_buf[p+12]-"0")
    lngm += (gps_buf[p+13]-"0")
    p += 10

  if gps_buf[p+5] == "W"
    -lngd
  if gps_buf[p+5] <> ","
    p += 1

  gps_data[GPS_LAT_DEGREES] := latd
  gps_data[GPS_LAT_MINUTES_E4] := latm
  gps_data[GPS_LNG_DEGREES] := lngd
  gps_data[GPS_LNG_MINUTES_E4] := lngm

  if gps_buf[p+6] <> ","
    gps_data[GPS_VALID] := gps_buf[p+6]-"0"
    p += 1

  ' TODO altitude

  ++gps_data[GPS_COUNT]


PRI decode_gps_gpvtg
  if not verify_gps_checksum
    ++gps_errors
    return

  ' TODO course, speed


DAT
  hex_digits byte "0123456789ABCDEF"