File content as of revision 13:9a51747773af:

#include "CubicSpline.h"

//  To get voltage of TRP105F
AnalogIn g_Sensor_Voltage(p16);
//  To get sample distance via seral com
Serial g_Serial_Signal(USBTX, USBRX);

LocalFileSystem local("local");  // マウントポイントを定義（ディレクトリパスになる）
// for debug
#ifdef DEBUG
DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);
#endif

CubicSpline2d::CubicSpline2d()
    :_useType(AsMODULE)
{
    _Sample_Num = 5;
    _Sample_Set = (Vxyt  *)malloc(_Sample_Num * sizeof(Vxyt));
    _Last_Point = (Vxyt) {
        0.0, 0.0, 0.0
    };
    for(int i = 0; i < 4; i++) {
        _C_x[i]= (double*)malloc((_Sample_Num - 1)* sizeof(double));;
        _C_y[i]= (double*)malloc((_Sample_Num - 1)* sizeof(double));;
    }
    //calibrateSensor();
}

CubicSpline2d::CubicSpline2d(
    unsigned int arg_num
)
    :_useType(AsMODULE)
{
    _Sample_Num = arg_num;
    _Sample_Set = (Vxyt  *)malloc(_Sample_Num * sizeof(Vxyt));
    _Last_Point = (Vxyt) {
        0.0, 0.0, 0.0
    };
    for(int i = 0; i < 4; i++) {
        _C_x[i]= (double*)malloc((_Sample_Num - 1)* sizeof(double));;
        _C_y[i]= (double*)malloc((_Sample_Num - 1)* sizeof(double));;
    }
    //calibrateSensor();
}

CubicSpline2d::CubicSpline2d(
    unsigned int    arg_num,
    UseType         arg_useType
)
    :_useType(arg_useType)
{
    _Sample_Num = arg_num;
    _Sample_Set = (Vxyt  *)malloc(_Sample_Num * sizeof(Vxyt));
    _Last_Point = (Vxyt) {
        0.0, 0.0, 0.0
    };
    for(int i = 0; i < 4; i++) {
        _C_x[i]= (double*)malloc((_Sample_Num - 1)* sizeof(double));;
        _C_y[i]= (double*)malloc((_Sample_Num - 1)* sizeof(double));;
    }
    //calibrateSensor();
}

CubicSpline2d::~CubicSpline2d()
{
    free(_Sample_Set);
    //free(_u_param);
    for(int i = 0; i < 4; i++) {
        free(_C_x[i]);
        free(_C_y[i]);
    }
}

