//    Copyright 2013 Jeff Epler <jepler@unpythonic.net>
//
//    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 2 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, write to the Free Software
//    Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.

#define BOOST_PYTHON_MAX_ARITY 4
#include <cmath>
#include "rotarydeltakins-common.h"
#include <boost/python.hpp>
using namespace boost::python;

static object forward(double j0, double j1, double j2, double j3, double j4, double j5, double j6, double j7, double j8)
{
    double joints[9] = {j0, j1, j2, j3, j4, j5, j6, j7, j8};
    EmcPose pos;
    int result = kinematics_forward(joints, &pos);
    if(result == 0)
        return make_tuple(pos.tran.x, pos.tran.y, pos.tran.z, pos.tran.a, pos.tran.b, pos.tran.c, pos.tran.u, pos.tran.v, pos.tran.w);
    return object();
}

static object inverse(double x, double y, double z, double a, double b, double c, double u, double v, double w)
{
    double joints[9];
    EmcPose pos = {{x,y,z},a,b,c,u,v,w};
    int result = kinematics_inverse(&pos, joints);
    if(result == 0)
        return make_tuple(joints[0], joints[1], joints[2], joints[3], joints[4], joints[5], joints[6], joints[7], joints[8]);
    return object();
}

static object get_geometry()
{
    return make_tuple(platformradius, thighlength, shinlength, footradius);
}

BOOST_PYTHON_MODULE(rotarydeltakins)
{
    set_geometry(RDELTA_PFR, RDELTA_TL, RDELTA_SL, RDELTA_FR);
    def("set_geometry", set_geometry);
    def("get_geometry", get_geometry);
    def("forward", forward);
    def("inverse", inverse);
}