/*!
  \file
  \brief NMEA 形式のデータ

  \author Satofumi KAMIMURA

  $Id: NmeaData.cpp 775 2009-05-05 08:05:45Z satofumi $

  \todo ecef2enu(), blh2ecef() を別関数に置き換え、using namespace std: を適用する
*/

#include "NmeaData.h"
#include "split.h"
#include "str01.h"
#include <cstdlib>
#include <cmath>

using namespace qrk;

extern vector ecef2enu(vector dest, vector origin);
extern vector blh2ecef(double phi, double ramda, double height);


namespace
{
  bool checkSum(const std::string& line)
  {
    size_t n = line.size();
    if (n < 2) {
      return false;
    }

    unsigned char actual = 0;
    for (size_t i = 1; i < (n - 3); ++i) {
      actual ^= line[i];
    }

    unsigned char expected = strtol(&line.c_str()[n - 2], NULL, 16);

    return (actual == expected) ? true : false;
  }


  double calculateTude(const std::vector<std::string>& tokens,
                     int tude_index, char sign_ch)
  {
    size_t tude_position = tokens[tude_index].find('.') - 2;
    return (atof(tokens[tude_index].substr(0, tude_position).c_str()) +
            (atof(tokens[tude_index].substr(tude_position).c_str()) / 60.0)
            * ((tokens[tude_index + 1][0] == sign_ch) ? -1 : +1));
  }


  // DOP and Active Satellites
  void parseGpgsa(NmeaData* nmea,
                  const std::vector<std::string>& tokens, size_t n)
  {
    static_cast<void>(nmea);
    static_cast<void>(tokens);
    static_cast<void>(n);

    // !!! 1:mode 1
    // !!! 2:mode 2
    // !!! 以下略。実装しなくてよい
  }


  // GNSS Satellites in View
  void parseGpgsv(NmeaData* nmea,
                  const std::vector<std::string>& tokens, size_t n)
  {
    static_cast<void>(nmea);
    static_cast<void>(tokens);
    static_cast<void>(n);
    // !!!

    // !!! 個々の衛星位置なのか？ 省略
  }


  // Recommented Minimum Specific GNSS Data
  void parseGprmc(NmeaData* nmea,
                  const std::vector<std::string>& tokens, size_t n)
  {
    static_cast<void>(nmea);

    if (n != 13) {
      return;
    }
    if (tokens[2].compare("A")) {
      // Data Valid な情報のみを格納する
      return;
    }

    nmea->hour = atoi(tokens[1].substr(0, 2).c_str());
    nmea->minute = atoi(tokens[1].substr(2, 2).c_str());
    nmea->sec = atoi(tokens[1].substr(4, 2).c_str());

    // !!! 小数点以下の数値がない可能性は、考慮していない
    nmea->msec = atoi(tokens[1].substr(7, 3).c_str());
    //fprintf(stderr, "%02d:%02d:%02d.%03d\n", nmea->hour, nmea->minute, nmea->sec, nmea->msec);

    // 緯度、経度
    nmea->latitude = rad(calculateTude(tokens, 3, 'N'));
    nmea->longitude = rad(calculateTude(tokens, 5, 'E'));

    //printf("%f, %f\n", nmea->latitude.to_rad(), nmea->longitude.to_rad());
    //fflush(stdout);

    // !!! 7:Speed Over Ground [knots]
    // !!! 8:Course Over Ground [degrees]
    // !!! 9:Date(ddmmyy)
    // !!! 10:Magnetic Variation [degrees]
  }


  // Global Positioning System
  void parseGpgga(NmeaData* nmea,
                  const std::vector<std::string>& tokens, size_t n)
  {
    static_cast<void>(nmea);
    static_cast<void>(tokens);
    static_cast<void>(n);

    // !!! 緯度経度、衛星数

    // !!! 1:UTC (hh:mm:ss.sss)
    // !!! 2:Latitude
    // !!! 3:N/S
    // !!! 4:Longitude
    // !!! 5:E/W
    // !!! 6:mode
    // !!! 7:satellites
    // !!! 8:HDOP
    // !!! 9:MSL Altitude
    // !!! 10:meter
    // !!! 11:Geoid Sepalation
  }
}


void NmeaData::setLine(const std::string& line)
{
  if (line.empty() || (line[0] != '$') || (! checkSum(line))) {
    return;
  }

  fprintf(stderr, "%s\n", line.c_str());

  std::vector<std::string> tokens;
  size_t n = split(tokens, line, ",*", false);

  // データを格納情報に反映
  std::string& first_token = tokens[0];
  if (! first_token.compare("$GPGSA")) {
    parseGpgsa(this, tokens, n);

  } else if (! first_token.compare("$GPGSV")) {
    parseGpgsv(this, tokens, n);

  } else if (! first_token.compare("$GPRMC")) {
    parseGprmc(this, tokens, n);

  } else if (! first_token.compare("$GPGGA")) {
    parseGpgga(this, tokens, n);
  }
}


bool NmeaData::validPosition(void)
{
  return ((latitude.to_rad() == 0.0) &&
          (longitude.to_rad() == 0.0)) ? false : true;
}


void NmeaData::setOrigin(const Angle& lat, const Angle& lng,
                         double alt)
{
  origin_latitude_ = lat;
  origin_longitude_ = lng;
  origin_altitude_ = alt;
}


Position<long> NmeaData::position(const Angle& lat, const Angle& lng,
                                  double alt)
{
  vector origin_ecef = blh2ecef(origin_latitude_.to_rad(),
                                origin_longitude_.to_rad(),
                                origin_altitude_);

  vector point_ecef = blh2ecef(lat.to_rad(), lng.to_rad(), alt);
  vector enu = ecef2enu(point_ecef, origin_ecef);

  // !!! 適切な向きを返すようにする
  // !!! 高さも返せるようにする
  // !!! Position3D() を定義すべき

  return Position<long>(static_cast<long>(round(enu.a[0] * 1000)),
                        static_cast<long>(round(enu.a[1] * 1000)),
                        rad(0.0));
  //static_cast<long>(round(enu.a[2] * 1000)));
}