void CubicSpline2d::_sampleData()
{
    int     tmp;
    char    sig;
    Vxyt    tmp_set;

    int floatflag = 0;

    //  For evry set,
    //  1, get dst data via serai com,
    //  2, get vol data,
    //  and then do same for next index set.
    for(int i = 0; i < _Sample_Num; i++) {
        if(_useType == AsDEBUG) {
            //
            //  Recieve a Distance datus and store it into member
            //
            g_Serial_Signal.printf("X:");
            _Sample_Set[i].x = 0.0;
            do {
                sig = g_Serial_Signal.getc();
                if('0' <= sig && sig <= '9') {
                    if(floatflag == 0) {
                        _Sample_Set[i].x = 10.0 * _Sample_Set[i].x + (double)(sig - 48);
                    } else {
                        _Sample_Set[i].x = _Sample_Set[i].x + (double)(sig - 48) * pow(10.0, (double)- floatflag);
                        floatflag++;
                    }
                    g_Serial_Signal.putc(char(sig));
                } else if(sig == '.') {
                    if(floatflag == 0) {
                        floatflag = 1;
                        g_Serial_Signal.putc(char(sig));
                    }
                } else if(sig == 0x08) {
                    _Sample_Set[i].x = 0.0;
                    g_Serial_Signal.printf("[canseled!]");
                    g_Serial_Signal.putc('\n');
                    g_Serial_Signal.putc('>');
                }
            } while (!(sig == 0x0a || sig == 0x0d));
            floatflag = 0;
            g_Serial_Signal.putc('\n');
            g_Serial_Signal.printf("x:%f|",_Sample_Set[i].x);
            //
            //  Recieve a Voltage datus and store it into member
            //
            //  LOW PASS FILTERED
            //  Get 10 data and store mean as a sample.
            //  After get one original sample, system waits for 0.1 sec,
            //  thus it takes 1 sec evry sampling.
            _Sample_Set[i].y = 0.0;
            for(int j = 0; j < 10; j++) {
                tmp_set.y = g_Sensor_Voltage.read();
#ifdef DEBUG
                g_Serial_Signal.printf("%f,",tmp_set.y);
#endif
                _Sample_Set[i].y += (tmp_set.y / 10.0);
                wait(0.1);
            }
#ifdef DEBUG
            g_Serial_Signal.printf("(%f)\n",_Sample_Set[i].y);
#endif
        }

        //  if the input data is over the bound, it is calibrated
        if (_Sample_Set[i].x < 0.0)
            _Sample_Set[i].x = 0.0;
    }
    //
    //  Sort set data array in x-Ascending order
    //
    tmp = 0;
    for(    int i = 0              ; i < _Sample_Num; i++) {
        for(int j = _Sample_Num - 1; i < j          ; j--) {
            //  use dst as index for dst range [2,20]
            if (_Sample_Set[i].x > _Sample_Set[j].x) {
                tmp_set.x     = _Sample_Set[i].x;
                tmp_set.y     = _Sample_Set[i].y;
                _Sample_Set[i].x = _Sample_Set[j].x;
                _Sample_Set[i].y = _Sample_Set[j].y;
                _Sample_Set[j].x = tmp_set.x;
                _Sample_Set[j].y = tmp_set.y;
            }
            // if a same dst has been input, calcurate mean.
            else if (_Sample_Set[i].x == _Sample_Set[j].x) {
                tmp_set.y = (_Sample_Set[i].y + _Sample_Set[j].y)/2.0;
                _Sample_Set[i].y = tmp_set.y;
                for(int k = j; k < _Sample_Num - 1; k++)
                    _Sample_Set[k] = _Sample_Set[k+1];
                tmp++;
            }
        }
    }
#ifdef DEBUG
    g_Serial_Signal.printf("  _Sample_num: %d\n", _Sample_Num );
    g_Serial_Signal.printf("-)        tmp: %d\n", tmp );
#endif
    //  substruct tmp from number of sample.
    _Sample_Num -= tmp;
#ifdef DEBUG
    g_Serial_Signal.printf("-----------------\n");
    g_Serial_Signal.printf("  _Sample_num: %d\n", _Sample_Num );
#endif

    //  generate t which is parameter related to x,y
    _Sample_Set[0].t = 0.0;
    for(int i = 1; i < _Sample_Num; i++)
        _Sample_Set[i].t =
            _Sample_Set[i-1].t
            + sqrt(pow(_Sample_Set[i].x - _Sample_Set[i-1].x, 2.0)
                   +pow(_Sample_Set[i].y - _Sample_Set[i-1].y, 2.0));
}

