/******************************************************************
 * Arduino Command Line Interface - Stepper motor rev 0.1
 * Quentin Berthet // modprobe.ch
 * tumnao@gmail.com
 * 3.10.2010
 *****************************************************************/

/* Pin definition ************************************************/
int SR_pin = 6;
int MS1_pin = 4;
int MS2_pin = 7;
int ENABLE_pin = 5;
int DIR_pin = 3;
int STEP_pin = 2;

/* Commands arrays ***********************************************/
const char* cmd_array[] = {"help",
                           "delay",
                           "step",
                           "dir",
                           "start",
                           "stop",
                           0};

typedef void(*cmd_func_ptr)(char*);                           
cmd_func_ptr cmd_array_func_ptr[] = {&display_help,
                                     &set_delay,
                                     &set_step_mode,
                                     &set_dir,
                                     &do_start,
                                     &do_stop,
                                     0};

/* Globals *******************************************************/
unsigned long delay_between_step = 100000; //0.1 s
byte step_mode = 1;
byte dir = 0;
byte motor_started = 0;
unsigned long step_to_do = 0;

/* Motor control *************************************************/
void do_step(void)
{
  delayMicroseconds(2);  //datasheet specify 1us min
  digitalWrite(STEP_pin, HIGH);
  delayMicroseconds(2);  //datasheet specify 1us min
  digitalWrite(STEP_pin, LOW);
  if (motor_started == 2) 
    if (--step_to_do == 0)
    {
      motor_started = 0;
      digitalWrite(ENABLE_pin, HIGH);
    }
}

void check_deadline()
{
  static unsigned long next_dead_line = 0;

  /* TODO : add micros() overflow check */  
  if (next_dead_line <= micros())
  {
    do_step();
    next_dead_line = micros() + delay_between_step;
  }
}

void change_step_mode(int s)
{
  switch(s)
  {
    case 1:
      digitalWrite(MS1_pin, LOW);     
      digitalWrite(MS2_pin, LOW);
      break;
    case 2:
      digitalWrite(MS1_pin, HIGH);     
      digitalWrite(MS2_pin, LOW);
      break;
    case 4:
      digitalWrite(MS1_pin, LOW);     
      digitalWrite(MS2_pin, HIGH);
      break;
    case 8:
      digitalWrite(MS1_pin, HIGH);     
      digitalWrite(MS2_pin, HIGH);
      break;
    default:
      break;
  }
}

/* Command line Functions ****************************************/

void display_help(char* cmd)
{
  int i=0;
  Serial.println("Command list:");
  while (cmd_array[i])
  {
    Serial.print("  ");
    Serial.println(cmd_array[i++]);
  }
}

void do_start(char* cmd)
{
  unsigned long temp;
  
  if (!motor_started)
  {
    /* TODO : add if delay is below a limit, 
     * use an acceleration curve */  
    if (sscanf(cmd,"%*s %lu", &temp) == 1)
    {
      step_to_do = temp;
      motor_started = 2;
      digitalWrite(ENABLE_pin, LOW);
      Serial.print("Start motor to do ");
      Serial.print(step_to_do);
      Serial.println(" steps");
    }
    else
    {
      motor_started = 1;
      digitalWrite(ENABLE_pin, LOW);
      Serial.println("Start motor");
    }
  }
  else 
  {
    Serial.println("Motor is already started");
  }
}

void do_stop(char* cmd)
{
  if (motor_started)
  {
    motor_started = 0;
    digitalWrite(ENABLE_pin, HIGH);
    if (step_to_do > 0)
    {
      Serial.print("Motor stopped, but ");
      Serial.print(step_to_do);
      Serial.println(" steps to do remaining...");
    }
    else
      Serial.println("Motor stopped");
      
    step_to_do = 0;
  }
  else
  {
    Serial.println("Error: motor already stopped");
  }
}

void set_delay(char* cmd)
{
  unsigned long temp;
  
  if (sscanf(cmd,"%*s %lu", &temp) == 1)
    delay_between_step = temp;
   
  Serial.print("delay between step : ");
  Serial.print(delay_between_step);
  Serial.println(" us");
}

void set_step_mode(char* cmd)
{
  int temp;
  
  if (sscanf(cmd,"%*s %d", &temp) == 1)
  {
    if ((temp == 1) ||
        (temp == 2) ||
        (temp == 4) ||
        (temp == 8))
    {
      step_mode = temp;
      change_step_mode(step_mode);
    }
    else
      Serial.println("Error: step-mode must be 1,2,4,8");
  }
  switch (step_mode)
  {
    case 1:
      Serial.println("Full-step");
      break;
    case 2:
      Serial.println("Half-step");
      break;
    case 4:
      Serial.println("Quarter-step");
      break;
    case 8:
      Serial.println("Eighth-step");
      break;
  }  
}

void set_dir(char* cmd)
{
  int temp;
  
  if (sscanf(cmd,"%*s %d", &temp) == 1)
  {
    if (temp == 0)
      dir = 0;
    else
      dir = 1;
    digitalWrite(DIR_pin, dir);
  }
  Serial.print("direction : ");
  Serial.println(dir, DEC);
}

/* Command line parsing ******************************************/

void parse_command(char* cmd)
{
  byte i = 0;
  
  do
    if (strncmp(cmd_array[i], cmd, strlen(cmd_array[i])) == 0)
      return (*cmd_array_func_ptr[i])(cmd);
  while(cmd_array[i++]);
  
  Serial.print("unkown command: ");
  Serial.println(cmd);
}

void push_byte_to_cmd(char b)
{
  static char cmd_buf[32];
  static char cmd_ptr = 0;

  if (b == '\n')
  {
    cmd_buf[cmd_ptr] = 0; //assure that there is a null byte
    Serial.println();
    parse_command(cmd_buf);  
    Serial.print(">");
    //clear command
    cmd_ptr = 0;
    memset(cmd_buf, 0, sizeof(cmd_buf));
  }
  else if(isprint(b))
  {
    Serial.print(b);
    cmd_buf[cmd_ptr++] = b;
    if (cmd_ptr >= sizeof(cmd_buf)-1) //1 for null byte
    {
      Serial.println();
      Serial.println("Command too long");
      cmd_ptr = 0;
      memset(cmd_buf, 0, sizeof(cmd_buf));
    }
  }
}

/* Main **********************************************************/

void setup(void)
{
  pinMode(SR_pin,     OUTPUT);
  pinMode(MS1_pin,    OUTPUT);
  pinMode(MS2_pin,    OUTPUT);
  pinMode(ENABLE_pin, OUTPUT);
  pinMode(DIR_pin,     OUTPUT);
  pinMode(STEP_pin,   OUTPUT);
  //A3977 config
  digitalWrite(ENABLE_pin, HIGH); // disable input
  digitalWrite(SR_pin, LOW);  // low: active mode off
  step_mode = 1;
  change_step_mode(step_mode);  // start with full steps
  dir = 0;
  digitalWrite(DIR_pin, dir);
  digitalWrite(STEP_pin, LOW);
  Serial.begin(115200);
  Serial.println("arduino CLI v0.1");
  Serial.print(">");
}

void loop(void)
{
  if (motor_started > 0) check_deadline();
  if (Serial.available() > 0) push_byte_to_cmd(Serial.read());
}

/* EOF ***********************************************************/