#include <string.h>
#include <unistd.h>
#include <stdlib.h>
#include <stdbool.h>

#include "screen.h"

#include <psp2/ctrl.h>
#include <psp2/touch.h>
#include <psp2/types.h>
#include <psp2/sysmodule.h>
#include <psp2/net/net.h>
#include <psp2/net/netctl.h>
#include <psp2/kernel/threadmgr.h>
#include <psp2/kernel/processmgr.h>

#include "common/packet.h"

_Static_assert(SCE_TOUCH_SAMPLING_STATE_START == 1, "Stop using old VitaSDK");

#define ARRAY_SIZE(x) (sizeof(x) / sizeof(x[0]))
#define BIT_TOGGLE(w, m, f) (w & ~m) | (-f & m)

/**
 * Neccessary uinput / evdev constants
 */

#define UINPUT_IOCTL_BASE 'U'
#define EV_SYN       0x00
#define EV_KEY       0x01
#define EV_ABS       0x03

#define BTN_DPAD_UP     0x220
#define BTN_DPAD_DOWN   0x221
#define BTN_DPAD_LEFT   0x222
#define BTN_DPAD_RIGHT  0x223

#define BTN_SOUTH    0x130
#define BTN_EAST     0x131
#define BTN_NORTH    0x133
#define BTN_WEST     0x134
#define BTN_TL       0x136
#define BTN_TR       0x137
#define BTN_TL2      0x138
#define BTN_TR2      0x139
#define BTN_SELECT   0x13a
#define BTN_START    0x13b
#define BTN_MODE     0x13c

#define ABS_X        0x00
#define ABS_Y        0x01
#define ABS_RX       0x03
#define ABS_RY       0x04
#define ABS_HAT0X    0x10
#define ABS_HAT0Y    0x11
#define ABS_MT_POSITION_X  0x35
#define ABS_MT_POSITION_Y  0x36
#define ABS_MAX      0x3f
#define ABS_CNT      (ABS_MAX+1)

#define SYN_REPORT   0

#define BUS_VIRTUAL 0x06

struct input_id {
   uint16_t bustype;
   uint16_t vendor;
   uint16_t product;
   uint16_t version;
};

#define UINPUT_MAX_NAME_SIZE  80

struct uinput_user_dev {
   char name[UINPUT_MAX_NAME_SIZE];
   struct input_id id;
   uint32_t ff_effects_max;
   int32_t absmax[ABS_CNT];
   int32_t absmin[ABS_CNT];
   int32_t absfuzz[ABS_CNT];
   int32_t absflat[ABS_CNT];
};

struct timeval {
#ifdef HOST_32BIT
   uint32_t tv_sec, tv_usec;
#else
   uint64_t tv_sec, tv_usec;
#endif
};

struct input_event {
   struct timeval time;
   uint16_t type;
   uint16_t code;
   int32_t value;
};

/**
 * We don't have way to poll for input being ready, so we have to resort for
 * having delay to avoid huge power drain while vita-uinput is running.
 */
#define INPUT_LAG 20 // ms

/**
 * Expands the SCE button bitmaks.
 * We emulate these buttons by using the 2 touch screens.
 */
enum {
   SCE_CTRL_EXTRA_L2TRIGGER     = 0x020000,
   SCE_CTRL_EXTRA_R2TRIGGER     = 0x040000,
   SCE_CTRL_EXTRA_MENU          = 0x080000,
};

static struct timeval
us_to_timeval(const uint64_t us)
{
   return (struct timeval){
      .tv_sec = us / (uint64_t)1e6,
      .tv_usec = us % (uint64_t)1e6,
   };
}

static SceUInt
ms_to_us(const SceUInt ms)
{
   return ms * 1000;
}

static SceUInt
s_to_us(const SceUInt s)
{
   return ms_to_us(s * 1000);
}

