/*
 * This file is part of the libsidservo project.
 * Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include <asm-generic/termbits.h>
#include <errno.h>
#include <fcntl.h>
#include <math.h>
#include <pthread.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/ioctl.h>
#include <sys/stat.h>
#include <time.h>
#include <unistd.h>

#include "dbg.h"
#include "serial.h"
#include "ssii.h"

// serial devices FD
static int encfd = -1, mntfd = -1;
// main mount data
static mountdata_t mountdata = {0};

// mutexes for RW operations with mount device and data
static pthread_mutex_t  mntmutex = PTHREAD_MUTEX_INITIALIZER,
                        datamutex = PTHREAD_MUTEX_INITIALIZER;
// encoders thread and mount thread
static pthread_t encthread, mntthread;
// max timeout for 1.5 bytes of encoder and 2 bytes of mount
static struct timeval encRtmout = {0}, mntRtmout = {0};
// encoders raw data
typedef struct __attribute__((packed)){
    uint8_t magick;
    int32_t encX;
    int32_t encY;
    uint8_t CRC[4];
} enc_t;

/**
 * @brief dtime - UNIX time with microsecond
 * @return value
 */
double dtime(){
    double t;
    struct timeval tv;
    gettimeofday(&tv, NULL);
    t = tv.tv_sec + ((double)tv.tv_usec)/1e6;
    return t;
}

#if 0
// init start time
static void gttime(){
    struct timeval tv;
    gettimeofday(&tv, NULL);
    tv_sec_got = tv.tv_sec;
    tv_usec_got = tv.tv_usec;
}
#endif

/**
 * @brief parse_encbuf - check encoder buffer (for separate encoder) and fill fresh data
 * @param databuf - input buffer with 13 bytes of data
 * @param nexttime - time when databuf[0] got
 */
static void parse_encbuf(uint8_t databuf[ENC_DATALEN], struct timeval *tv){
    enc_t *edata = (enc_t*) databuf;
    if(edata->magick != ENC_MAGICK){
        DBG("No magick");
        return;
    }
    if(edata->CRC[3]){
        DBG("No 0 @ end: 0x%02x", edata->CRC[3]);
        return;
    }
    uint32_t POS_SUM = 0;
    for(int i = 1; i < 9; ++i) POS_SUM += databuf[i];
    uint8_t x = POS_SUM >> 8;
    if(edata->CRC[0] != x){
        DBG("CRC[0] = 0x%02x, need 0x%02x", edata->CRC[0], x);
        return;
    }
    uint8_t y = ((0xFFFF - POS_SUM) & 0xFF) - x;
    if(edata->CRC[1] != y){
        DBG("CRC[1] = 0x%02x, need 0x%02x", edata->CRC[1], y);
        return;
    }
    y = (0xFFFF - POS_SUM) >> 8;
    if(edata->CRC[2] != y){
        DBG("CRC[2] = 0x%02x, need 0x%02x", edata->CRC[2], y);
        return;
    }
    pthread_mutex_lock(&datamutex);
    mountdata.encposition.X = X_ENC2RAD(edata->encX);
    mountdata.encposition.Y = Y_ENC2RAD(edata->encY);
    mountdata.encposition.msrtime = *tv;
    pthread_mutex_unlock(&datamutex);
    DBG("time = %zd+%zd/1e6, X=%g deg, Y=%g deg", tv->tv_sec, tv->tv_usec, mountdata.encposition.X*180./M_PI, mountdata.encposition.Y*180./M_PI);
}