//
//  Function to define _u_spline, specific constants of spline.
//
void CubicSpline2d::_makeModel(const double* arg_sampled_t, const double* arg_sampled_ft, double* arg_C[4], const unsigned int arg_num)
{
    //  arg_t :     t; The variable of f(t)
    //  arg_ft:     f(t); The cubic poliminal in Interval-j.
    //  arg_C[i]:   Ci; The coefficient of t^i of f(t) that defines Spline Model Poliminal f(t).
    //  arg_num:    j in [0,_Sample_Num-1]; The number of interval.
    //  f(t)j = C3j*t^3 + C2j*t^2 + C1j*t + C0j
    //
    //  N: max of index <=> (_Sample_Num - 1)
    //
    //  u[i] === d^2/dx^2(Spline f)[i]
    //  i:[0,N]
    //  u[0] = u[N] = 0
#if defined (VERSION_C)
    double *u = (double*)malloc((arg_num    ) * sizeof(double));
#elif defined (VERSION_Cpp)
    double *u = new double[arg_num];
#elif defined (VERSION_Cpp11)
    std::array<double,arg_num> u;
#endif
    //
    //  h[i] = x[i+1] - x[i]
    //  i:[0,N-1]; num of elm: N<=>_Sample_Num - 1
    double *h = (double*)malloc((arg_num - 1) * sizeof(double));
    //
    //  v[i] = 6*((y[i+2]-y[i+1])/h[i+1] + (y[i+1]-y[i])/h[i])
    //  i:[0,N-2]
    double *v = (double*)malloc((arg_num - 2) * sizeof(double));
    //
    //  temporary array whose num of elm equals v array
    double *w = (double*)malloc((arg_num - 2) * sizeof(double));
    //
    //  [ 2(h[0]+h[1])  , h[1]          ,                                 O                 ]   [u[1]  ]   [v[0]  ]
    //  [ h[1]          , 2(h[1]+h[2])  , h[2]                                              ]   [u[2]  ]   [v[1]  ]
    //  [                       ...                                                         ] * [...   ] = [...   ]
    //  [                   h[j]          , 2(h[j]+h[j+1])  , h[j+1]                        ]   [u[j+1]]   [v[j]  ]
    //  [                                   ...                                             ]   [ ...  ]   [ ...  ]
    //  [                               h[N-3]        , 2(h[N-3]+h[N-2]), h[N-2]            ]   [u[j+1]]   [v[j]  ]
    //  [       O                                       h[N-2]          , 2(h[N-2]+h[N-1])  ]   [u[N-1]]   [v[N-2]]
    //
    // For LU decomposition
    double *Upper = (double*)malloc((arg_num - 2) * sizeof(double));
    double *Lower = (double*)malloc((arg_num - 2) * sizeof(double));
#ifdef DEBUG_MAKE_MODEL
    _printOutDataCouple(arg_sampled_t, arg_sampled_ft, arg_num, "\nargument set\n");
#endif

    for(int i = 0; i < arg_num - 1; i++)
        h[i] =  (double)(arg_sampled_t[i + 1] - arg_sampled_t[i]);

    for(int i = 0; i < arg_num - 2; i++)
        v[i] = 6.0 * (
                   ((double)(arg_sampled_ft[i + 2] - arg_sampled_ft[i + 1])) / h[i + 1]
                   -
                   ((double)(arg_sampled_ft[i + 1] - arg_sampled_ft[i]))     / h[i]
               );

    //
    //  LU decomposition
    //
    Upper[0] = 2.0 * (h[0] + h[1]);
    Lower[0] = 0.0;
    for (int i = 1; i < arg_num - 2; i++) {
        Lower[i] = h[i] / Upper[i - 1];
        Upper[i] = 2.0 * (h[i] + h[i + 1]) - Lower[i] * h[i];
    }
    //
    //  forward substitution
    //
    w[0] = v[0];
    for (int i = 1; i < arg_num - 2; i ++) {
        w[i] = v[i] - Lower[i] * w[i-1];
    }
    //
    //  backward substitution
    //
    u[arg_num - 2] =  w[arg_num - 3]                     / Upper[arg_num - 3];
    for(int i = arg_num - 3; i > 0; i--) {
        u[i]       = (w[(i - 1)] -  h[(i)] * u[(i) + 1]) / Upper[(i - 1)];
    }
    // _u_spline[i] === d^2/dx^2(Spline f)[i]
    u[0] = u[arg_num - 1] = 0.0;
#ifdef DEBUG_MAKE_MODEL
    _printOutData(h,     arg_num - 1, "h");
    _printOutData(v,     arg_num - 2, "v");
    _printOutData(w,     arg_num - 2, "w");
    _printOutData(Upper, arg_num - 2, "Upper");
    _printOutData(Lower, arg_num - 2, "Lower");
    _printOutData(u,     arg_num    , "u");
#endif

    for(int ival = 0; ival < arg_num - 1; ival++) {
        arg_C[3][ival] = (u[ival + 1] - u[ival]) / 6.0 / (arg_sampled_t[ival + 1] - arg_sampled_t[ival]);
        arg_C[2][ival] = (u[ival]) / 2.0;
        arg_C[1][ival] = (arg_sampled_ft[ival + 1] - arg_sampled_ft[ival]) / (arg_sampled_t[ival + 1] - arg_sampled_t[ival])
                         -
                         (arg_sampled_t[ival + 1]  - arg_sampled_t[ival])  * (u[ival + 1] + 2.0 * u[ival]) / 6.0;
        arg_C[0][ival] = (arg_sampled_ft[ival]);
    }
#ifdef DEBUG_MAKE_MODEL
    for(int ival = 0; ival < arg_num - 1; ival++) {
        for(int i = 0; i < 4; i++)
            g_Serial_Signal.printf("C[%d][%d]: %f\n", i, ival, arg_C[i][ival]);
    }
#endif

    free(h);
    free(u);
    free(v);
    free(w);
    free(Upper);
    free(Lower);
}
void CubicSpline2d::_makeModel(const double* arg_t, const double* arg_ft, double* arg_C[4])
{
    _makeModel(arg_t, arg_ft, arg_C, _Sample_Num);
}


void CubicSpline2d::calibrateSensor()
{
    double t[_Sample_Num];
    double ft[_Sample_Num];

    _sampleData();
    _Last_Point = _Sample_Set[0];

    for(int i = 0; i < _Sample_Num; i++) {
        t[i] = _Sample_Set[i].t;
        ft[i]= _Sample_Set[i].x;
    }
    _makeModel(t,ft,_C_x);

    for(int i = 0; i < _Sample_Num; i++) {
        ft[i]= _Sample_Set[i].y;
    }
    _makeModel(t,ft,_C_y);

}


