// Program to test sequencing of coils on stepper motor since some of
// the cheap 28BYJ-48 motors I have seem to be wired wrong. If it is
// just the wrong wire in the socket position, should be able to deduce
// which positions are the right wires by observation (that's the theory).
//
// Turns out the wires aren't wrong at all, what I really discovered using this
// program was that the 800 microsecond delay wasn't good enough for three
// of the five steppers. Bump it up to 900, and they all work.
//
// In any case, this is a fairly useful program for demonstrating how
// to run stepper motors with libgpiod on the raspberry pi.

#include <gpiod.h>
#include <stdio.h>
#include <unistd.h>

// The gpio pins I connect to the driver board
#define BLUE_GPIO 22
#define PINK_GPIO 23
#define YELLOW_GPIO 24
#define ORANGE_GPIO 25

#define GPIO_DELAY 1800

struct gpiod_chip *chip;
struct gpiod_line *lineBlue;
struct gpiod_line *linePink;
struct gpiod_line *lineYellow;
struct gpiod_line *lineOrange;

// Colors of the four wires in the stepper socket
const char * names[4] = {"blue", "pink", "yellow", "orange"};

// All possible orderings of the sequence of 4 pins.
//
int perms[24][4] = {
   {0, 1, 2, 3},
   {0, 1, 3, 2},
   {0, 2, 1, 3},
   {0, 2, 3, 1},
   {0, 3, 2, 1},
   {0, 3, 1, 2},
   {1, 0, 2, 3},
   {1, 0, 3, 2},
   {1, 2, 0, 3},
   {1, 2, 3, 0},
   {1, 3, 2, 0},
   {1, 3, 0, 2},
   {2, 1, 0, 3},
   {2, 1, 3, 0},
   {2, 0, 1, 3},
   {2, 0, 3, 1},
   {2, 3, 0, 1},
   {2, 3, 1, 0},
   {3, 1, 2, 0},
   {3, 1, 0, 2},
   {3, 2, 1, 0},
   {3, 2, 0, 1},
   {3, 0, 2, 1},
   {3, 0, 1, 2}
};

int curperm=-1;

int step_state = 0;
int step_dir = 1;

#define WAVE_STEP 0
#define FULL_STEP 1
#define HALF_STEP 2

int driver=WAVE_STEP;

void
print_curperm() {
   switch(driver) {
   case WAVE_STEP:
      printf("Using wave stepping.\n");
      break;
   case FULL_STEP:
      printf("Using full stepping.\n");
      break;
   case HALF_STEP:
      printf("Using half stepping.\n");
      break;
   }
   printf("permutation[%d]: %s, %s, %s, %s\n",
      curperm,
      names[perms[curperm][0]],
      names[perms[curperm][1]],
      names[perms[curperm][2]],
      names[perms[curperm][3]]);

}

void
reset_step_state() {
   step_state = 0;
   step_dir = 1;
   gpiod_line_set_value(lineBlue, 0);
   gpiod_line_set_value(linePink, 0);
   gpiod_line_set_value(lineYellow, 0);
   gpiod_line_set_value(lineOrange, 0);
}

void
wave_back_forth() {
   int i,j,k;
   struct gpiod_line * lines[4];
   lines[perms[curperm][0]] = lineBlue;
   lines[perms[curperm][1]] = linePink;
   lines[perms[curperm][2]] = lineYellow;
   lines[perms[curperm][3]] = lineOrange;
   for (i = 0; i < 2; ++i) {
      for (j = 0; j < 2048; ++j) {
         switch(step_state) {
         case 0:
            gpiod_line_set_value(lines[0],1);
            gpiod_line_set_value(lines[1],0);
            gpiod_line_set_value(lines[2],0);
            gpiod_line_set_value(lines[3],0);
            break;
         case 1:
            gpiod_line_set_value(lines[0],0);
            gpiod_line_set_value(lines[1],1);
            gpiod_line_set_value(lines[2],0);
            gpiod_line_set_value(lines[3],0);
            break;
         case 2:
            gpiod_line_set_value(lines[0],0);
            gpiod_line_set_value(lines[1],0);
            gpiod_line_set_value(lines[2],1);
            gpiod_line_set_value(lines[3],0);
            break;
         case 3:
            gpiod_line_set_value(lines[0],0);
            gpiod_line_set_value(lines[1],0);
            gpiod_line_set_value(lines[2],0);
            gpiod_line_set_value(lines[3],1);
            break;
         }
         usleep(GPIO_DELAY); // Too small and motor doesn't turn, just vibrate
         step_state += step_dir;
         if (step_state < 0) {
            step_state = 3;
         } else if (step_state > 3) {
            step_state = 0;
         }
      }
      step_dir = - step_dir;
   }
}

void full_back_forth() {
   int i,j,k;
   struct gpiod_line * lines[4];
   lines[perms[curperm][0]] = lineBlue;
   lines[perms[curperm][1]] = linePink;
   lines[perms[curperm][2]] = lineYellow;
   lines[perms[curperm][3]] = lineOrange;
   for (i = 0; i < 2; ++i) {
      for (j = 0; j < 2048; ++j) {
         switch(step_state) {
         case 0:
            gpiod_line_set_value(lines[0],1);
            gpiod_line_set_value(lines[1],1);
            gpiod_line_set_value(lines[2],0);
            gpiod_line_set_value(lines[3],0);
            break;
         case 1:
            gpiod_line_set_value(lines[0],0);
            gpiod_line_set_value(lines[1],1);
            gpiod_line_set_value(lines[2],1);
            gpiod_line_set_value(lines[3],0);
            break;
         case 2:
            gpiod_line_set_value(lines[0],0);
            gpiod_line_set_value(lines[1],0);
            gpiod_line_set_value(lines[2],1);
            gpiod_line_set_value(lines[3],1);
            break;
         case 3:
            gpiod_line_set_value(lines[0],1);
            gpiod_line_set_value(lines[1],0);
            gpiod_line_set_value(lines[2],0);
            gpiod_line_set_value(lines[3],1);
            break;
         }
         usleep(GPIO_DELAY); // Too small and motor doesn't turn, just vibrate
         step_state += step_dir;
         if (step_state < 0) {
            step_state = 3;
         } else if (step_state > 3) {
            step_state = 0;
         }
      }
      step_dir = - step_dir;
   }
}

