extern "C" {
#include "ramp.h"
}

ramp_t ramp = {};

float new_val = 0;

void help()
{
  Serial.println("h : help (this)");

  Serial.println("b : brake");
  Serial.println("r : release");
  Serial.println("s : goto with end speed");
  Serial.println("n : norm (goto)");
  Serial.println("o : offset (moveby)");
  Serial.println("w : wipe (reset the amount)");

  Serial.println("Enter the amount first");
}

void callback(ramp_t *r, void *cookie) {
  Serial.println("move end");
}

void setup() {
  Serial.begin(115200);
  ramp_init(&ramp);

  ramp_setAccel(&ramp, 300);
  ramp_setSpeed(&ramp, 1000);
  ramp_setDecel(&ramp, 350);
  ramp_setBrakeDecel(&ramp, 500);

  ramp_setCallback(&ramp, &callback, NULL);

  //ramp_setWrap(&ramp, 100);

  help();
}

void loop() {
  while (Serial.available()) {
    char incoming = Serial.read();

    if (incoming == '\n' || incoming == '\r')
      continue;

    switch (incoming)
    {
      case 'h' :
        help();
        break;
      case 'b' :    
        Serial.println(" brake");
        ramp_brake(&ramp);
        new_val = 0;
        break;
      case 'r' :
        Serial.println(" release");
        ramp_release(&ramp);
        new_val = 0;
        break;
      case 's' :    
        Serial.println(" speed");
        ramp_gotoEndSpeed(&ramp, new_val, 300);
        new_val = 0;
        break;
      case 'n':
        Serial.println(" goto");
        ramp_goto(&ramp, new_val);
        new_val = 0;
        break;
      case 'o' :    
        Serial.println(" offset");
        ramp_moveBy(&ramp, new_val);
        new_val = 0;
        break;
      case 'w' :
        new_val = 0;
        Serial.println(" [Abort]");
        break;
      default :
        Serial.print(incoming);

        if(!(incoming >= '0' && incoming <= '9')) {
          Serial.println(" [Unknown]");
          break;
        }

        incoming -= '0';

        new_val *= 10;
        new_val += incoming;
        break;
    }
  }

  ramp_update(&ramp, micros());
  if (true)//ramp_getSpeed(&ramp))
  {
    Serial.print(ramp_getPosition(&ramp));
    Serial.print("\t");
    //Serial.print(ramp_getRemaining(&ramp));
    //Serial.print("\t");
    Serial.print(ramp_getSpeed(&ramp));
    Serial.print("\t");
    Serial.print(ramp_getAccel(&ramp));

    Serial.println();
  }

  delay(5);
}