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

// some simplest traectories
// all traectories runs increasing X and Y from starting point

#include <stdio.h>
#include <string.h>
#include <usefull_macros.h>

#include "simpleconv.h"
#include "traectories.h"

static traectory_fn cur_traectory = NULL;
// starting point of traectory
static coords_t XYstart = {0};
static double tstart = 0.;
// convert Xe/Ye to approximate motor coordinates:
// Xnew = Xcor+Xe; Ynew = Ycor+Ye; as Ye goes backwards to Ym, we have
// Xcor = Xm0 - Xe0; Ycor = Xm0 + Ye0
static coords_t XYcor = {0};

/**
 * @brief init_traectory - init traectory fn, sync starting positions of motor & encoders
 * @param f - function calculating next point
 * @param XY0 - starting point
 * @return FALSE if failed
 */
int init_traectory(traectory_fn f, coords_t *XY0){
    if(!f || !XY0) return FALSE;
    cur_traectory = f;
    XYstart = *XY0;
    tstart = sl_dtime();
    mountdata_t mdata;
    int ntries = 0;
    for(; ntries < 10; ++ntries){
        if(MCC_E_OK == Mount.getMountData(&mdata)) break;
    }
    if(ntries == 10) return FALSE;
    XYcor.X = mdata.motposition.X - mdata.encposition.X;
    XYcor.Y = mdata.motposition.X + mdata.encposition.Y;
    DBG("STARTING POINTS: x=%g, y=%g degrees", DEG2RAD(XYcor.X), DEG2RAD(XYcor.Y));
    return TRUE;
}

/**
 * @brief traectory_point - get traectory point for given time
 * @param nextpt (o) - next point coordinates
 * @param t - UNIX-time of event
 * @return FALSE if something wrong (e.g. X not in -90..90 or Y not in -180..180)
 */
int traectory_point(coords_t *nextpt, double t){
    if(t < 0. || !cur_traectory) return FALSE;
    coords_t pt;
    if(!cur_traectory(&pt, t)) return FALSE;
    pt.msrtime.tv_sec = floor(t);
    pt.msrtime.tv_usec = (uint32_t) t - pt.msrtime.tv_sec;
    if(nextpt) *nextpt = pt;
    if(pt.X < -M_PI_2 || pt.X > M_PI_2 || pt.Y < -M_PI || pt.Y > M_PI) return FALSE;
    return TRUE;
}

// current telescope position according to starting motor coordinates
// @return FALSE if failed to get current coordinates
int telpos(coords_t *curpos){
    mountdata_t mdata;
    int ntries = 0;
    for(; ntries < 10; ++ntries){
        if(MCC_E_OK == Mount.getMountData(&mdata)) break;
    }
    if(ntries == 10) return FALSE;
    coords_t pt;
    pt.X = XYcor.X + mdata.encposition.X;
    pt.Y = XYcor.Y + mdata.encposition.Y;
    pt.msrtime = mdata.encposition.msrtime;
    if(curpos) *curpos = pt;
    return TRUE;
}

// X=X0+1'/s, Y=Y0+15''/s
int Linear(coords_t *nextpt, double t){
    coords_t pt;
    pt.X = XYstart.X + ASEC2RAD(1.) * (t - tstart);
    pt.Y = XYstart.Y + ASEC2RAD(15.)* (t - tstart);
    if(nextpt) *nextpt = pt;
    return TRUE;
}

// X=X0+5'*sin(t/30*2pi), Y=Y0+10'*cos(t/200*2pi)
int SinCos(coords_t *nextpt, double t){
    coords_t pt;
    pt.X = XYstart.X + AMIN2RAD(5.) * sin((t-tstart)/30.*2*M_PI);
    pt.Y = XYstart.Y + AMIN2RAD(10.)* cos((t-tstart)/200.*2*M_PI);
    if(nextpt) *nextpt = pt;
    return TRUE;
}

typedef struct{
    traectory_fn f;
    const char *name;
    const char *help;
} tr_names;

static tr_names names[] = {
    {Linear, "linear", "X=X0+1'/s, Y=Y0+15''/s"},
    {SinCos, "sincos", "X=X0+5'*sin(t/30*2pi), Y=Y0+10'*cos(t/200*2pi)"},
    {NULL, NULL, NULL}
};

traectory_fn traectory_by_name(const char *name){
    traectory_fn f = NULL;
    for(int i = 0; ; ++i){
        if(!names[i].f) break;
        if(strcmp(names[i].name, name) == 0){
            f = names[i].f;
            break;
        }
    }
    return f;
}

// print all acceptable traectories names with help
void print_tr_names(){
    for(int i = 0; ; ++i){
        if(!names[i].f) break;
        printf("%s: %s\n", names[i].name, names[i].help);
    }
}