//
//             |************|
//             |*U1**U2**U3*|
//             |************|
//             |*U4**U5**U6*|
//             |************|
//             |*U7**U8**U9*|
//             |************|
// ************|************|************|************
// *L1**L2**L3*|*F1**F2**F3*|*R1**R2**R3*|*B1**B2**B3*
// ************|************|************|************
// *L4**L5**L6*|*F4**F5**F6*|*R4**R5**R6*|*B4**B5**B6*
// ************|************|************|************
// *L7**L8**L9*|*F7**F8**F9*|*R7**R8**R9*|*B7**B8**B9*
// ************|************|************|************
//             |************|
//             |*D1**D2**D3*|
//             |************|
//             |*D4**D5**D6*|
//             |************|
//             |*D7**D8**D9*|
//             |************|
//
//This is designed for 6 stepper motors - one for each face
//Assume a solution is generated and sent to this controller for execution
//All center faces must be in the proper orientations before executing the code 
//

#include "A4988.h"   //from library StepperDriver

#define MUL 1 
#define RPM 360
const int stepsPerRevolution = 200;  
const int num_motors=6;
const int motorPins[6][2]={{12,14},{27,26},{25,33},{32,23}, {19,21} ,{5,18} }; //{DIR,STEP}
////////////////////////////R         L       D       B        F       U


enum faces{R,L,D,U,F,B};
//         0,1,2,3,4,5


int currentStates[6];//={R,L,D,U,F,B};
//  0=R1, 1=R2, 2=R3
//  3=L1, 4=L2, 5=L3
//  6=D1, 7=D2, 8=D3
//  9=U1,10=U2,11=U3
// 12=F1,13=F2,14=F3
// 15=B1,16=B2,17=B3
// 18=X1,19=X2,20=X3  (rotation about horizontal (WRT R)-axis by 90, 180, 270=-90 degree)
// 21=Y1,22=Y2,23=Y3 (rotation about vertical (WRT D)-axis by 90, 180, 270=-90 degree)
//Test solution
String cubeSolution="R1 U1 D1 R1 U1 L2 F3 R1 F2 B3 L2 D1 R2 U1 R2 D2 L2 D3 B2 D1 (20f)";
//String cubeSolution="L3 U1 B1 R2 F3 L1 F3 U2 L1 U3 B3 U2 B1 L2 F1 U2 R2 L2 B2 (19f)";
//String cubeSolution="L3 D2 R1 (3f)"; //L3 D2 R1 -good
//String cubeSolution="F1 R1 F1 (3f)";  //Y1 R1 Y1 L1 Y1 L1 -good
//String cubeSolution="R1 R1 F1 (3f)";  //R1 R1 Y1 R1 - good
//String cubeSolution="F1 R1 (2f)";  //Y1 R1 Y1 L1 -good
//String cubeSolution="F1 U1 D1 B1 (4f)"; //Y1 R1 X2 D1 X2 D1 L1 -good
//String cubeSolution="Y1 X2 Y3 (3f)"; //Y1
//String cubeSolution="F2 L2 L1 (2f)";  
//String cubeSolution="F1 F2 (1f)";


//God's number is 20. The choice of 60 should be more than enough but it all depends on the algorithm
//chosen to generate a cube solution.
char cubeSol[60][2] ; //from cubeSoution to actual sol consists of standard moves only 
int cubeSolLen=0;

int countMove=0;
A4988 *mySteppers[6];     //(MOTOR_STEPS, DIR, STEP);


void parseSol();//int & len); //read in cubeSolution and output cubeIntSol
//parse the solution. Convert to sols that contains only standard moves plus rotations. 



void setup() {
  Serial.begin(9600);
  // set the speed at 60 rpm:
  for (int i =0;i<num_motors;i++){
    mySteppers[i]=new A4988(stepsPerRevolution, motorPins[i][0], 
                              motorPins[i][1]);
    mySteppers[i]->begin(RPM, MUL);
  }
  
  //Magically we get the len of the cube soution
  parseSol();
  Serial.print("cubeSolution = ");
  Serial.println(cubeSolution);
  Serial.print("Cube len= ");
  Serial.println(cubeSolLen);
  
  Serial.println("Done setup ------");
  delay(200);
 
 Serial.println("Moving ...");
 
}


void loop() { 
  countMove=0;  
  Serial.println("Move Motors ------ ");
  for (;countMove<cubeSolLen;countMove++){
     //Each loop = one solution step
    // step one revolution  in one direction:
    moveValue(cubeSol[countMove][0],cubeSol[countMove][1]);
    delay(200);
  }
  
  delay(1000);
  /*
  //Each loop = one solution step
  // step one revolution  in one direction:
  
  if (countMove<cubeSolLen){
    moveValue(cubeSol[countMove][0],cubeSol[countMove][1]);
    countMove++;
    if (countMove==cubeSolLen){
      Serial.println("Done cubing!");
    }
  }
  delay(200);
  */
}


void parseSol(){  //converts moves into standard moves plus rotations
  int from=0;
  char c1,c2;   
  while(1){//Create cubeIntSol
    c1=cubeSolution.charAt(from++); //motor - R,L,D, (U, F, B)
    c2=cubeSolution.charAt(from);  //angle -  0=90,1=180,2=270 (clockwise)
    from=from +2; //skip a space
    if (isalpha(c1)){ //A  valid move  
      cubeSol[cubeSolLen][0]=c1;
      cubeSol[cubeSolLen][1]=c2;
      cubeSolLen++;
    }else{ //no more moves
      break;
    }
  }
  return;
}


void moveValue(char m, char a){
  String buff="Move=" ;
  int sm, angle=int(a)-int('0');
  boolean changeState=false;
  switch(m){
    case 'R': sm=R; break;
    case 'L': sm=L; break;
    case 'D': sm=D; break;
    case 'U': sm=U; break;
    case 'F': sm=F; break;
    case 'B': sm=B; break;
    default: break;
  }
  if (angle==3){
    angle=-1;
  }
  buff=buff+String(countMove+1)+" m="+String(sm)+
    " a="+String(angle);
  Serial.println(buff);
  
  mySteppers[sm]->move(angle*(stepsPerRevolution/4)*MUL); 
  return;
}