/*
 * 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/>.
 */

// logging of mount position

#include <usefull_macros.h>

#include "dump.h"
#include "simpleconv.h"

/**
 * @brief logmnt - log mount data into file
 * @param fcoords - file to dump
 * @param m - mount data
 */
void logmnt(FILE *fcoords, mountdata_t *m){
    if(!fcoords || !m) return;
    DBG("LOG");
    static double t0 = -1.;
    if(!fcoords) return;
    if(!m){ // write header
        fprintf(fcoords, "# time Xmot(deg) Ymot(deg) Xenc(deg) Yenc(deg) millis T V\n");
        return;
    }
    if(t0 < 0.) t0 = m->motposition.msrtime.tv_sec + (double)(m->motposition.msrtime.tv_usec) / 1e6;
    double t = m->motposition.msrtime.tv_sec + (double)(m->motposition.msrtime.tv_usec) / 1e6 - t0;
    // write data
    fprintf(fcoords, "%12.6f %10.6f %10.6f %10.6f %10.6f %10u %6.1f %4.1f\n",
            t, RAD2DEG(m->motposition.X), RAD2DEG(m->motposition.Y),
            RAD2DEG(m->encposition.X), RAD2DEG(m->encposition.Y),
            m->millis, m->temperature, m->voltage);
}

/**
 * @brief dumpmoving - dump conf while moving
 * @param fcoords - dump file
 * @param t - max waiting time
 * @param N - number of cycles to wait while motors aren't moving
 */
void dumpmoving(FILE *fcoords, double t, int N){
    if(!fcoords) return;
    mountdata_t mdata;
    DBG("Start dump");
    int ntries = 0;
    for(; ntries < 10; ++ntries){
        if(MCC_E_OK == Mount.getMountData(&mdata)) break;
    }
    if(ntries == 10){
        WARNX("Can't get mount data");
        LOGWARN("Can't get mount data");
    }
    uint32_t millis = mdata.millis;
    int ctr = -1;
    double xlast = mdata.motposition.X, ylast = mdata.motposition.Y;
    double t0 = sl_dtime();
    DBG("millis = %u", millis);
    while(sl_dtime() - t0 < t && ctr < N){
        usleep(10000);
        if(MCC_E_OK != Mount.getMountData(&mdata)){ WARNX("Can't get data"); continue;}
        if(mdata.millis == millis) continue;
        DBG("Got new data, posX=%g, posY=%g", mdata.motposition.X, mdata.motposition.Y);
        millis = mdata.millis;
        if(fcoords) logmnt(fcoords, &mdata);
        if(mdata.motposition.X != xlast || mdata.motposition.Y != ylast){
            xlast = mdata.motposition.X;
            ylast = mdata.motposition.Y;
            ctr = 0;
        }else ++ctr;
    }
}

/**
 * @brief waitmoving - wait until moving by both axes stops at least for N cycles
 * @param N - amount of stopped cycles
 */
void waitmoving(int N){
    mountdata_t mdata;
    int ctr = -1;
    uint32_t millis = 0;
    double xlast = 0., ylast = 0.;
    while(ctr < N){
        usleep(10000);
        if(MCC_E_OK != Mount.getMountData(&mdata)){ WARNX("Can't get data"); continue;}
        if(mdata.millis == millis) continue;
        if(mdata.motposition.X != xlast || mdata.motposition.Y != ylast){
            xlast = mdata.motposition.X;
            ylast = mdata.motposition.Y;
            ctr = 0;
        }else ++ctr;
    }
}