//
//  Fuction to return the value of Cubic polynomial f(t)
//
double  CubicSpline2d::_cubic_f(const double   arg_t, const double arg_C[4])
{
    double ft;  //the value of Spline f(t).

    ft = arg_C[3] * pow(arg_t, 3.0) + arg_C[2] * pow(arg_t, 2.0) + arg_C[1] * arg_t + arg_C[0];

    return ft;
}


//
//  Function to solve a cubic polinomial
//  by using Gardano-Tartaglia formula
//
void CubicSpline2d::_solve_cubic_f(
    std::complex<double> arg_t[3],
    const double  arg_C[4],
    const double  arg_ft)
{
#ifdef DEBUG_SOLVE
    for(int i = 0; i < 4; i++)
        g_Serial_Signal.printf("C%d: %f\n", i, arg_C[i]);
#endif

    //arg_t: solution that's expected to be solved in this function.
    //t_sol: the solution that's actually wanted as Sprine's solution.
    //t0_ival: _Sample_Set[ival].t
    //arg_t  = t_sol - t0_ival
    //=>
    //arg_ft = C[3](t_sol - t0_ival)^3 + C[2](t_sol - t0_ival)^2 + C[1](t_sol - t0_ival) + C[0]
    //       = C[3]arg_t^3 + C[2]arg_t^2 + C[1]arg_t + C[0]
    double c[3];
    //f(t) := arg_ft/arg_C[3]
    //      = arg_t^3 + c[2]*arg_t^2 + c[1]*arg_t + c[0].
    for(int i = 0; i < 3; i++) {
        c[i] = arg_C[i] / arg_C[3];
    }
    //modify the formula
    //t^3 + c[2]*t^2 + c[1]*t + (c[0] - f(t)) = 0.
    c[0] -= arg_ft / arg_C[3];
#ifdef DEBUG_SOLVE
    for(int i = 0; i < 3; i++)
        g_Serial_Signal.printf("c%d: %f\n", i, c[i]);
#endif

    std::complex<double> d;
    //d := c[2] / 3
    //T := t + d (== arg_t - t_iavl + c[2]/3)
    d = std::complex<double>(c[2] / 3.0, 0.0);
    //=>    T^3 + 3pT + 2q = 0.
    double p,q;
    //The values defined from coefficients of the formula
    //that identify solutions
    p = (     -pow(c[2], 2.0) + 3.0 * c[1]) / 9.0;
    q = (2.0 * pow(c[2], 3.0) - 9.0 * c[2] * c[1] + 27.0 * c[0]) / 54.0;

    //Discriminant section
    double D;
    D = pow(p, 3.0) + pow(q, 2.0);
#ifdef DEBUG_SOLVE
    g_Serial_Signal.printf("p: %f\n", p);
    g_Serial_Signal.printf("q: %f\n", q);
    g_Serial_Signal.printf("D: %f\n", D);
#endif

    //For all T, u, there exsists v: T = u + v;  increment degree of freedom.
    //Futhermore,
    //  u^3 + v^3 - 2q = 0
    //  uv + p = 0 <=> u^3v^3 = -p^3
    //those because: T = u + v, T^3 + 3pT + 2q = 0,
    //=>    (u + v)^3 + 3p(u + v) + 2q = 0
    //=>    u^3 + v^3 + 3(uv + p)(u+v) + 2q = 0
    //The values defined from p and q
    //that idetify solutions
    std::complex<double> u,v;
    //Real root only
    if(D <= 0.0) {
        u = std::complex<double>(-q, sqrt(-D));
        v = std::complex<double>(-q,-sqrt(-D));
        //u = pow(u, 1/3);
        //v = pow(v, 1/3);
        u = std::exp(std::log(u)/std::complex<double>(3.0,0.0));
        v = std::exp(std::log(v)/std::complex<double>(3.0,0.0));
    }
    //One real root and two complex root
    else {
        u = std::complex<double>(-q+sqrt(D),0.0);
        v = std::complex<double>(-q-sqrt(D),0.0);
        u = std::complex<double>(cbrt(u.real()), 0.0);
        v = std::complex<double>(cbrt(v.real()), 0.0);
    }
#ifdef DEBUG_SOLVE
    g_Serial_Signal.printf("u: %f + (%f)i\n", u.real(), u.imag());
    g_Serial_Signal.printf("v: %f + (%f)i\n", v.real(), v.imag());
    g_Serial_Signal.printf("d: %f + (%f)i\n", d.real(), d.imag());
#endif

    //Cubic root of 1
    std::complex<double> omega[3]= {
        std::complex<double>( 1.0, 0.0),
        std::complex<double>(-1.0/2.0, sqrt(3.0)/2.0),
        std::complex<double>(-1.0/2.0,-sqrt(3.0)/2.0)
    };

    //Solution of the formula
    arg_t[0] = omega[0] * u + omega[0] * v - d;
    arg_t[1] = omega[1] * u + omega[2] * v - d;
    arg_t[2] = omega[2] * u + omega[1] * v - d;

#ifdef DEBUG_SOLVE
    for(int i = 0; i < 3; i++)
        g_Serial_Signal.printf("t%d: %f + (%f)i\n", i, arg_t[i].real(), arg_t[i].imag() );
#endif
}