static void
setup(void)
{
   sceCtrlSetSamplingMode(SCE_CTRL_MODE_ANALOG);
   sceTouchSetSamplingState(SCE_TOUCH_PORT_FRONT, SCE_TOUCH_SAMPLING_STATE_START);
   sceTouchSetSamplingState(SCE_TOUCH_PORT_BACK, SCE_TOUCH_SAMPLING_STATE_START);
   sceTouchEnableTouchForce(SCE_TOUCH_PORT_FRONT);
   sceTouchEnableTouchForce(SCE_TOUCH_PORT_BACK);

   if (!screen_init())
      errx(EXIT_FAILURE, "screen_init");

   if (sceSysmoduleLoadModule(SCE_SYSMODULE_NET) != 0)
      errx(EXIT_FAILURE, "sceSysmoduleLoadModule");

   if ((unsigned int)sceNetShowNetstat() == SCE_NET_ERROR_ENOTINIT) {
      static uint8_t sce_net_memory[1024 * 1024];
      SceNetInitParam initparam = {
         .memory = sce_net_memory,
         .size = sizeof(sce_net_memory),
      };

      if (sceNetInit(&initparam) != 0)
         errx(EXIT_FAILURE, "sceNetInit");
   }

   if (sceNetCtlInit() != 0)
      errx(EXIT_FAILURE, "sceNetCtlInit");
}

static void
terminate(void)
{
   sceNetCtlTerm();
   sceNetTerm();
   sceSysmoduleUnloadModule(SCE_SYSMODULE_NET);
   screen_deinit();
}

static void
disconnect(int fd)
{
   sceNetSocketClose(fd);
}

static int
connect(const char *ip, SceNetCtlInfo *info)
{
   assert(ip && info);

   if (sceNetCtlInetGetInfo(SCE_NETCTL_INFO_GET_IP_ADDRESS, info) != 0) {
      warnx("sceNetCtlInetGetInfo");
      return -1;
   }

   SceNetSockaddrIn addr = {
      .sin_family = SCE_NET_AF_INET,
      .sin_port = sceNetHtons(5000),
   };

   if (!sceNetInetPton(SCE_NET_AF_INET, ip, &addr.sin_addr)) {
      warnx("sceNetInetPton");
      return -1;
   }

   int fd;
   if ((fd = sceNetSocket("vita_uinput", SCE_NET_AF_INET, SCE_NET_SOCK_STREAM, 0)) < 0) {
      warnx("sceNetSocket: %d", fd);
      return fd;
   }

   if (sceNetConnect(fd, (SceNetSockaddr*)&addr, sizeof(addr))) {
      warnx("sceNetConnect: failed connecting to %s", ip);
      disconnect(fd);
      return -1;
   }

   return fd;
}

static bool
uinputd_ioctl_other(const int fd, const enum ioctl_dir dir, const uint8_t type, const uint8_t nr, const uint16_t bytes, void *arg)
{
   const struct packet packet = {
      .type = PACKET_IOCTL,
      .ioctl.dir = dir,
      .ioctl.type = type,
      .ioctl.nr = nr,
      .ioctl.arg = IOCTL_ARG_OTHER,
      .ioctl.bytes = bytes,
   };

   if (sceNetSend(fd, &packet, sizeof(packet), 0) != sizeof(packet)) {
      warnx("sceNetSend: failed to send %u bytes", sizeof(packet));
      return false;
   }

   if (bytes > 0 && sceNetSend(fd, arg, bytes, 0) != bytes) {
      warnx("sceNetSend: failed to send %u bytes", bytes);
      return false;
   }

   return true;
}

static bool
uinputd_ioctl_int(const int fd, const enum ioctl_dir dir, const uint8_t type, const uint8_t nr, int arg)
{
   const struct packet packet = {
      .type = PACKET_IOCTL,
      .ioctl.dir = dir,
      .ioctl.type = type,
      .ioctl.nr = nr,
      .ioctl.arg = IOCTL_ARG_INT,
      .ioctl.integer = arg,
   };

   if (sceNetSend(fd, &packet, sizeof(packet), 0) != sizeof(packet)) {
      warnx("sceNetSend: failed to send %u bytes", sizeof(packet));
      return false;
   }

   return true;
}

static bool
uinputd_write(const int fd, const int32_t bytes, const void *data)
{
   if (bytes <= 0 || !data)
      return true;

   const struct packet packet = {
      .type = PACKET_WRITE,
      .write.bytes = bytes,
   };

   if (sceNetSend(fd, &packet, sizeof(packet), 0) != sizeof(packet)) {
      warnx("sceNetSend: failed to send %u bytes", sizeof(packet));
      return false;
   }

   if (sceNetSend(fd, data, bytes, 0) != bytes) {
      warnx("sceNetSend: failed to send %ld bytes", bytes);
      return false;
   }

   return true;
}

