/*
 * File    : rover_calc.c
 * Author  : Makoto Yagi
 * Version : 1.4.0
 * Date    : 2025-12-11
 *
 * F9P Moving-Base Rover
 * - UBX-NAV-RELPOSNED receiver (UART)
 * - NMEA(GGA) receiver (USB)
 * - Baseline sender (UDP)
 * - Log writer with Lat / Lon / HDOP
 * - 1-hour log rotation (filename has date+time)
 */

#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <errno.h>
#include <fcntl.h>
#include <arpa/inet.h>
#include <sys/socket.h>
#include <sys/select.h>
#include <netinet/in.h>
#include <time.h>
#include <math.h>

#define UBX_SYNC1 0xB5
#define UBX_SYNC2 0x62
#define CLS_NAV 0x01
#define ID_RELPOS 0x3C
#define PAYLOAD_LEN 64

#define UDP_PORT_DEFAULT 60000
#define UDP_ADDR_DEFAULT "192.168.100.255"

#define NMEA_BUF_SIZE 512

/* --- ログローテーション用の状態 --- */
static FILE *log_fp = NULL;
static int log_day = -1;  /* tm_yday */
static int log_hour = -1; /* tm_hour */
static char log_fullpath[512];

/* ========================================= */
/* 共通ユーティリティ                        */
/* ========================================= */

static int read_exact(int fd, uint8_t *p, size_t n)
{
    size_t tot = 0;
    while (tot < n)
    {
        ssize_t r = read(fd, p + tot, n - tot);

        if (r == 0)
            return -1;

        if (r < 0)
        {
            if (errno == EAGAIN || errno == EWOULDBLOCK)
            {
                usleep(1000);
                continue;
            }
            return -1;
        }

        tot += (size_t)r;
    }
    return 0;
}

static void now_str(char *s, size_t sz)
{
    time_t t = time(NULL);
    struct tm tmv;
    localtime_r(&t, &tmv);
    strftime(s, sz, "%Y-%m-%d %H:%M:%S", &tmv);
}

/* ========================================= */
/* ログローテーション                        */
/* ========================================= */

/* 必要ならログを新しいファイルに切り替える（1時間単位） */
static int rotate_log_if_needed(const char *logdir)
{
    time_t t = time(NULL);
    struct tm tmv;
    localtime_r(&t, &tmv);

    /* まだ開いていて、かつ同じ日＆同じ時間ならそのまま */
    if (log_fp != NULL &&
        tmv.tm_yday == log_day &&
        tmv.tm_hour == log_hour)
    {
        return 0;
    }

    /* ここに来たら新しいファイルが必要（初回 or 時間が変わった） */
    if (log_fp)
    {
        fclose(log_fp);
        log_fp = NULL;
    }

    /* ファイル名: rover_YYYYMMDD_HHMMSS.log */
    snprintf(log_fullpath, sizeof(log_fullpath),
             "%s/rover_%04d%02d%02d_%02d%02d%02d.log",
             logdir,
             tmv.tm_year + 1900,
             tmv.tm_mon + 1,
             tmv.tm_mday,
             tmv.tm_hour,
             tmv.tm_min,
             tmv.tm_sec);
    log_fullpath[sizeof(log_fullpath) - 1] = '\0';

    log_fp = fopen(log_fullpath, "a");
    if (!log_fp)
    {
        perror("fopen log");
        return -1;
    }

    log_day = tmv.tm_yday;
    log_hour = tmv.tm_hour;

    fprintf(stderr, "LOG ROTATED: %s\n", log_fullpath);
    return 0;
}

/* ========================================= */
/* UBX RELPOSNED リーダ（read_exact版）      */
/* ========================================= */

