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

ramp_t ramp_theta;
ramp2d_t ramp_xy;

float new_val = 0;

static FILE uartout = {0} ;

static int uart_putchar (char c, FILE *stream)
{
  Serial.write(c) ;
  return 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("a : abort (reset the amount)");
  //Serial.println("w : waypoint goto");
  //Serial.println("v : waypoint offset");

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

void on_reach(ramp_t *r, void *cookie) {
  Serial.println("REACHED");
}

void on_reach_2d(ramp2d_t *r, void *cookie) {
  Serial.println("REACHED");
}

void setup() {
  Serial.begin(115200);
  fdev_setup_stream (&uartout, uart_putchar, NULL, _FDEV_SETUP_WRITE);
  stdout = &uartout ;

  printf("Init\n");

  ramp_init(&ramp_theta);
  ramp2d_init(&ramp_xy);

  ramp_setAccel(&ramp_theta, 3);
  ramp_setSpeed(&ramp_theta, 10);
  ramp_setDecel(&ramp_theta, 3.1);
  ramp_setBrakeDecel(&ramp_theta, 5);

  ramp2d_setAccel(&ramp_xy, 3);
  ramp2d_setSpeed(&ramp_xy, 10);
  ramp2d_setDecel(&ramp_xy, 3.1);
  ramp2d_setBrakeDecel(&ramp_xy, 5);

  //ramp_setCallback(&ramp_xy, on_reach, NULL);
  //ramp_setWrap(&ramp, 10);

  ramp2d_setCallback(&ramp_xy, on_reach_2d, NULL);

  help();
}
/*
void loop() {
  while (Serial.available()) {
    char incoming = Serial.read();

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

    switch (incoming)
    {
      case 'h' :
        help();
        break;
      case 'b' :
        ramp_brake(&ramp_theta);
        new_val = 0;
        break;
      case 'r' :
        ramp_release(&ramp_theta);
        new_val = 0;
        break;
      case 's' :
        ramp_gotoEndSpeed(&ramp_theta, new_val, 10);
        new_val = 0;
        break;
      case 'n':
        ramp_goto(&ramp_theta, new_val);
        new_val = 0;
        break;
      case 'o' :
        ramp_moveBy(&ramp_theta, new_val);
        new_val = 0;
        break;
      case 'a' :
        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_theta, micros());
  if (ramp_getSpeed(&ramp_theta))
  {
    Serial.print(ramp_getPosition(&ramp_theta));
    Serial.print("\t");
    //Serial.print(ramp_getRemaining(&ramp_theta));
    //Serial.print("\t");
    Serial.print(ramp_getSpeed(&ramp_theta));
    Serial.print("\t");
    Serial.print(ramp_theta.acc);

    Serial.print("\t");
    Serial.print(30);
    Serial.println();
  }
  delay(1);
}
*/


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

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

    switch (incoming)
    {
      case 'h' :
        help();
        break;
      case 'b' :
        ramp2d_brake(&ramp_xy);
        new_val = 0;
        break;
      case 'r' :
        ramp2d_release(&ramp_xy);
        new_val = 0;
        break;
      case 's' :
        ramp2d_gotoEndSpeed(&ramp_xy, new_val, 0, 10);
        new_val = 0;
        break;
      case 'n':
        ramp2d_goto(&ramp_xy, new_val, 0);
        new_val = 0;
        break;
      case 'o' :
        ramp2d_moveBy(&ramp_xy, new_val, 0);
        new_val = 0;
        break;
      case 'a' :
        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;
    }
  }

  ramp2d_update(&ramp_xy, micros());
  if (ramp2d_getSpeed(&ramp_xy))
  {
    Serial.print(ramp2d_getPositionX(&ramp_xy));
    Serial.print("\t");
    //Serial.print(ramp2d_getRemaining(&ramp_xy));
    //Serial.print("\t");
    Serial.print(ramp2d_getSpeed(&ramp_xy));
    Serial.print("\t");
    Serial.print(ramp2d_getAccel(&ramp_xy));

    Serial.print("\t");
    Serial.print(30);
    Serial.println();
  }
  delay(1);
}