static bool
process_touch(const SceTouchData *new, SceTouchData *old, SceCtrlData *ctrl, const bool back_touchpad)
{
   // Exposing vita touchpads as multitouch wasn't particularly useful..
   // Thus lets give them more useful function.
   // Back touch will be mapped to L2/L2 and front touch to menu button.

   // Button emulation will only be done on new touch.
   // e.g. Swiping back touchpad from left to right, will only trigger L2 or R2 from the starting touch point.
   if (old->reportNum > 0)
      goto end;

   for (size_t i = 0; i < new->reportNum; ++i) {
      if (back_touchpad) {
         // Touch x resolution is resx * 2, thus 960 is half
         // Touch y resolution is resy * 2, thus 544 is half
         // We add some padding for the touch area to avoid accidental touches.
         if (new->report[i].y > 362 && new->report[i].y < 726) {
            if (new->report[i].x < 960 && new->report[i].x > 480) {
               ctrl->buttons |= SCE_CTRL_EXTRA_L2TRIGGER;
            } else if (new->report[i].x > 960 && new->report[i].x < 1440) {
               ctrl->buttons |= SCE_CTRL_EXTRA_R2TRIGGER;
            }
         }
      } else {
         ctrl->buttons |= SCE_CTRL_EXTRA_MENU;
      }
   }

end:
   *old = *new;
   return true;
}

static void
ctrl_query_changed_analogs(const SceCtrlData *new, const SceCtrlData *old, uint32_t out_changed[4])
{
   assert(new && old);

   for (size_t i = 0; i < 4; ++i)
      out_changed[i] = (uint32_t)~0;

   const uint8_t fuzz = 2;
   const uint32_t code[4] = { ABS_X, ABS_Y, ABS_RX, ABS_RY };
   const uint8_t new_analogs[4] = { new->lx, new->ly, new->rx, new->ry };
   const uint8_t old_analogs[4] = { old->lx, old->ly, old->rx, old->ry };
   for (size_t i = 0, c = 0; i < ARRAY_SIZE(new_analogs); ++i) {
      if (old_analogs[i] - fuzz <= new_analogs[i] && old_analogs[i] + fuzz >= new_analogs[i])
         continue;

      assert(c < ARRAY_SIZE(code));
      out_changed[c++] = code[i];
   }
}