// try to read 1 byte from encoder; return -1 if nothing to read or -2 if device seems to be disconnected
static int getencbyte(){
    if(encfd < 0) return -1;
    uint8_t byte;
    fd_set rfds;
    struct timeval tv;
    do{
        FD_ZERO(&rfds);
        FD_SET(encfd, &rfds);
        tv = encRtmout;
        int retval = select(encfd + 1, &rfds, NULL, NULL, &tv);
        if(!retval) break;
        if(retval < 0){
            if(errno == EINTR) continue;
            return -1;
        }
        if(FD_ISSET(encfd, &rfds)){
            ssize_t l = read(encfd, &byte, 1);
            if(l != 1) return -2; // disconnected ??
            break;
        } else return -1;
    }while(1);
    return (int)byte;
}
// read 1 byte from mount; return -1 if nothing to read, -2 if disconnected
static int getmntbyte(){
    if(mntfd < 0) return -1;
    uint8_t byte;
    fd_set rfds;
    struct timeval tv;
   /* ssize_t l = read(mntfd, &byte, 1);
    //DBG("MNT read=%zd byte=0x%X", l, byte);
    if(l == 0) return -1;
    if(l != 1) return -2; // disconnected ??
    return (int) byte;*/
    do{
        FD_ZERO(&rfds);
        FD_SET(mntfd, &rfds);
        tv = mntRtmout;
        int retval = select(mntfd + 1, &rfds, NULL, NULL, &tv);
        if(retval < 0){
            if(errno == EINTR) continue;
            DBG("Error in select()");
            return -1;
        }
        //DBG("FD_ISSET = %d", FD_ISSET(mntfd, &rfds));
        if(FD_ISSET(mntfd, &rfds)){
            ssize_t l = read(mntfd, &byte, 1);
            //DBG("MNT read=%zd byte=0x%X", l, byte);
            if(l != 1) return -2; // disconnected ??
            break;
        } else return -1;
    }while(1);
    return (int)byte;
}

// main encoder thread (for separate encoder): read next data and make parsing
static void *encoderthread(void _U_ *u){
    uint8_t databuf[ENC_DATALEN];
    int wridx = 0, errctr = 0;
    struct timeval tv;
    while(encfd > -1 && errctr < MAX_ERR_CTR){
        int b = getencbyte();
        if(b == -2) ++errctr;
        if(b < 0) continue;
        errctr = 0;
        DBG("Got byte from Encoder: 0x%02X", b);
        if(wridx == 0){
            if((uint8_t)b == ENC_MAGICK){
                DBG("Got magic -> start filling packet");
                databuf[wridx++] = (uint8_t) b;
                gettimeofday(&tv, NULL);
            }
            continue;
        }else databuf[wridx++] = (uint8_t) b;
        if(wridx == ENC_DATALEN){
            parse_encbuf(databuf, &tv);
            wridx = 0;
        }
    }
    if(encfd > -1){
        close(encfd);
        encfd = -1;
    }
    return NULL;
}

data_t *cmd2dat(const char *cmd){
    if(!cmd) return  NULL;
    data_t *d = calloc(1, sizeof(data_t));
    if(!d) return NULL;
    d->buf = (uint8_t*)strdup(cmd);
    d->len = strlen(cmd);
    d->maxlen = d->len + 1;
    return d;
}
void data_free(data_t **x){
    if(!x || !*x) return;
    free((*x)->buf);
    free(*x);
    *x = NULL;
}

// main mount thread
static void *mountthread(void _U_ *u){
    int errctr = 0;
    uint8_t buf[2*sizeof(SSstat)];
    SSstat *status = (SSstat*) buf;
    // data to get
    data_t d = {.buf = buf, .maxlen = sizeof(buf)};
    // cmd to send
    const data_t *cmd_getstat = cmd2dat(CMD_GETSTAT);
    double t0 = dtime();
/*
#ifdef EBUG
    double t00 = t0;
#endif
*/
    while(mntfd > -1 && errctr < MAX_ERR_CTR){
        // read data to status
        struct timeval tgot;
        if(0 != gettimeofday(&tgot, NULL)) continue;
        if(!MountWriteRead(cmd_getstat, &d) || d.len != sizeof(SSstat)){
            DBG("Can't read SSstat, need %zd got %zd bytes", sizeof(SSstat), d.len);
            ++errctr; continue;
        }
        if(SScalcChecksum(buf, sizeof(SSstat)-2) != status->checksum){
            DBG("BAD checksum of SSstat, need %d", status->checksum);
            ++errctr; continue;
        }
        errctr = 0;
        pthread_mutex_lock(&datamutex);
        // now change data
        SSconvstat(status, &mountdata, &tgot);
        pthread_mutex_unlock(&datamutex);
        // allow writing & getters
        //DBG("t0=%g, tnow=%g", t0-t00, dtime()-t00);
        while(dtime() - t0 < Conf.MountReqInterval);
        t0 = dtime();
    }
    if(mntfd > -1){
        close(mntfd);
        mntfd = -1;
    }
    return NULL;
}