static int read_relposned(int fd,
                          double *out_n,
                          double *out_e,
                          double *out_d,
                          int *out_carrSoln)
{
    uint8_t b;

    /* SYNC1 */
    for (;;)
    {
        if (read_exact(fd, &b, 1) < 0)
            return 0;
        if (b == UBX_SYNC1)
            break;
    }

    /* SYNC2 */
    if (read_exact(fd, &b, 1) < 0)
        return 0;
    if (b != UBX_SYNC2)
        return 0;

    uint8_t hdr[4];
    if (read_exact(fd, hdr, 4) < 0)
        return 0;

    uint8_t cls = hdr[0];
    uint8_t id = hdr[1];
    uint16_t len = (uint16_t)hdr[2] | ((uint16_t)hdr[3] << 8);

    if (cls != CLS_NAV || id != ID_RELPOS || len != PAYLOAD_LEN)
    {
        /* このフレームは興味ないのでスキップ */
        uint8_t dummy[256];
        size_t remain = len + 2; /* payload + checksum */
        while (remain > 0)
        {
            size_t chunk = (remain > sizeof(dummy)) ? sizeof(dummy) : remain;
            if (read_exact(fd, dummy, chunk) < 0)
                break;
            remain -= chunk;
        }
        return 0;
    }

    uint8_t payload[PAYLOAD_LEN];
    if (read_exact(fd, payload, PAYLOAD_LEN) < 0)
        return 0;

    uint8_t ck_in[2];
    if (read_exact(fd, ck_in, 2) < 0)
        return 0;

    /* 必要ならチェックサム計算をここでしても良い（省略可） */

    int32_t relN_cm =
        (int32_t)payload[28] |
        ((int32_t)payload[29] << 8) |
        ((int32_t)payload[30] << 16) |
        ((int32_t)payload[31] << 24);

    int32_t relE_cm =
        (int32_t)payload[32] |
        ((int32_t)payload[33] << 8) |
        ((int32_t)payload[34] << 16) |
        ((int32_t)payload[35] << 24);

    int32_t relD_cm =
        (int32_t)payload[36] |
        ((int32_t)payload[37] << 8) |
        ((int32_t)payload[38] << 16) |
        ((int32_t)payload[39] << 24);

    int8_t hpN = (int8_t)payload[40];
    int8_t hpE = (int8_t)payload[41];
    int8_t hpD = (int8_t)payload[42];

    uint32_t flags =
        (uint32_t)payload[52] |
        ((uint32_t)payload[53] << 8) |
        ((uint32_t)payload[54] << 16) |
        ((uint32_t)payload[55] << 24);

    int carr = (flags >> 3) & 0x03; /* 0:none 1:float 2:fix */

    double N = relN_cm * 0.01 + hpN * 0.0001;
    double E = relE_cm * 0.01 + hpE * 0.0001;
    double D = relD_cm * 0.01 + hpD * 0.0001;

    *out_n = N;
    *out_e = E;
    *out_d = D;
    *out_carrSoln = carr;

    return 1;
}

/* ========================================= */
/* NMEA(GGA) 処理                            */
/* ========================================= */

static double nmea_deg(const char *s)
{
    if (!s || !*s)
        return NAN;
    double v = atof(s);
    int d = (int)(v / 100.0);
    double m = v - d * 100.0;
    return d + m / 60.0;
}

static void parse_gga_line(const char *line,
                           double *lat, double *lon, double *hdop)
{
    char buf[NMEA_BUF_SIZE];
    strncpy(buf, line, sizeof(buf) - 1);
    buf[sizeof(buf) - 1] = '\0';

    char *save = NULL;
    char *tok;
    int field = 0;

    char *lat_s = NULL, *ns = NULL;
    char *lon_s = NULL, *ew = NULL;
    char *fix_s = NULL, *hd_s = NULL;

    for (tok = strtok_r(buf, ",", &save);
         tok;
         tok = strtok_r(NULL, ",", &save))
    {
        switch (field)
        {
        case 2:
            lat_s = tok;
            break;
        case 3:
            ns = tok;
            break;
        case 4:
            lon_s = tok;
            break;
        case 5:
            ew = tok;
            break;
        case 6:
            fix_s = tok;
            break;
        case 8:
            hd_s = tok;
            break;
        default:
            break;
        }
        field++;
    }

    if (!lat_s || !ns || !lon_s || !ew || !fix_s || !hd_s)
        return;

    if (atoi(fix_s) <= 0)
        return;

    double la = nmea_deg(lat_s);
    double lo = nmea_deg(lon_s);

    if (*ns == 'S' || *ns == 's')
        la = -la;
    if (*ew == 'W' || *ew == 'w')
        lo = -lo;

    if (lat)
        *lat = la;
    if (lon)
        *lon = lo;
    if (hdop)
        *hdop = atof(hd_s);
}

static int nmea_read_line(int fd, char *out, size_t outsz)
{
    static char buf[NMEA_BUF_SIZE];
    static size_t n = 0;

    for (;;)
    {
        char c;
        ssize_t r = read(fd, &c, 1);
        if (r < 0)
        {
            if (errno == EAGAIN || errno == EWOULDBLOCK)
                return 0;
            return -1;
        }
        if (r == 0)
            return 0;

        if (c == '\n')
        {
            buf[n] = '\0';
            strncpy(out, buf, outsz - 1);
            out[outsz - 1] = '\0';
            n = 0;
            return 1;
        }
        if (c != '\r' && n < sizeof(buf) - 1)
        {
            buf[n++] = c;
        }
    }
}

/* ========================================= */
/* main                                      */
/* ========================================= */