static bool
process_ctrl(const int fd, const SceCtrlData *new, SceCtrlData *old, bool *out_changed)
{
   assert(new && old && out_changed);

   {
      // https://www.kernel.org/doc/Documentation/input/gamepad.txt
      // Digital -> Analog emulation
      struct {
         uint32_t code, value;
         bool changed;
      } abs[] = {
         { ABS_HAT0X, 127, false },
         { ABS_HAT0Y, 127, false },
      };

      const struct {
         uint32_t bit, abs, value;
      } map[] = {
         { SCE_CTRL_UP, 1, -127 },
         { SCE_CTRL_DOWN, 1, 128 },
         { SCE_CTRL_LEFT, 0, -127 },
         { SCE_CTRL_RIGHT, 0, 128},
      };

      for (size_t i = 0; i < ARRAY_SIZE(map); ++i) {
         if ((new->buttons & map[i].bit) == (old->buttons & map[i].bit))
            continue;

         abs[map[i].abs].changed = true;
         abs[map[i].abs].value += (new->buttons & map[i].bit ? map[i].value : 0);
      }

      for (size_t i = 0; i < ARRAY_SIZE(abs); ++i) {
         if (!abs[i].changed)
            continue;

         const struct input_event ev = {
            .time = us_to_timeval(new->timeStamp),
            .type = EV_ABS,
            .code = abs[i].code,
            .value = abs[i].value,
         };

         if (!uinputd_write(fd, sizeof(ev), &ev)) {
            warnx("write: failed to write %u bytes", sizeof(ev));
            return false;
         }
      }
   }

   {
      // https://www.kernel.org/doc/Documentation/input/gamepad.txt
      const struct {
         uint32_t bit, code;
      } map[] = {
         { SCE_CTRL_TRIANGLE, BTN_NORTH },
         { SCE_CTRL_CROSS, BTN_SOUTH },
         { SCE_CTRL_SQUARE, BTN_WEST },
         { SCE_CTRL_CIRCLE, BTN_EAST },
         { SCE_CTRL_START, BTN_START },
         { SCE_CTRL_SELECT, BTN_SELECT },
         { SCE_CTRL_UP, BTN_DPAD_UP },
         { SCE_CTRL_DOWN, BTN_DPAD_DOWN },
         { SCE_CTRL_LEFT, BTN_DPAD_LEFT },
         { SCE_CTRL_RIGHT, BTN_DPAD_RIGHT },
         { SCE_CTRL_LTRIGGER, BTN_TL },
         { SCE_CTRL_RTRIGGER, BTN_TR },
         { SCE_CTRL_EXTRA_L2TRIGGER, BTN_TL2 },
         { SCE_CTRL_EXTRA_R2TRIGGER, BTN_TR2 },
         { SCE_CTRL_EXTRA_MENU, BTN_MODE },
      };

      for (size_t i = 0; i < ARRAY_SIZE(map); ++i) {
         if ((new->buttons & map[i].bit) == (old->buttons & map[i].bit))
            continue;

         const struct input_event ev = {
            .time = us_to_timeval(new->timeStamp),
            .type = EV_KEY,
            .code = map[i].code,
            .value = !!(new->buttons & map[i].bit),
         };

         if (!uinputd_write(fd, sizeof(ev), &ev)) {
            warnx("write: failed to write %u bytes", sizeof(ev));
            return false;
         }

         *out_changed = true;
         old->buttons = BIT_TOGGLE(old->buttons, map[i].bit, (new->buttons & map[i].bit));
      }
   }

   {
      // https://www.kernel.org/doc/Documentation/input/gamepad.txt
      const uint8_t values[] = {
         new->lx, // ABS_X
         new->ly, // ABS_Y
         0, // ABS_Z
         new->rx, // ABS_RX
         new->ry, // ABS_RY
      };

      uint8_t dummy;
      uint8_t* old_values_ref[] = {
         &old->lx, // ABS_X
         &old->ly, // ABS_Y
         &dummy, // ABS_Z
         &old->rx, // ABS_RX
         &old->ry, // ABS_RY
      };

      uint32_t changed[4];
      ctrl_query_changed_analogs(new, old, changed);
      for (size_t i = 0; i < ARRAY_SIZE(changed) && changed[i] != (uint32_t)~0; ++i) {
         assert(changed[i] < ARRAY_SIZE(values));

         const struct input_event ev = {
            .time = us_to_timeval(new->timeStamp),
            .type = EV_ABS,
            .code = changed[i],
            .value = values[changed[i]],
         };

         if (!uinputd_write(fd, sizeof(ev), &ev)) {
            warnx("write: failed to write %u bytes", sizeof(ev));
            return false;
         }

         *out_changed = true;
         *old_values_ref[changed[i]] = values[changed[i]];
      }
   }

   return true;
}

static bool
process_syn(const int fd)
{
   const struct input_event ev = {
      .type = EV_SYN,
      .code = SYN_REPORT,
   };

   if (!uinputd_write(fd, sizeof(ev), &ev)) {
      warnx("write: failed to write %u bytes", sizeof(ev));
      return false;
   }

   return true;
}

