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

ramp_t ramp_theta;
ramp_t ramp_x;
ramp_t ramp_y;
ramp2d_t ramp_xy;

waypoint_manager_t waypoints;

int 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 (val, val/2");
  //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.print(30);
    Serial.println();
    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_x, &ramp_y);

  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);

  ramp2d_setCallback(&ramp_xy, on_reach_2d, NULL); // absorbé par waypoint manager
  ramp_update(&ramp_theta, micros());

  waypoint_init(&waypoints, &ramp_xy, &ramp_theta);
  waypoint_t waypoint = {0};
  
  waypoint.x = 5;
  waypoint.y = 0;
  waypoint.type_xy = WAYPOINT_GOTO;
  waypoint_add(&waypoints, &waypoint);
/*
  waypoint.x = 10;
  waypoint.y = 5;
  waypoint.type_xy = WAYPOINT_GOTO;
  waypoint_add(&waypoints, &waypoint);

  waypoint.x = 30;
  waypoint.y = 10;
  waypoint.type_xy = WAYPOINT_GOTO;
  waypoint_add(&waypoints, &waypoint);

  waypoint.x = 20;
  waypoint.y = 0;
  waypoint.type_xy = WAYPOINT_GOTO;
  waypoint_add(&waypoints, &waypoint);

  waypoint.x = 0;
  waypoint.y = 10;
  waypoint.type_xy = WAYPOINT_GOTO;
  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)
    {
      default :
        if(!(incoming >= '0' && incoming <= '9')) {
          Serial.println(" [Unknown]");
          break;
        }

        incoming -= '0';

        new_val *= 10;
        new_val += incoming;
        break;
      case 'h' :
        help();
        break;
      case 'b' :
        ramp_brake(&ramp_x);
        ramp_brake(&ramp_y);
        new_val = 0;
        break;
      case 'r' :
        ramp_release(&ramp_x);
        ramp_release(&ramp_y);
        new_val = 0;
        break;
      case 's' :
        ramp2d_gotoEndSpeed(&ramp_xy, new_val, 0, 10, 0);
        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 'w':
          waypoint_t waypoint = {0};
          waypoint.x = new_val;
          waypoint.y = new_val / 2;
          waypoint.type_xy = WAYPOINT_GOTO;
          waypoint_add(&waypoints, &waypoint);
          new_val = 0;
          break;
      case 'a' :
        new_val = 0;
        Serial.println(" [Abort]");
        break;
    }
  }

  ramp2d_update(&ramp_xy, micros());
  if (ramp2d_getSpeed(&ramp_xy))
  {
    Serial.print(ramp_getPosition(&ramp_x));
    Serial.print("\t");
    Serial.print(ramp_getPosition(&ramp_y));
    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);
}