double  CubicSpline2d::getX(double arg_y)
{
    double x;
    double C[4];
    double the_t;
    int    the_i;
    std::complex<double>t_sol[3];
    std::vector<double> t_real;
    std::vector<int>    t_ival;

#ifdef DEBUG_GETX
    g_Serial_Signal.printf(DEBUG_GETX);
#endif
    //  For the every Intervals of Spline,
    //it solves the polynomial defined by C[i] of the interval,
    //checks the solutions are real number,
    //and ckecks the solutions are in the interval.
    //  And if not-excluded solutions are more than one,
    //it trys to find which one is more nearest to last point.
    for(int ival = 0; ival < _Sample_Num - 1; ival++) {
        for(int i = 0; i < 4; i++) C[i] = _C_y[i][ival];
        _solve_cubic_f(t_sol, C, arg_y);
        for(int i = 0; i < 3; i++) t_sol[i] += _Sample_Set[ival].t;
#ifdef DEBUG_GETX
        g_Serial_Signal.printf("interval:%d \t %f < t < %f\n", ival, _Sample_Set[ival].t, _Sample_Set[ival + 1].t);
#endif
        for(int i = 0; i < 3; i++) {
            //  regarding only real solution
            //  acuracy (error range) is supposed +-10E-3 here(groundless)
            if(std::abs(t_sol[i].imag()) < 0.000001) {
                /*  */ if (ival == 0               && t_sol[i].real() < _Sample_Set[ival].t) {
                    t_real.push_back(t_sol[i].real());
                    t_ival.push_back(ival);
#ifdef DEBUG_GETX
                    g_Serial_Signal.printf("(t, i) = (%f, %d)\n", t_real[t_real.size() - 1], ival);
#endif
                } else if (ival == _Sample_Num - 2 && _Sample_Set[ival + 1].t <= t_sol[i].real()) {
                    t_real.push_back(t_sol[i].real());
                    t_ival.push_back(ival);
#ifdef DEBUG_GETX
                    g_Serial_Signal.printf("(t, i) = (%f, %d)\n", t_real[t_real.size() - 1], ival);
#endif
                } else if (_Sample_Set[ival].t <= t_sol[i].real() && t_sol[i].real() < _Sample_Set[ival + 1].t) {
                    t_real.push_back(t_sol[i].real());
                    t_ival.push_back(ival);
#ifdef DEBUG_GETX
                    g_Serial_Signal.printf("(t, i) = (%f, %d)\n", t_real[t_real.size() - 1], ival);
#endif
                }
            }
        }
    }

    if(!t_real.empty()) {
        the_t = t_real[0];
        the_i = t_ival[0];
        //if t's size is bigger than 1
        for(int i = 1; i < t_real.size(); i++) {
            if(std::abs(t_real[i] - _Last_Point.t) < std::abs(the_t - _Last_Point.t)) {
                the_t = t_real[i];
                the_i = t_ival[i];
            }
        }
    } else {
#ifdef DEBUG_GETX
        g_Serial_Signal.printf("LastPoint\n");
#endif
        the_t = _Last_Point.t;
        for (int i = 0; i < _Sample_Num - 1; i++)
            if(_Sample_Set[i].t <= the_t && the_t <= _Sample_Set[i+1].t)
                the_i = i;
    }
    /* */if (the_t < _Sample_Set[0].t) the_t = _Sample_Set[0].t;
    else if (_Sample_Set[_Sample_Num - 1].t <= the_t) the_t = _Sample_Set[_Sample_Num - 1].t;

    for(int i = 0; i < 4; i++) C[i] = _C_x[i][the_i];
    x = _cubic_f(the_t - _Sample_Set[the_i].t, C);
#ifdef DEBUG_GETX
    g_Serial_Signal.printf("(the_t, the_i):= (%f , %d)\n",the_t, the_i);
#endif
    _Last_Point = (Vxyt) {
        x, arg_y, the_t
    };
    return x;
}