static bool
setup_uinput(const int fd)
{
   if (!uinputd_ioctl_int(fd, IOCTL_IOW, UINPUT_IOCTL_BASE, 100, EV_KEY)) {
      warnx("ioctl(UI_SET_EVBIT): EV_KEY");
      return false;
   }

   if (!uinputd_ioctl_int(fd, IOCTL_IOW, UINPUT_IOCTL_BASE, 100, EV_ABS)) {
      warnx("ioctl(UI_SET_EVBIT): EV_ABS");
      return false;
   }

   if (!uinputd_ioctl_int(fd, IOCTL_IOW, UINPUT_IOCTL_BASE, 100, EV_SYN)) {
      warnx("ioctl(UI_SET_EVBIT): EV_SYN");
      return false;
   }

   {
      const int keys[] = {
         BTN_NORTH,
         BTN_SOUTH,
         BTN_WEST,
         BTN_EAST,
         BTN_START,
         BTN_SELECT,
         BTN_DPAD_UP,
         BTN_DPAD_DOWN,
         BTN_DPAD_LEFT,
         BTN_DPAD_RIGHT,
         BTN_TL,
         BTN_TR,
         BTN_TL2,
         BTN_TR2,
         BTN_MODE
      };

      for (size_t i = 0; i < ARRAY_SIZE(keys); ++i) {
         if (!uinputd_ioctl_int(fd, IOCTL_IOW, UINPUT_IOCTL_BASE, 101, keys[i])) {
            warnx("ioctl (UI_SET_KEYBIT:%d)", keys[i]);
            return false;
         }
      }
   }

   {
      const int abs[] = {
         ABS_X,
         ABS_Y,
         ABS_RX,
         ABS_RY,
         ABS_HAT0X,
         ABS_HAT0Y,
         ABS_MT_POSITION_X, // To disable Xorg from handling the analogs as mouse
         ABS_MT_POSITION_Y, // To disable Xorg from handling the analogs as mouse
      };

      for (size_t i = 0; i < ARRAY_SIZE(abs); ++i) {
         if (!uinputd_ioctl_int(fd, IOCTL_IOW, UINPUT_IOCTL_BASE, 103, abs[i])) {
            warnx("ioctl (UI_SET_ABSBIT:%d)", abs[i]);
            return false;
         }
      }
   }

   {
      struct uinput_user_dev setup = {
         .name = "PS Vita",
         .id = {
            // http://www.vitadevwiki.com/index.php?title=USB
            .bustype = BUS_VIRTUAL,
            .vendor = 0x054C, // Sony Corp.
            .product = 0x04E4, // PS Vita
            .version = 0x0100,
         },
         .absmax = {
            [ABS_X] = 255,
            [ABS_Y] = 255,
            [ABS_RX] = 255,
            [ABS_RY] = 255,
            [ABS_HAT0X] = 255,
            [ABS_HAT0Y] = 255,
         },
      };

      if (!uinputd_write(fd, sizeof(setup), &setup)) {
         warnx("write: failed to write %u bytes", sizeof(setup));
         return false;
      }

      if (!uinputd_ioctl_other(fd, IOCTL_NONE, UINPUT_IOCTL_BASE, 1, 0, NULL)) {
         warnx("ioctl (UI_DEV_CREATE)");
         return false;
      }
   }

   return true;
}

static void
run(const int fd)
{
   struct {
      SceCtrlData ctrl;
      SceTouchData touch[2];
   } old = {0}, new = {0};

   bool error = !setup_uinput(fd);
   while (!error) {
      sceKernelPowerTick(0);
      sceKernelDelayThread(ms_to_us(INPUT_LAG));

      // NOTE: These don't actually block, thus ^ sleep above.
      //       Sucks a bit for power usage and latency.
      sceTouchRead(SCE_TOUCH_PORT_FRONT, &new.touch[0], 1);
      sceTouchRead(SCE_TOUCH_PORT_BACK, &new.touch[1], 1);
      sceCtrlReadBufferPositive(0, &new.ctrl, 1);

      bool changed = false;

      for (size_t i = 0; i < ARRAY_SIZE(new.touch); ++i) {
         // Doesn't actually send or "change" anything.
         // Just simulates button presses that will be appended to new.ctrl.buttons
         // Actual change checking and sending is done in process_ctrl.
         if (!process_touch(&new.touch[i], &old.touch[i], &new.ctrl, (i == 1)))
            break;
      }

      if (!process_ctrl(fd, &new.ctrl, &old.ctrl, &changed))
         break;

      if (changed && !process_syn(fd))
         break;
   }
}

int
main(void)
{
   setup();
   atexit(terminate);

   // TODO: Needs way to input ip, if this ever goes out

   while (true) {
      int fd;
      SceNetCtlInfo info;
      const char *ip = "192.168.1.2";
      if ((fd = connect(ip, &info)) >= 0) {
         printf("vita-uinput %s\n", VITA_UINPUT_VERSION);
         printf("Vita IP: %s\n", info.ip_address);
         printf("Server IP: %s\n", ip);
         printf("sizeof(struct packet) is %u bytes\n", sizeof(struct packet));
         printf("sizeof(struct input_event) is %u bytes\n", sizeof(struct input_event));
         printf("sizeof(struct uinput_user_dev) is %u bytes\n", sizeof(struct uinput_user_dev));
         run(fd);
      }

      disconnect(fd);

      warnx("Reconnecting after 5 seconds");
      sceKernelDelayThread(s_to_us(5));
      screen_clear();
   }

   exit(EXIT_SUCCESS);
   return EXIT_SUCCESS;
}