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

ramp_t ramp;

ramp_t ramp_y;
ramp_t ramp_theta;
waypoint_manager_t waypoints;

waypoint_t waypoint = {0};
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 setup() {
  Serial.begin(115200);
  fdev_setup_stream (&uartout, uart_putchar, NULL, _FDEV_SETUP_WRITE);
  stdout = &uartout ;

  printf("Init\n");

  ramp_init(&ramp);
  waypoint_init(&waypoints, &ramp, &ramp_y, &ramp_theta);

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

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

  waypoint.x = 5;
  waypoint.type = WAYPOINT_OFFSET_X;
  waypoint_add(&waypoints, &waypoint);

  waypoint.x = 10;
  waypoint.type = WAYPOINT_OFFSET_X;
  waypoint_add(&waypoints, &waypoint);

  waypoint.x = 30;
  waypoint.type = WAYPOINT_GOTO_X;
  waypoint_add(&waypoints, &waypoint);

  waypoint.x = 20;
  waypoint.type = WAYPOINT_GOTO_X;
  waypoint_add(&waypoints, &waypoint);

  waypoint.x = -20;
  waypoint.type = WAYPOINT_OFFSET_X;
  waypoint_add(&waypoints, &waypoint);

  waypoint_start(&waypoints);

  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);
        new_val = 0;
        break;
      case 'r' :
        ramp_release(&ramp);
        new_val = 0;
        break;
      case 's' :
        ramp_gotoEndSpeed(&ramp, new_val, 10);
        new_val = 0;
        break;
      case 'n':
        ramp_goto(&ramp, new_val);
        new_val = 0;
        break;
      case 'o' :
        ramp_moveBy(&ramp, new_val);
        new_val = 0;
        break;
      case 'a' :
        new_val = 0;
        Serial.println(" [Abort]");
        break;
      case 'w' :
        waypoint.x = new_val;
        waypoint.type = WAYPOINT_GOTO_X;
        waypoint_add(&waypoints, &waypoint);
        new_val = 0;
        break;
      case 'v' :
        waypoint.x = new_val;
        waypoint.type = WAYPOINT_OFFSET_X;
        waypoint_add(&waypoints, &waypoint);
        new_val = 0;
        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 (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.acc);

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