#include "stepSeq.h"


/*
int main(void) {

  StepControl_t stepme;
  StepperSet_t  motors;
  
  stepInit(&motors);
  
  stepme = M1F | M2R | M4F ;
  
  dumpStates(&motors.phases);// debug
  stepmotors(&motors, stepme);
  dumpStates(&motors.phases);// debug
    
  return 0;
}

*/

void stepInit( StepperSet_t * this) {
  this->phases.solo.M1 = 0;
  this->phases.solo.M2 = 0;
  this->phases.solo.M3 = 0;
  this->phases.solo.M4 = 0;
}

void stepmotors( StepperSet_t * this, StepControl_t these ) {

  unsigned char stepFor[] = { 1, 3, 0, 2 }; // mini state machinie :)
  unsigned char stepBac[] = { 2, 0, 3, 1 }; 
  
  
  
  if (these & M1F)  this->phases.solo.M1 = stepFor[this->phases.solo.M1];  
  if (these & M1R)  this->phases.solo.M1 = stepBac[this->phases.solo.M1];
  
  if (these & M2F)  this->phases.solo.M2 = stepFor[this->phases.solo.M2];
  if (these & M2R)  this->phases.solo.M2 = stepBac[this->phases.solo.M2];
  
  if (these & M3F)  this->phases.solo.M3 = stepFor[this->phases.solo.M3];
  if (these & M3R)  this->phases.solo.M3 = stepBac[this->phases.solo.M3];
 
  if (these & M4F)  this->phases.solo.M4 = stepFor[this->phases.solo.M4];
  if (these & M4R)  this->phases.solo.M4 = stepBac[this->phases.solo.M4];
  

}

void SyncMotors(StepperSet_t * this) {

  stepmotors(this, this->stepBuffer);
  this->stepBuffer = 0;

}


void dumpStates( PhaseBlock_t this ) {
 
  printf("-----------------------\n");
  printf("Motor 1: "); printBin(this.solo.M1); printf("\n");
  printf("Motor 2: "); printBin(this.solo.M2); printf("\n");
  printf("Motor 3: "); printBin(this.solo.M3); printf("\n");
  printf("Motor 4: "); printBin(this.solo.M4); printf("\n");
  printf("-----------------------\n");
}



// just 2 digits thanks
void printBin(unsigned char this) {

  unsigned char n;
  for (n = 0; n < 2; n++) {
    printf("%d", (this & 2)!=0);
    this = this << 1;
  }
  
}