int main(int argc, char *argv[])
{
    const char *dev_ubx = "/dev/ttyAMA0";
    const char *dev_nmea = "/dev/ttyACM0";
    const char *udp_addr = UDP_ADDR_DEFAULT;
    int udp_port = UDP_PORT_DEFAULT;

    /* ログディレクトリ: $HOME/DATA/RES */
    char logdir[256];
    const char *home = getenv("HOME");
    if (home && *home)
        snprintf(logdir, sizeof(logdir), "%s/DATA/RES", home);
    else
        snprintf(logdir, sizeof(logdir), "./"); /* HOMEなければカレント */

    logdir[sizeof(logdir) - 1] = '\0';

    if (argc >= 2)
        dev_ubx = argv[1];
    if (argc >= 3)
        dev_nmea = argv[2];
    if (argc >= 4)
        udp_addr = argv[3];
    if (argc >= 5)
        udp_port = atoi(argv[4]);

    fprintf(stderr,
            "rover_calc.c Version 1.4.0 (1-hour log rotation)\n"
            " UBX  : %s\n"
            " NMEA : %s\n"
            " UDP  : %s:%d\n"
            " LOGD : %s\n",
            dev_ubx, dev_nmea, udp_addr, udp_port, logdir);

    int fd_ubx = open(dev_ubx, O_RDONLY | O_NOCTTY);
    if (fd_ubx < 0)
    {
        perror("open UBX");
        return 1;
    }

    int fd_nmea = open(dev_nmea, O_RDONLY | O_NOCTTY);
    if (fd_nmea < 0)
    {
        perror("open NMEA");
        close(fd_ubx);
        return 1;
    }

    int sock = socket(AF_INET, SOCK_DGRAM, 0);
    if (sock < 0)
    {
        perror("socket");
        return 1;
    }

    int yes = 1;
    setsockopt(sock, SOL_SOCKET, SO_BROADCAST, &yes, sizeof(yes));

    struct sockaddr_in addr;
    memset(&addr, 0, sizeof(addr));
    addr.sin_family = AF_INET;
    addr.sin_port = htons(udp_port);
    addr.sin_addr.s_addr = inet_addr(udp_addr);

    double lat = NAN, lon = NAN, hdop = NAN;

    for (;;)
    {
        fd_set fds;
        FD_ZERO(&fds);
        FD_SET(fd_ubx, &fds);
        FD_SET(fd_nmea, &fds);
        int maxfd = (fd_ubx > fd_nmea) ? fd_ubx : fd_nmea;

        struct timeval tv = {1, 0};
        int r = select(maxfd + 1, &fds, NULL, NULL, &tv);
        if (r < 0)
        {
            if (errno == EINTR)
                continue;
            perror("select");
            break;
        }

        /* NMEA(GGA) */
        if (FD_ISSET(fd_nmea, &fds))
        {
            char line[NMEA_BUF_SIZE];
            int lr = nmea_read_line(fd_nmea, line, sizeof(line));
            if (lr == 1)
            {
                if (strncmp(line, "$GPGGA", 6) == 0 ||
                    strncmp(line, "$GNGGA", 6) == 0)
                {
                    parse_gga_line(line, &lat, &lon, &hdop);
                }
            }
        }

        /* RELPOSNED */
        if (FD_ISSET(fd_ubx, &fds))
        {
            double N, E, D;
            int carr;
            int ok = read_relposned(fd_ubx, &N, &E, &D, &carr);
            if (ok == 1 && carr >= 1)
            {
                char ts[32];
                now_str(ts, sizeof(ts));

                char msg[256];
                snprintf(msg, sizeof(msg),
                         "%s, %d, %.4f, %.4f, %.4f, %.9f, %.9f, %.2f",
                         ts, carr, N, E, D,
                         isnan(lat) ? 0.0 : lat,
                         isnan(lon) ? 0.0 : lon,
                         isnan(hdop) ? 0.0 : hdop);

                /* UDP */
                sendto(sock, msg, strlen(msg), 0,
                       (struct sockaddr *)&addr, sizeof(addr));

                /* ログファイルを必要ならローテーション */
                if (rotate_log_if_needed(logdir) == 0 && log_fp)
                {
                    fprintf(log_fp, "%s\n", msg);
                    fflush(log_fp);
                }

                /* stderr にも一応出しておく */
                fprintf(stderr, "LOG: %s\n", msg);
            }
        }
    }

    if (log_fp)
        fclose(log_fp);
    close(sock);
    close(fd_ubx);
    close(fd_nmea);
    return 0;
}

./rover_calc /dev/ttyAMA0 /dev/ttyACM0 A

2025-12-11 14:43:20, 1, 0.0100, 11602012.2500, 1.0300, 35.680756500, 139.559280000, 0.82

#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <unistd.h>
#include <fcntl.h>
#include <termios.h>
#include <string.h>
#include <errno.h>

#define UBX_SYNC1 0xB5
#define UBX_SYNC2 0x62
#define CLS_NAV   0x01
#define ID_RELPOS 0x3C
#define PAYLOAD_LEN 64