double  CubicSpline2d::getY(double arg_x)
{

    double y;
    double C[4];
    double the_t;
    int    the_i;
    std::complex<double>t_sol[3];
    std::vector<double> t_real;
    std::vector<int>    t_ival;

#ifdef DEBUG_GETY
    g_Serial_Signal.printf(DEBUG_GETY);
    g_Serial_Signal.printf("arg_x: %f\n", arg_x);
#endif
    //  For the every Intervals of Spline,
    //it solves the polynomial defined by C[i] of the interval,
    //checks the solutions are real number,
    //and ckecks the solutions are in the interval.
    //  And if not-excluded solutions are more than one,
    //it trys to find which one is more nearest to last point.
    for(int ival = 0; ival < _Sample_Num - 1; ival++) {
        for(int i = 0; i < 4; i++) C[i] = _C_x[i][ival];
        _solve_cubic_f(t_sol, C, arg_x);
        for(int i = 0; i < 3; i++) t_sol[i] += _Sample_Set[ival].t;
        //arg_ft = C[3](t_sol - t0_ival)^3 + C[2](t_sol - t0_ival)^2 + C[1](t_sol - t0_ival) + C[0]
        //       = C[3]arg_t^3 + C[2]arg_t^2 + C[1]arg_t + C[0]
#ifdef DEBUG_GETY
        g_Serial_Signal.printf("interval:%d \t %f <= t < %f\n", ival, _Sample_Set[ival].t, _Sample_Set[ival + 1].t);
        for(int i = 0; i < 3; i++)
            g_Serial_Signal.printf("t%d \t %f + (%f)i\n", i, t_sol[i].real(), t_sol[i].imag());
#endif
        for(int i = 0; i < 3; i++) {
            //  regarding only real solution
            //  acuracy (error range) is supposed +-10E-6 here(groundless)
            if(std::abs(t_sol[i].imag()) < 0.000001) {
                /**/ if (ival == 0               && t_sol[i].real() < _Sample_Set[ival].t) {
                    t_real.push_back(t_sol[i].real());
                    t_ival.push_back(ival);
#ifdef DEBUG_GETY
                    g_Serial_Signal.printf("(t, i) = (%f, %d)\n", t_real[t_real.size() - 1], ival);
#endif
                }//
                else if (ival == _Sample_Num - 2 && _Sample_Set[ival + 1].t <= t_sol[i].real()) {
                    t_real.push_back(t_sol[i].real());
                    t_ival.push_back(ival);
#ifdef DEBUG_GETY
                    g_Serial_Signal.printf("(t, i) = (%f, %d)\n", t_real[t_real.size() - 1], ival);
#endif
                }
                //  There sometimes be a so fucking small error in solutions,
                //  so acuracy is set at 10E-6(groundless)
                else if (static_cast<int>(_Sample_Set[ival].t * std::pow(10., 6.)) <= static_cast<int>(t_sol[i].real() * std::pow(10., 6.))
                         &&  static_cast<int>(t_sol[i].real() * std::pow(10., 6.))     <  static_cast<int>(_Sample_Set[ival + 1].t * std::pow(10., 6.))) {
                    t_real.push_back(t_sol[i].real());
                    t_ival.push_back(ival);
#ifdef DEBUG_GETY
                    g_Serial_Signal.printf("(t, i) = (%f, %d)\n", t_real[t_real.size() - 1], ival);
#endif
                }
            }
        }
    }

    if(!t_real.empty()) {
        the_t = t_real[0];
        the_i = t_ival[0];
        //if t's size is bigger than 1
        for(int i = 1; i < t_real.size(); i++) {
            if(std::abs(t_real[i] - _Last_Point.t) < std::abs(the_t - _Last_Point.t)) {
                the_t = t_real[i];
                the_i = t_ival[i];
            }
        }
    }
    //if no solution muched due to any errors
    else {
#ifdef DEBUG_GETY
        g_Serial_Signal.printf("LastPoint\n");
#endif
        the_t = _Last_Point.t;
        for (int i = 0; i < _Sample_Num - 1; i++)
            if(_Sample_Set[i].t <= the_t && the_t <= _Sample_Set[i+1].t)
                the_i = i;
    }

    //
    /* */if (the_t < _Sample_Set[0].t) the_t = _Sample_Set[0].t;
    else if (_Sample_Set[_Sample_Num - 1].t < the_t) the_t = _Sample_Set[_Sample_Num - 1].t;

    //
    for(int i = 0; i < 4; i++) C[i] = _C_y[i][the_i];
    y = _cubic_f(the_t - _Sample_Set[the_i].t, C);
#ifdef DEBUG_GETY
    g_Serial_Signal.printf("(the_t, the_i):= (%f , %d)\n",the_t, the_i);
#endif
    _Last_Point = (Vxyt) {
        y, arg_x, the_t
    };
    return y;
}



