/* LICENSE:
  =========================================================================
    CMPack'04 Source Code Release for OPEN-R SDK 1.1.5-r2 for ERS7
    Copyright (C) 2004 Multirobot Lab [Project Head: Manuela Veloso]
    School of Computer Science, Carnegie Mellon University
    All rights reserved.
  ========================================================================= */

#include <errno.h>
#include <signal.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>

#include <sys/socket.h>
#include <sys/time.h>
#include <sys/types.h>

#include <arpa/inet.h>
#include <netinet/in.h>
#include <netinet/tcp.h>
#include <netdb.h>

#include "../../agent/Behaviors/PupPilotMsg.h"
#include "Joystick.h"

// Variables and constants for the network side of things.
const unsigned PORT = 4000;

// Interval between sending motion commands to the robot.
static const double batch_delay = 100000;

// Mapping between joystick buttons and kicks

#define SPECIAL_MOTIONS 1024
#define HEAD_DOWN      (SPECIAL_MOTIONS+1)
#define TRANSLATE_SLOW (SPECIAL_MOTIONS+2)
#define ROTATE_SLOW    (SPECIAL_MOTIONS+3)
#define PAUSE          (SPECIAL_MOTIONS+4)
#define NOTHING        (SPECIAL_MOTIONS+5)

int Kicks[] = {
  Motion::MOTION_KICK_FOREWARD, // 0
  Motion::MOTION_KICK_HEAD_L,   // 1
  Motion::MOTION_KICK_HEAD_R,   // 2
  Motion::MOTION_KICK_DIVE,     // 3
  Motion::MOTION_KICK_HEAD_L,   // 4
  Motion::MOTION_KICK_DIVE,     // 5
  Motion::MOTION_KICK_HEAD_R,   // 6
  Motion::MOTION_KICK_FOREWARD, // 7
  PAUSE,                        // 8
  HEAD_DOWN,                    // 9
  TRANSLATE_SLOW,               //10
  ROTATE_SLOW,                  //11
};

int connectToRobot(char *hostname);
void usage();
void doLoop(int socket_fd, char *device_name);
bool sendMsg(PupPilotMsg &msg, int socket);

double microTime() {
  static struct timeval time={0,0};
  static struct timezone tz={0,0};
  
  gettimeofday(&time,&tz);
  
  double seconds = time.tv_sec;
  double useconds = seconds*1000000+time.tv_usec;
  
  return useconds;
}


int main(int argc, char **argv) {

  int socket_fd = -1;

  if(argc!=2 && argc!=3) {
    usage();
    return 0;
  }

  socket_fd = connectToRobot(argv[1]);
  
  if(socket_fd==-1) {
    usage();
    return 0;
  }

  if(argc==2)
    doLoop(socket_fd, NULL);
  else
    doLoop(socket_fd, argv[2]);
  
  // Cleanup socket file descriptor.
  close(socket_fd);
  
  return 0;
}

void usage() {
  printf("PupPilot\n(c) Carnegie Mellon Multi-Robot Lab\n\nUsage: PupPilot <hostname> [device]\n\n");
}

