#include "kinematicsplugin.h"
#include <math.h>
KinematicsPlugin::KinematicsPlugin(QObject *parent) : QObject(parent)
{
    for (int i=0;i<32;i++)
    {
        InputRatio[i]=1;
        OutputRatio[i]=1;
    }
    R1=100;
    R2=50;
}

void KinematicsPlugin::setParameters(uint32_t addr, double param)
{
    switch (addr)
    {
    case 0: R1=param;break;
    case 1: R2=param;break;
    }
}

void KinematicsPlugin::MKinematics(int64_t *input, int64_t *output, uint32_t naxes)
{
    for (uint32_t i=0;i<naxes;i++) output[i]=input[i];

//    for (uint32_t i=0;i<naxes;i++) output[i]=input[0]/(i+1);
    return;

    for (uint32_t i=0;i<naxes;i++) output[i]=input[i]*InputRatio[i]*OutputRatio[i];

    double x=input[0]*InputRatio[0];
    double y=input[1]*InputRatio[1];
    double z=input[2]*InputRatio[2];

    double M2=x*x+y*y;
    double M=sqrt(M2);
    double L2=M2+z*z;
    double L=sqrt(L2);

    double a=acos(x/M);
    double b=asin(z/L);
    double d=acos((R1*R1+R2*R2-L2)/(2*R1*R2));
    double f=asin(sin(d)*R2/L);
    double c=b+f;

    output[0]=a*OutputRatio[0];
    output[1]=c*OutputRatio[1];
    output[2]=d*OutputRatio[2];

}


void KinematicsPlugin::setInputRatios(double * ratio, uint32_t naxes)
{
    for (uint32_t i=0;i<naxes;i++) InputRatio[i]=ratio[i];
}

void KinematicsPlugin::setOutputRatios(double * ratio, uint32_t naxes)
{
    for (uint32_t i=0;i<naxes;i++) OutputRatio[i]=ratio[i];
}