#include "interlin.h"

void axisInit(axies_t * this, void (*Sync)(void)) {
 this->axii = NULL;
 this->axisCount = 0;
 this->Sync = Sync;
}

void axisAdd(axies_t * this, int start, int end, void (*StepFor)(void), void (*StepBak)(void)) {
  this->axisCount++;
  this->axii = realloc(this->axii, sizeof(axis_t) * this->axisCount);
  this->axii[(this->axisCount)-1].start    = start;
  this->axii[(this->axisCount)-1].current  = start;
  this->axii[(this->axisCount)-1].target   = end;
  this->axii[(this->axisCount)-1].StepFor  = StepFor;
  this->axii[(this->axisCount)-1].StepBak  = StepBak;
}

void dumpaxies(axies_t * this ) {
  long i;
  
  for (i = 0; i < this->axisCount; i++) {
    printf( "%d: %d  %d  %d\n", i, this->axii[i].start, this->axii[i].current, this->axii[i].target);
  }
  printf("\n");
}


void SimotaniouslyLinearlyInterpolateMultiAxis(axies_t * this) {
  int longest;
  int i, j;
  int temp;
    
  // set it all up, work out how big our master index is  
  longest = 0;
  for (i = 0; i < this->axisCount; i++) {
    this->axii[i].current = this->axii[i].start;
    if (abs(this->axii[i].target - this->axii[i].start) > abs(longest))   longest = abs(this->axii[i].target - this->axii[i].start);
  }     
  
  if (longest == 0) return;  // nothing to move here!
   
  for ( i = 0; i <= longest; i++) {
    for (j = 0; j < this->axisCount; j++) {   
    // y2 = y1 + (x2 - x1) * ((y3-y1) / (x3-x1))  ; where X is master, 1 is initial, 2 is current, and 3 is target
      temp = this->axii[j].start + (i * (this->axii[j].target - this->axii[j].start)) / longest;
      if (0) { // we need to do this the long way so we can do the callbacks right
      } else if (this->axii[j].current > temp ) {
        this->axii[j].current--;
        (*this->axii[j].StepBak)();
      } else if (this->axii[j].current < temp ) {
        this->axii[j].current++;
        (*this->axii[j].StepFor)();
      }
        
    //  printf("%d  ", this->axii[j].current);
    }
    (*this->Sync) ();
  //  printf("\n");
  }
 
}




//=============================-- Testing stuff --=================================


/*

void setFor1() {
  printf("1 Stepping forward\n");
}

void setBak1() {
  printf("1 Stepping back \n");
}

void setFor2() {
  printf("2 Stepping forward\n");
}

void setBak2() {
  printf("2 Stepping back \n");
}

void setFor3() {
  printf("3 Stepping forward\n");
}

void setBak3() {
  printf("3 Stepping back \n");
}

void setFor4() {
  printf("4 Stepping forward\n");
}

void setBak4() {
  printf("4 Stepping back \n");
}

void motSync() {
  printf("Sync \n");
}



int main(void) {

  axies_t axies;

  axisInit( &axies, motSync );
  axisAdd(&axies,  2,  27, setFor1, setBak1);
  axisAdd(&axies, -4, -18, setFor2, setBak2);
  axisAdd(&axies,  9, -10, setFor3, setBak3);
  axisAdd(&axies,  -10, 40, setFor4, setBak4);
  dumpaxies( &axies );
  SimotaniouslyLinearlyInterpolateMultiAxis( &axies );
  
 
  return 0;

}


*/