// ----- シリアル設定 -----
int open_serial(const char *dev, speed_t baud)
{
    int fd = open(dev, O_RDONLY | O_NOCTTY);
    if (fd < 0) {
        perror("open_serial");
        return -1;
    }

    struct termios tio;
    if (tcgetattr(fd, &tio) < 0) {
        perror("tcgetattr");
        close(fd);
        return -1;
    }

    cfmakeraw(&tio);              // rawモード

    cfsetispeed(&tio, baud);
    cfsetospeed(&tio, baud);

    tio.c_cflag |=  (CLOCAL | CREAD);
    tio.c_cflag &= ~(PARENB | CSTOPB | CSIZE);
    tio.c_cflag |=  CS8;          // 8N1

    if (tcsetattr(fd, TCSANOW, &tio) < 0) {
        perror("tcsetattr");
        close(fd);
        return -1;
    }

    return fd;
}

// ----- 読み取りユーティリティ -----
static uint8_t get_byte(int fd)
{
    uint8_t b;
    ssize_t n;

    while (1) {
        n = read(fd, &b, 1);
        if (n == 1) return b;
        if (n < 0) {
            if (errno == EINTR) continue;
            perror("read");
            exit(1);
        }
        // n == 0: EOF 相当だけどシリアルなので基本来ない
    }
}

static uint16_t get_u2(int fd)
{
    uint8_t b0 = get_byte(fd);
    uint8_t b1 = get_byte(fd);
    return (uint16_t)(b0 | (b1 << 8));
}

// ----- main -----
int main(int argc, char *argv[])
{
    const char *dev = "/dev/ttyAMA0";
    if (argc >= 2) {
        dev = argv[1];    // 引数でデバイス指定可能
    }

    int fd = open_serial(dev, B115200);
    if (fd < 0) {
        fprintf(stderr, "Failed to open serial: %s\n", dev);
        return 1;
    }

    while (1) {
        // UBX同期
        uint8_t b;
        do {
            b = get_byte(fd);
        } while (b != UBX_SYNC1);

        if (get_byte(fd) != UBX_SYNC2) continue;

        uint8_t cls = get_byte(fd);
        uint8_t id  = get_byte(fd);
        uint16_t len = get_u2(fd);

        if (!(cls == CLS_NAV && id == ID_RELPOS && len == PAYLOAD_LEN)) {
            // payload + checksum を読み捨て
            for (uint16_t i = 0; i < len + 2; i++) {
                get_byte(fd);
            }
            continue;
        }

        uint8_t buf[PAYLOAD_LEN];
        for (int i = 0; i < PAYLOAD_LEN; i++) {
            buf[i] = get_byte(fd);
        }

        // チェックサム捨て
        get_byte(fd);
        get_byte(fd);

        // cm成分
        int32_t relPosN = *(int32_t *)(buf + 8);
        int32_t relPosE = *(int32_t *)(buf + 12);
        int32_t relPosD = *(int32_t *)(buf + 16);

        // mm成分
        int8_t relPosHPN = *(int8_t *)(buf + 20);
        int8_t relPosHPE = *(int8_t *)(buf + 21);
        int8_t relPosHPD = *(int8_t *)(buf + 22);

        int32_t relPosN_mm = relPosN * 10 + relPosHPN;
        int32_t relPosE_mm = relPosE * 10 + relPosHPE;
        int32_t relPosD_mm = relPosD * 10 + relPosHPD;

        // flags
        uint32_t flags = *(uint32_t *)(buf + 60);

        int gnssFixOK          = (flags >> 0) & 0x01;
        int diffSoln           = (flags >> 1) & 0x01;
        int relPosValid        = (flags >> 2) & 0x01;
        int carrSoln           = (flags >> 3) & 0x03;   // 0:none 1:float 2:fix
        int isMoving           = (flags >> 5) & 0x01;
        int refPosMiss         = (flags >> 6) & 0x01;
        int refObsMiss         = (flags >> 7) & 0x01;
        int relPosHeadingValid = (flags >> 8) & 0x01;

        printf("N=%d mm E=%d mm D=%d mm  ", relPosN_mm, relPosE_mm, relPosD_mm);
        printf("FLAGS=0x%08X gnssFixOK=%d diffSoln=%d relPosValid=%d ",
               flags, gnssFixOK, diffSoln, relPosValid);
        printf("CARR=%d (0:none 1:float 2:fix) isMoving=%d refPosMiss=%d refObsMiss=%d relPosHeadOK=%d\n",
               carrSoln, isMoving, refPosMiss, refObsMiss, relPosHeadingValid);

        fflush(stdout);
    }

    close(fd);
    return 0;
}

sudo ./relpos_serial /dev/ttyAMA0