void CubicSpline2d::saveSetting()
{
    FILE *fp;

    fp = fopen("/local/savedata.log", "wb");

    //  Save _Sample_Num
    fwrite(&_Sample_Num, sizeof(unsigned int), 1, fp);
    fputc(0x3b, fp);
    //  Save _Sample_Set
    for(int i = 0; i < _Sample_Num; i++) {
        fwrite(&_Sample_Set[i].x,    sizeof(double), 1, fp);
        fputc(0x2c, fp);
        fwrite(&_Sample_Set[i].y,    sizeof(double), 1, fp);
        fputc(0x2c, fp);
        fwrite(&_Sample_Set[i].t,    sizeof(double), 1, fp);
        fputc(0x3b, fp);
    }
    //  Save _C_x
    for(int i = 0; i < _Sample_Num - 1; i++) {
        for(int j = 0; j < 4; j++) {
            fwrite(&_C_x[j][i],    sizeof(double), 1, fp);
            fputc((j != 3)? 0x2c : 0x3b, fp);
        }
    }
    //  Save _C_y
    for(int i = 0; i < _Sample_Num - 1; i++) {
        for(int j = 0; j < 4; j++) {
            fwrite(&_C_y[j][i],    sizeof(double), 1, fp);
            fputc((j != 3)? 0x2c : 0x3b, fp);
        }
    }

    fclose(fp);

}

void CubicSpline2d::saveSetting(
    const char *filename
)
{
    FILE *fp;
    char *filepath;
    int fnnum = 0;

    while (filename[fnnum] != 0) fnnum++;
    filepath = (char *)malloc((fnnum + 8) * sizeof(char)); // "/local/" are 7 char and \0 is 1 char.

    sprintf(filepath, "/local/%s", filename);
    fp = fopen(filepath, "wb");

    //  Save _Sample_Num
    fwrite(&_Sample_Num, sizeof(unsigned int), 1, fp);
    fputc(0x3b, fp);
    //  Save _Sample_Set
    for(int i = 0; i < _Sample_Num; i++) {
        fwrite(&_Sample_Set[i].x,    sizeof(double), 1, fp);
        fputc(0x2c, fp);
        fwrite(&_Sample_Set[i].y,    sizeof(double), 1, fp);
        fputc(0x2c, fp);
        fwrite(&_Sample_Set[i].t,    sizeof(double), 1, fp);
        fputc(0x3b, fp);
    }
    //  Save _C_x
    for(int i = 0; i < _Sample_Num - 1; i++) {
        for(int j = 0; j < 4; j++) {
            fwrite(&_C_x[j][i],    sizeof(double), 1, fp);
            fputc((j != 3)? 0x2c : 0x3b, fp);
        }
    }
    //  Save _C_y
    for(int i = 0; i < _Sample_Num - 1; i++) {
        for(int j = 0; j < 4; j++) {
            fwrite(&_C_y[j][i],    sizeof(double), 1, fp);
            fputc((j != 3)? 0x2c : 0x3b, fp);
        }
    }

    fclose(fp);
    free(filepath);
}

void CubicSpline2d::loadSetting()
{
    FILE *fp;
    char tmp;

    //sprintf(filepath, "/local/%s", filename);
    //fp = fopen(filepath, "rb");
    fp = fopen("/local/savedata.log", "rb");

    //  Load _Sample_Num
    fread(&_Sample_Num, sizeof(unsigned int),  1, fp);
    fread(&tmp,         sizeof(char), 1, fp);

    //  Load _Sample_Set
    for(int i = 0; i < _Sample_Num; i++) {
        fread(&_Sample_Set[i].x, sizeof(double), 1, fp);
        fread(&tmp, sizeof(char),1,fp);
        fread(&_Sample_Set[i].y, sizeof(double), 1, fp);
        fread(&tmp, sizeof(char),1,fp);
        fread(&_Sample_Set[i].t, sizeof(double), 1, fp);
        fread(&tmp, sizeof(char),1,fp);
    }

    //  Load _C_x
    for(int i = 0; i < _Sample_Num - 1; i++) {
        for(int j = 0; j < 4; j++) {
            fread(&_C_x[j][i], sizeof(double), 1, fp);
            fread(&tmp, sizeof(char),1,fp);
        }
    }
    //  Load _C_y
    for(int i = 0; i < _Sample_Num - 1; i++) {
        for(int j = 0; j < 4; j++) {
            fread(&_C_y[j][i], sizeof(double), 1, fp);
            fread(&tmp, sizeof(char),1,fp);
        }
    }
    fclose(fp);
}


