// © 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 <LinkAxisCommand.h>

#include <Timers.h>

#include <InterpolationTimer.h>

#include <Axis.h>

#include <Link.h>


const char LinkAxisCommand::myName[] =

     "LinkAxis";


const char*

     LinkAxisCommand::name() {

     

     return

          myName; };


LinkAxisCommand::LinkAxisCommand( char* data ) {

     firstLinkAxis          = NULL;

     char coordinate[20];

     uint32_t length          = 0;


     axisNameC       = data[0];

     while( sscanf( data+1,

               " %s%ln",

               &coordinate,

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


          LinkAxis* linkAxisPtr      = new LinkAxis( coordinate );

          linkAxisPtr->nextLinkAxis  = firstLinkAxis;

          firstLinkAxis              = linkAxisPtr;

          data                       += length; }; };


LinkAxisCommand::~LinkAxisCommand() {


    LinkAxis*  linkAxis = firstLinkAxis;

    while ( linkAxis ) {

          LinkAxis* deleteLinkAxis     = linkAxis;

          linkAxis                         = linkAxis->nextLinkAxis;

          delete deleteLinkAxis; }; };


bool

     LinkAxisCommand::execute( Machine* machine ) {


    // Machine coordinate space is subordinate to program coordinate space

    subordinateAxis      = machine->findAxis(

          axisNameC,

          NULL );

    if ( ! subordinateAxis )

          return

               true;     // error


     // update the CTM for this axis

     subordinateAxis->deleteSuperiorLinks();


     LinkAxis* linkAxis  = firstLinkAxis;

     while ( linkAxis ) {

          Axis* superiorAxis  = machine->findAxis(

               linkAxis->axisNameC,

               NULL );


          if ( superiorAxis && linkAxis->scale != 0.0 ) {

               Link* link     = new Link(

                    superiorAxis,

                    subordinateAxis,

                    linkAxis->scale );


               link->nextSuperiorLink              = subordinateAxis->firstSuperiorLink;

               subordinateAxis->firstSuperiorLink  = link;


               link->nextSubordinateLink           = superiorAxis->firstSubordinateLink;

               superiorAxis->firstSubordinateLink  = link; };


          linkAxis  = linkAxis->nextLinkAxis; };


    return true; };


void

     LinkAxisCommand::report(

          Machine*     machine,

          String*          msgPtr ) {


     *msgPtr

          +=     " Link "

          +     String( axisNameC );


     LinkAxis*  linkAxis = firstLinkAxis;

     while ( linkAxis ) {

          linkAxis->report( msgPtr );

          linkAxis  = linkAxis->nextLinkAxis; };


     *msgPtr

          +=     "\n"; };