// © RiceMotion ( Robert Carl Rice ) 2012-2016 - All rights reserved


// This software makes use of tools and libraries obtained from open source projects or released for

// use by relevant hardware manufactures. However, this software is NOT a part of any open source project.

// It is released only on a "need to know" basis for beta testers of the "RiceCNC Interpolation Engine".

// Recipents of this source code must respect the confidential nature of this software and prevent any

// distribution that could result in counterfeit copies of the "RiceCNC Interpolation Engine".


// © RiceMotion ( Robert Carl Rice ) 2012-2016 - All rights reserved

#include <SplineCommand.h>

#include <Machine.h>

#include <MachineCommands.h>

#include <Axis.h>

#include <SysTick.h>

#include <MainCommands.h>

#include <InterpolationTimer.h>

#include <Probe.h>

#include <SplineAxis.h>


// derived classes

#include <ProbingSplineCommand.h>


const char Spline::myName[] =

     "Spline";


const char*

     Spline::name() {


     return

          myName; };


// Initializer

Spline::Spline(

     char*          data,

     Axis*          firstMachineAxis,

     String*          msgPtr  ) {


     stepControl.number     = 0;

     uint32_t length          = 0;


     sscanf( data,

          "%lu%ln",

          &stepControl.number,

          &length );

     data          += length;


     firstAxis     = NULL;


     char     coordinate[40];


     while( sscanf( data,

          " %s%ln",

          &coordinate,

          &length ) > 0 ) { // returns -1 at eof


          SplineAxis* splineAxisPtr     = new SplineAxis(

               coordinate,

               firstMachineAxis,

               msgPtr );

     

          if ( splineAxisPtr->valid() ) {

               splineAxisPtr->nextSplineAxis     = firstAxis;

               firstAxis                              = splineAxisPtr; }


          else

               delete splineAxisPtr;


          data     += length; };


     aborting     = false;


     stepControl.t                         = 0.0;

     stepControl.status                    = ' ';

     stepControl.progress.minimum     = 0;

     stepControl.progress.maximum     = 0;


     nextCommand          = NULL; };



Spline::~Spline() {

     while ( firstAxis ) {

        SplineAxis* splineAxisPtr     = firstAxis;

        firstAxis                         = splineAxisPtr->nextSplineAxis;

        delete splineAxisPtr; }; };


void

     Spline::broadcastToSplineAxes( BroadcastToSplineAxes method ) {


     SplineAxis* splineAxisPtr  = firstAxis;

     while ( splineAxisPtr ) {

        (splineAxisPtr->*method)();

        splineAxisPtr   = splineAxisPtr->nextSplineAxis; }; };


void

     Spline::trackPadJogAbort(

          Machine*     machine ) {


     aborting = true; };



bool // This method performs program position linear and Bezier interpolation

     Spline::execute(

          Machine* machine ) {


     if ( aborting )

          return

               true;


     // The target changes for the axes included in the interpolation and linked axes

     machine->adjustStepSize( &stepControl );

     calculateProgramTarget();     // Calucalte the target position for interpolated axes


     return

          stepControl.t == 1.0; };



void

     Spline::report(

          Machine*     machine,

          String*          msgPtr ) {


     machine->reportSplineCompleted(

          &stepControl,

          msgPtr );


     if ( aborting )

          *msgPtr     += " Aborted";


     *msgPtr     += "\n"; };


void

     Spline::reportInterpolationStatus(

          Machine*     machine,

          String*          msgPtr ) {


     machine->reportInterpolationStatus(

          &stepControl,

          msgPtr ); };


void

     Spline::calculateProgramTarget() { // Calculate the target positions for new t


     if ( stepControl.t > 1.0 )

          stepControl.t = 1.0;


     // Intermediates for optimization

     float     t2     =

               stepControl.t

          *     stepControl.t;          // t squared


     float     it1     =

               1.0

          -     stepControl.t;          // 1 - t

     

     float     p2f     =

               3.0

          *     stepControl.t

          *     it1

          *     it1;


     float     p3f     =

               3.0

          *     t2

          *     it1;


     float     p4f     =

               t2

          *     stepControl.t;          // t cubed


     // Calculate a target position for each axis

     SplineAxis* splineAxisPtr  = firstAxis;

     while ( splineAxisPtr ) {

          splineAxisPtr->calculateProgramTarget(

               stepControl.t,

               p2f,

               p3f,

               p4f );

          splineAxisPtr     = splineAxisPtr->nextSplineAxis; }; };