void CubicSpline2d::loadSetting(
    const char *filename
)
{
    FILE *fp;
    char *filepath;
    char tmp;
    int fnnum = 0;

    while (filename[fnnum] != 0) fnnum++;
    filepath = (char *)malloc((fnnum + 8) * sizeof(char)); // "/local/" are 7 char and \0 is 1 char.

    sprintf(filepath, "/local/%s", filename);
    fp = fopen(filepath, "rb");

    //  Load _Sample_Num
    fread(&_Sample_Num, sizeof(unsigned int),  1, fp);
    fread(&tmp,         sizeof(char), 1, fp);

    //  Load _Sample_Set
    for(int i = 0; i < _Sample_Num; i++) {
        fread(&_Sample_Set[i].x, sizeof(double), 1, fp);
        fread(&tmp, sizeof(char),1,fp);
        fread(&_Sample_Set[i].y, sizeof(double), 1, fp);
        fread(&tmp, sizeof(char),1,fp);
        fread(&_Sample_Set[i].t, sizeof(double), 1, fp);
        fread(&tmp, sizeof(char),1,fp);
    }

    //  Load _C_x
    for(int i = 0; i < _Sample_Num - 1; i++) {
        for(int j = 0; j < 4; j++) {
            fread(&_C_x[j][i], sizeof(double), 1, fp);
            fread(&tmp, sizeof(char),1,fp);
        }
    }

    //  Load _C_y
    for(int i = 0; i < _Sample_Num - 1; i++) {
        for(int j = 0; j < 4; j++) {
            fread(&_C_y[j][i], sizeof(double), 1, fp);
            fread(&tmp, sizeof(char),1,fp);
        }
    }
    fclose(fp);
    free(filepath);
}

void CubicSpline2d::printOutData()
{
    FILE *fp;
    double x,y,t;
    double d = (_Sample_Set[_Sample_Num - 1].x - _Sample_Set[0].x) / 100.0;

    fp = fopen("/local/log.txt", "w");  // open file in writing mode

    fprintf(fp, "x, y, (t)\n");
    for(int ival = 0; ival < _Sample_Num - 1; ival++) {
        fprintf(fp, "ival: %d \n", ival);
        for(x = _Sample_Set[ival].x; x <= _Sample_Set[ival + 1].x; x += d) {
            y = getY(x);
            if(ival == 0)
                t = sqrt((x - _Sample_Set[ival].x)*(x - _Sample_Set[ival].x) + (x - _Sample_Set[ival].y)*(x - _Sample_Set[ival].y));
            else
                t = _Sample_Set[ival-1].t + sqrt((x - _Sample_Set[ival-1].x)*(x - _Sample_Set[ival-1].x) + (x - _Sample_Set[ival].y)*(x - _Sample_Set[ival].y));
            fprintf(fp, "%f,%f,(%f)\n", x, y, t);
        }
        fprintf(fp, "-----------------------------------------\n");
    }

    fprintf(fp, "\nSample:dst, vol\n");
    for(int i = 0; i < _Sample_Num; i++) {
        fprintf(fp, "%f,%f,(%f)\n", _Sample_Set[i].x, _Sample_Set[i].y, _Sample_Set[i].t);
    }
    fclose(fp);

}
void CubicSpline2d::_printOutData(const double *arg, const int num, const char* name)
{
    FILE *fp;

    fp = fopen("/local/varlog.txt", "a");  // open file in add mode
    fprintf(fp, "%10s\n", name);
    for(int i = 0; i < num; i++) {
        fprintf(fp, "%.2f, ", arg[i]);
    }
    fprintf(fp, "\n");
    fclose(fp);
}
void CubicSpline2d::_printOutDataCouple(const double *arg1, const double *arg2, const int num, const char* name)
{
    FILE *fp;

    fp = fopen("/local/varlog.txt", "a");  // open file in add mode
    fprintf(fp, "%10s\n", name);
    for(int i = 0; i < num; i++) {
        fprintf(fp, "(%.2f, %.2f)\n", arg1[i], arg2[i]);
    }
    fprintf(fp, "\n");
    fclose(fp);
}
void CubicSpline2d::_printOutData(const Vxyt *arg, int num, const char* name)
{
    FILE *fp;

    fp = fopen("/local/varlog.txt", "a");  // open file in add mode
    fprintf(fp, "%10s\n", name);
    for(int i = 0; i < num; i++) {
        fprintf(fp, "%f, ", arg[i].y);
    }
    fprintf(fp, "\n");
    fclose(fp);
}