// open device and return its FD or -1
static int ttyopen(const char *path, speed_t speed){
    int fd = -1;
    struct termios2 tty;
    DBG("Try to open %s @ %d", path, speed);
    if((fd = open(path, O_RDWR|O_NOCTTY)) < 0) return -1;
    if(ioctl(fd, TCGETS2, &tty)){ close(fd); return -1; }
    tty.c_lflag     = 0; // ~(ICANON | ECHO | ECHOE | ISIG)
    tty.c_iflag     = 0; // don't do any changes in input stream
    tty.c_oflag     = 0; // don't do any changes in output stream
    tty.c_cflag     = BOTHER | CS8 | CREAD | CLOCAL; // other speed, 8bit, RW, ignore line ctrl
    tty.c_ispeed = speed;
    tty.c_ospeed = speed;
    //tty.c_cc[VMIN]  = 0;  // non-canonical mode
    //tty.c_cc[VTIME] = 5;
    if(ioctl(fd, TCSETS2, &tty)){ close(fd); return -1; }
    DBG("Check speed");
    if(tty.c_ispeed != (speed_t) speed || tty.c_ospeed != (speed_t)speed){ close(fd); return -1; }
    // try to set exclusive
    if(ioctl(fd, TIOCEXCL)){DBG("Can't make exclusive");}
    return fd;
}

// return FALSE if failed
int openEncoder(const char *path, int speed){
    if(!Conf.SepEncoder) return FALSE; // try to open separate encoder when it's absent
    if(encfd > -1) close(encfd);
    encfd = ttyopen(path, (speed_t) speed);
    if(encfd < 0) return FALSE;
    encRtmout.tv_sec = 0;
    encRtmout.tv_usec = 200000000 / speed; // 20 bytes
    if(pthread_create(&encthread, NULL, encoderthread, NULL)){
        close(encfd);
        encfd = -1;
        return FALSE;
    }
    DBG("Encoder opened, thread started");
    return TRUE;
}

// return FALSE if failed
int openMount(const char *path, int speed){
    if(mntfd > -1) close(mntfd);
    DBG("Open mount %s @ %d", path, speed);
    mntfd = ttyopen(path, (speed_t) speed);
    if(mntfd < 0) return FALSE;
    DBG("mntfd=%d", mntfd);
    // clear buffer
    while(getmntbyte() > -1);
    /*int g = write(mntfd, "XXS\r", 4);
    DBG("Written %d", g);
    uint8_t buf[100];
    do{
        ssize_t l = read(mntfd, buf, 100);
        DBG("got %zd", l);
    }while(1);*/
    mntRtmout.tv_sec = 0;
    mntRtmout.tv_usec = 500000000 / speed; // 50 bytes
    if(pthread_create(&mntthread, NULL, mountthread, NULL)){
        DBG("Can't create thread");
        close(mntfd);
        mntfd = -1;
        return FALSE;
    }
    DBG("Mount opened, thread started");
    return TRUE;
}

// close all opened serial devices and quit threads
void closeSerial(){
    if(mntfd > -1){
        DBG("Kill mount thread");
        if(0 == pthread_cancel(mntthread))
            pthread_join(mntthread, NULL);
        DBG("close fd");
        close(mntfd);
        mntfd = -1;
    }
    if(encfd > -1){
        DBG("Kill encoder thread");
        if(0 == pthread_cancel(encthread))
            pthread_join(encthread, NULL);
        DBG("close fd");
        close(encfd);
        encfd = -1;
    }
}

// get fresh encoder information
mcc_errcodes_t getMD(mountdata_t  *d){
    if(!d) return MCC_E_BADFORMAT;
    pthread_mutex_lock(&datamutex);
    *d = mountdata;
    pthread_mutex_unlock(&datamutex);
    return MCC_E_OK;
}

/**
 * @brief MountWriteRead - write and read @ once (or only read/write)
 * @param out (o) - data to write or NULL if not need
 * @param in  (i) - data to read or NULL if not need
 * @return FALSE if failed
 */
int MountWriteRead(const data_t *out, data_t *in){
    if((!out && !in) || mntfd < 0) return FALSE;
    int ret = FALSE;
    pthread_mutex_lock(&mntmutex);
    if(out){
        if(out->len != (size_t)write(mntfd, out->buf, out->len)) goto ext;
        //DBG("Send to mount %zd bytes: %s", out->len, out->buf);
        int g = write(mntfd, "\r", 1); // add EOL
        (void) g;
    }
    if(in){
        in->len = 0;
        for(size_t i = 0; i < in->maxlen; ++i){
            int b = getmntbyte();
            if(b < 0) break; // nothing to read -> go out
            in->buf[in->len++] = (uint8_t) b;
        }
    }
    ret = TRUE;
ext:
    pthread_mutex_unlock(&mntmutex);
    return ret;
}