/*
 * robotNav.c
 *
 * Author: Erich Styger
 * License: PDX-License-Identifier: BSD-3-Clause
 */

#include "robotNav.h"
#include "gpio.h"
#include "udp.h"
#include <stdio.h>
#include <unistd.h> /* interface to sleep */

static void BlinkLed(Gpio_Pins_e led) {
  Gpio_WritePin(led, true); /* on */
  usleep(100*1000);
  Gpio_WritePin(led, false); /* off */
}

int ROBONAV_test(void) {
  bool buttonPressed;

  printf("Use NAV switch on HAT, <CENTER> for > 1 sec to exit.\n");
  for(;;) {
    buttonPressed = false;
    if (Gpio_ReadPin(Gpio_Pins_BTN_LEFT_BCM)==0) {
      printf("left\n");
      printf("green\n"); BlinkLed(Gpio_Pins_LED_GREEN_PIN_BCM);
    }
    if (Gpio_ReadPin(Gpio_Pins_BTN_RIGHT_BCM)==0) {
      printf("right\n");
      printf("red\n"); BlinkLed(Gpio_Pins_LED_RED_PIN_BCM);
    }
    if (Gpio_ReadPin(Gpio_Pins_BTN_UP_BCM)==0) {
      printf("up\n");
      printf("blue\n"); BlinkLed(Gpio_Pins_LED_BLUE_PIN_BCM);
    }
    if (Gpio_ReadPin(Gpio_Pins_BTN_DOWN_BCM)==0) {
      printf("down\n");
      printf("yellow\n"); BlinkLed(Gpio_Pins_LED_YELLOW_PIN_BCM);
    }
    if (Gpio_ReadPin(Gpio_Pins_BTN_CENTER_BCM)==0) {
      printf("center\n");

      int timeMs = 0;
      while(Gpio_ReadPin(Gpio_Pins_BTN_CENTER_BCM)==0 && timeMs <= 1000) {
        usleep(100*1000);
        timeMs += 100;
      }
      if (Gpio_ReadPin(Gpio_Pins_BTN_CENTER_BCM)==0) { /* still pressed? */
        printf("exit program\n");
        break; /* break for loop and return */
      }
    }
    usleep(100*1000);
  } /* for */
  return 0; /* ok */
}

int ROBONAV_navigate(const char *server, int port) {
  unsigned int buttons; /* mask of buttons pressed */
  #define BTN_LEFT_MASK     (1<<0)
  #define BTN_RIGHT_MASK    (1<<1)
  #define BTN_UP_MASK       (1<<2)
  #define BTN_DOWN_MASK     (1<<3)
  #define BTN_CENTER_MASK   (1<<4)
  #define ALL_BUTTONS_MASK  (BTN_LEFT_MASK|BTN_RIGHT_MASK|BTN_UP_MASK|BTN_DOWN_MASK|BTN_CENTER_MASK)

  printf("Use NAV switch on HAT to navigate robot, <CENTER> for > 1 sec to exit.\n");
  udp_send(server, port, "test");
  for(;;) {
    buttons = 0;
    if (Gpio_ReadPin(Gpio_Pins_BTN_LEFT_BCM)==0) {
      buttons |= BTN_LEFT_MASK;
    }
    if (Gpio_ReadPin(Gpio_Pins_BTN_RIGHT_BCM)==0) {
      buttons |= BTN_RIGHT_MASK;
    }
    if (Gpio_ReadPin(Gpio_Pins_BTN_UP_BCM)==0) {
      buttons |= BTN_UP_MASK;
    }
    if (Gpio_ReadPin(Gpio_Pins_BTN_DOWN_BCM)==0) {
      buttons |= BTN_DOWN_MASK;
    }
    if (Gpio_ReadPin(Gpio_Pins_BTN_CENTER_BCM)==0) {
      buttons |= BTN_CENTER_MASK;
    }
    if ((buttons&ALL_BUTTONS_MASK)==(BTN_LEFT_MASK|BTN_UP_MASK)) {
      printf("left-up\n");
      udp_send(server, port, "nav_left-up");
    } else if ((buttons&ALL_BUTTONS_MASK)==BTN_LEFT_MASK) {
      printf("left\n");
      udp_send(server, port, "nav_left");
    } else if ((buttons&ALL_BUTTONS_MASK)==(BTN_RIGHT_MASK|BTN_UP_MASK)) {
      printf("right-up\n");
      udp_send(server, port, "nav_right-up");
    } else if ((buttons&ALL_BUTTONS_MASK)==BTN_RIGHT_MASK) {
      printf("right\n");
      udp_send(server, port, "nav_right");
    } else if ((buttons&ALL_BUTTONS_MASK)==BTN_UP_MASK) {
      printf("up\n");
      udp_send(server, port, "nav_up");
    } else if ((buttons&ALL_BUTTONS_MASK)==(BTN_LEFT_MASK|BTN_DOWN_MASK)) {
      printf("left-down\n");
      udp_send(server, port, "nav_down");
    } else if ((buttons&ALL_BUTTONS_MASK)==(BTN_RIGHT_MASK|BTN_DOWN_MASK)) {
      printf("left-down\n");
      udp_send(server, port, "nav_down");
    } else if ((buttons&ALL_BUTTONS_MASK)==BTN_DOWN_MASK) {
      printf("down\n");
      udp_send(server, port, "nav_down");
    } else if ((buttons&ALL_BUTTONS_MASK)==BTN_CENTER_MASK) {
      printf("center\n");
      udp_send(server, port, "nav_center");

      int timeMs = 0;
      while(Gpio_ReadPin(Gpio_Pins_BTN_CENTER_BCM)==0 && timeMs <= 1000) {
        usleep(100*1000);
        timeMs += 100;
      }
      if (Gpio_ReadPin(Gpio_Pins_BTN_CENTER_BCM)==0) { /* still pressed? */
        printf("exit program\n");
        udp_send(server, port, "exit");
        break; /* break for loop and return */
      }
    }
    usleep(100*1000);
  } /* for */
  return 0;
}

int ROBONAV_Init(void) {
  return 0; /* ok */
}

int ROBONAV_Deinit(void) {
  return 0; /* ok */
}