// Cable winch stepper kinematics
//
// Copyright (C) 2018-2019  Kevin O'Connor <kevin@koconnor.net>
//
// This file may be distributed under the terms of the GNU GPLv3 license.

#include <math.h> // sqrt
#include <stddef.h> // offsetof
#include <stdlib.h> // malloc
#include <string.h> // memset
#include "compiler.h" // __visible
#include "itersolve.h" // struct stepper_kinematics
#include "trapq.h" // move_get_coord

struct winch_stepper {
    struct stepper_kinematics sk;
    struct coord anchor;
};

static double
winch_stepper_calc_position(struct stepper_kinematics *sk, struct move *m
                            , double move_time)//roboprint eXperiment
{
    struct winch_stepper *hs = container_of(sk, struct winch_stepper, sk);
    struct coord c = move_get_coord(m, move_time);
    double dx = hs->anchor.x * c.x, dy = 130.75 - hs->anchor.y * c.y;
    double dz = 353.1 - (hs->anchor.z * c.z);
    return dx+sqrt(405*405-dy*dy-dz*dz);
}

struct stepper_kinematics * __visible
winch_stepper_alloc(double anchor_x, double anchor_y, double anchor_z)
{
    struct winch_stepper *hs = malloc(sizeof(*hs));
    memset(hs, 0, sizeof(*hs));
    hs->anchor.x = anchor_x;
    hs->anchor.y = anchor_y;
    hs->anchor.z = anchor_z;
    hs->sk.calc_position_cb = winch_stepper_calc_position;
    hs->sk.active_flags = AF_X | AF_Y | AF_Z;
    return &hs->sk;
}