void half_back_forth() {
   int i,j,k;
   struct gpiod_line * lines[4];
   lines[perms[curperm][0]] = lineBlue;
   lines[perms[curperm][1]] = linePink;
   lines[perms[curperm][2]] = lineYellow;
   lines[perms[curperm][3]] = lineOrange;
   for (i = 0; i < 2; ++i) {
      for (j = 0; j < 4096; ++j) {
         switch(step_state) {
         case 0:
            gpiod_line_set_value(lines[0],1);
            gpiod_line_set_value(lines[1],0);
            gpiod_line_set_value(lines[2],0);
            gpiod_line_set_value(lines[3],0);
            break;
         case 1:
            gpiod_line_set_value(lines[0],1);
            gpiod_line_set_value(lines[1],1);
            gpiod_line_set_value(lines[2],0);
            gpiod_line_set_value(lines[3],0);
            break;
         case 2:
            gpiod_line_set_value(lines[0],0);
            gpiod_line_set_value(lines[1],1);
            gpiod_line_set_value(lines[2],0);
            gpiod_line_set_value(lines[3],0);
            break;
         case 3:
            gpiod_line_set_value(lines[0],0);
            gpiod_line_set_value(lines[1],1);
            gpiod_line_set_value(lines[2],1);
            gpiod_line_set_value(lines[3],0);
            break;
         case 4:
            gpiod_line_set_value(lines[0],0);
            gpiod_line_set_value(lines[1],0);
            gpiod_line_set_value(lines[2],1);
            gpiod_line_set_value(lines[3],0);
            break;
         case 5:
            gpiod_line_set_value(lines[0],0);
            gpiod_line_set_value(lines[1],0);
            gpiod_line_set_value(lines[2],1);
            gpiod_line_set_value(lines[3],1);
            break;
         case 6:
            gpiod_line_set_value(lines[0],0);
            gpiod_line_set_value(lines[1],0);
            gpiod_line_set_value(lines[2],0);
            gpiod_line_set_value(lines[3],1);
            break;
         case 7:
            gpiod_line_set_value(lines[0],1);
            gpiod_line_set_value(lines[1],0);
            gpiod_line_set_value(lines[2],0);
            gpiod_line_set_value(lines[3],1);
            break;
         }
         usleep(GPIO_DELAY/2); // Turn half as much, delay half as much
         step_state += step_dir;
         if (step_state < 0) {
            step_state = 7;
         } else if (step_state > 7) {
            step_state = 0;
         }
      }
      step_dir = - step_dir;
   }
}

int main(int argc, char **argv)
{
   const char *chipname = "gpiochip0";
   char cmd[80];

   // Open GPIO chip
   chip = gpiod_chip_open_by_name(chipname);

   // Open GPIO lines
   lineBlue = gpiod_chip_get_line(chip, BLUE_GPIO);
   linePink = gpiod_chip_get_line(chip, PINK_GPIO);
   lineYellow = gpiod_chip_get_line(chip, YELLOW_GPIO);
   lineOrange = gpiod_chip_get_line(chip, ORANGE_GPIO);

   // Open lines for output
   gpiod_line_request_output(lineBlue, "stepcheck", 0);
   gpiod_line_request_output(linePink, "stepcheck", 0);
   gpiod_line_request_output(lineYellow, "stepcheck", 0);
   gpiod_line_request_output(lineOrange, "stepcheck", 0);

   for ( ; ; ) {
      fputs("cmd> ",stdout);
      fflush(stdout);
      fgets(cmd,sizeof(cmd),stdin);
      if (cmd[0]=='p' || cmd[0] == 'P') {
         ++curperm;
         if (curperm >= 24) {
            curperm=0;
         }
         print_curperm();
         reset_step_state();
         switch(driver) {
         case WAVE_STEP:
            wave_back_forth();
            break;
         case FULL_STEP:
            full_back_forth();
            break;
         case HALF_STEP:
            half_back_forth();
            break;
         }
      } else if (cmd[0]=='w' || cmd[0]=='W') {
         driver=WAVE_STEP;
         printf("Use wave stepping to drive motor\n");
      } else if (cmd[0]=='f' || cmd[0]=='F') {
         driver=FULL_STEP;
         printf("Use full stepping to drive motor\n");
      } else if (cmd[0]=='h' || cmd[0]=='H') {
         driver=HALF_STEP;
         printf("Use half stepping to drive motor\n");
      } else if (cmd[0]=='z' || cmd[0]=='Z') {
         printf("Reset current permutation\n");
         curperm=-1;
         reset_step_state();
      } else if (cmd[0]=='q' || cmd[0]=='Q') {
         printf("Bye!\n");
         break;
      } else {
         printf("q - quit\n\
p - try next permutation of wires\n\
z - reset to initial permutation\n\
w - use wave stepping\n\
f - use full stepping\n\
h - use half stepping\n\
otherwise print this help.\n");
      }
   }
   gpiod_line_release(lineBlue);
   gpiod_line_release(linePink);
   gpiod_line_release(lineYellow);
   gpiod_line_release(lineOrange);
   gpiod_chip_close(chip);
   return 0;
}