void doLoop(int socket_fd, char *device_name) {
  
  Joystick js;
  PupPilotMsg msg;
  double next_send_time;
  bool paused = true;
  bool pause_button_down = false;

  // Open the joystick
  if(device_name==NULL) {
    device_name = "/dev/js0";
  } 

  if(!js.open(device_name)) {
    printf("Unable to open joystick %s\n", device_name);
    return;
  }

  // We wait batch_delay micro secs between sending stuff.
  next_send_time = microTime() + batch_delay;

  while(true) {

    // Get joystick events. We'll wait up until we need to send the
    // frame.
    js.processEvents((int)((next_send_time - microTime())/1000.0));

    msg.reset();
    msg.cmd.motion_cmd = Motion::MOTION_WALK_TROT;
    msg.cmd.bound_mode = Motion::BOUND_SPEED;
    msg.cmd.head_cmd = Motion::HEAD_LOOKAT;
    msg.cmd.head_lookat.set(1000.0,0.0,0.0);

#ifdef TANK_CONTROLS
    vector2d tr;
    double a;
    tr.set(lf,ll);
    tr += vector2d(rf,rl);
    tr /= 2.0;
    a = (rf - lf) / 2.0;
    
    msg.cmd.vx = tr.x * 240;
    msg.cmd.vy = tr.y * 200;
    msg.cmd.va =    a * 2.2;
#else
    msg.cmd.vx = (-js.axes[1] / 32768.0) * 240;
    msg.cmd.vy = (-js.axes[0] / 32768.0) * 200;
    msg.cmd.va = (-js.axes[2] / 32768.0) * 2.2;
#endif

    for(uint i=0; i<min((unsigned int)js.num_buttons,sizeof(Kicks)/sizeof(Kicks[0])); i++) {
      if(js.buttons[i]){
        int motion;
        motion = Kicks[i];
        if(motion & SPECIAL_MOTIONS){
          switch(motion){
            case HEAD_DOWN:
              msg.cmd.head_cmd = Motion::HEAD_LOOKAT;
              msg.cmd.head_lookat.set(110.0,0.0,0.0);
              break;
            case TRANSLATE_SLOW:
              msg.cmd.vx /= 2.0;
              msg.cmd.vy /= 2.0;
              break;
            case ROTATE_SLOW:
              msg.cmd.va /= 2.0;
              break;
            case PAUSE:
              if(!pause_button_down){
                paused = !paused;
                pause_button_down = true;
              }
              break;
          }
        }else{
          msg.cmd.motion_cmd = Kicks[i];
        }
      }else{
        int motion;
        motion = Kicks[i];
        if(motion & SPECIAL_MOTIONS){
          switch(motion){
            case PAUSE:
              pause_button_down = false;
              break;
          }
        }
      }
    }

    msg.pause = (paused ? 1 : 0);

    // If we have time left before we resend our data, repoll the joystick
    // to get a more recent commant to send along.
    if(next_send_time <= microTime()) {
      sendMsg(msg, socket_fd);
      next_send_time = microTime() + batch_delay;
    }
  }
  
  js.close();
}

/* Connects to a remote server and returns the file descriptor
   if successful. Returns -1 on failure. 
*/
int connectToRobot(char *hostname) {
  
  int socket_fd;

  // Setup the address of the server
  sockaddr_in server_addr;
  server_addr.sin_family = AF_INET;
  server_addr.sin_port = htons(PORT);

  // Poke DNS to find out the correct IP (in network byte order)
  // for our server
  hostent *server_he = gethostbyname(hostname);
  
  if(server_he==NULL) {
    printf("DNS lookup of %s failed\n", hostname);
    return -1;
  }

  if(server_he->h_length!=4) {
    printf("Ummm... Something is very borked in DNS land.\n");
    return -1;
  }

  // Finally, stick the address returned from DNS into our
  // sockaddr_in struct.
  server_addr.sin_addr = *((struct in_addr *)server_he->h_addr);

  // fill in reserved portion of struct
  memset(&server_addr.sin_zero, 0, 8);

  // create a file descriptor
  socket_fd = socket(AF_INET, SOCK_STREAM, 0);

  int flag = 1;
  if(setsockopt(socket_fd, IPPROTO_TCP, TCP_NODELAY, &flag, sizeof(flag))!=0)
    printf("setsockopt call failed with error %d\n", errno);

  // connect to the server
  int error = connect(socket_fd, (sockaddr*)(&server_addr), sizeof(server_addr));

  if(error!=0) {
    printf("Connection to %s failed. Reason: %d\n", hostname, errno);
    close(socket_fd);
    return -1;
  }

  return socket_fd;
}

bool sendMsg(PupPilotMsg &msg, int socket) {

  int result = send(socket, &msg, sizeof(msg), 0);
  if(result != sizeof(msg)) { 
    printf("Send failed (sent %d bytes). Not finishing send.\n", result);
    return false;
  }
  
  return true;
}