
#include <stdlib.h>
#include <stdint.h>
#include <stdio.h>
#include <string.h>

#include "swis.h"
#include "Global/RISCOS.h"

#include "debugrep.h"
#include "usbjoystick.h"
#include "USBJoystickHdr.h"   // rumble_timer / rumble_callback veneers
#include "device.h"
#include "errors.h"
#include "joydev.h"
#include "joyswis.h"          // Joystick_Cap_*

#include "rumble.h"


// OS_Find: open for output, error if absent, no path (the name is a full
// DeviceFS path).
#define OPEN_OUTPUT   (0x80 | 0x08 | 0x03)


// A stop that fails leaves the motors running, so it is tried again, up to
// STOP_TRIES failures in a row. A device that can drop a command silently
// (JOY_QUIRK_LOSSY_OUTPUT) gets its stop sent STOP_SENDS times in all.
#define STOP_TRIES       5
#define STOP_RETRY_CS    30
#define STOP_SENDS       3
#define STOP_SPACING_CS  30


// True while the OS_CallAfter stop timer is outstanding.
static int timer_pending = FALSE;


static uint32_t now_cs(void)
{
  uint32_t t = 0;
  _swix(OS_ReadMonotonicTime, _OUT(0), &t);
  return t;
}


// A stop_at value for cs from now; 0 means no deadline, so it is never 0.
static uint32_t deadline_cs(uint32_t cs)
{
  uint32_t t = now_cs() + cs;
  return (t != 0) ? t : 1;
}


static _kernel_oserror *output_open(uint32_t slot)
{
  struct joy_output_struct *o = &joy_data[slot].output;

  if (o->fp != 0)
    return NULL;

  if (o->endpoint == 0)
    return geterror(Error_NoOutput);

  char path[USB_DEVICEFS_PATH_MAX_LENGTH];
  sprintf(path, USB_OUT_PATH_SPECIFIER, joy_data[slot].interface, o->endpoint, joy_data[slot].device_name);

  int fp = 0;
  _kernel_oserror *e = _swix(OS_Find, _INR(0,2) | _OUT(0), OPEN_OUTPUT, path, 0, &fp);
  if (e) {
    debug_printf("output_open: slot %u: cannot open output stream %s: %s\n", slot, path, e->errmess);
    return e;
  }
  if (fp == 0)
    return geterror(Error_NoOutput);

  o->fp = fp;
  return NULL;
}


static void output_close(uint32_t slot)
{
  struct joy_output_struct *o = &joy_data[slot].output;

  if (o->fp != 0) {
    _kernel_oserror *e = _swix(OS_Find, _INR(0,1), 0, o->fp);
    if (e)
      debug_printf("output_close: slot %u: cannot close output stream %d: %s\n", slot, o->fp, e->errmess);
    o->fp = 0;
  }
}


static void timer_cancel(void)
{
  if (timer_pending) {
    _swix(OS_RemoveTickerEvent, _INR(0,1), rumble_timer, g_module_pw);
    timer_pending = FALSE;
  }
  _swix(OS_RemoveCallBack, _INR(0,1), rumble_callback, g_module_pw);
}


// One timer serves every slot, armed for the earliest deadline.
static void timer_schedule(void)
{
  uint32_t now = now_cs();
  int have = FALSE;
  int32_t soonest = 0;

  for (uint32_t i=0; i<Joystick_MaxSlots; i++) {
    if (joy_data[i].output.rumble_stop_at != 0) {
      int32_t d = (int32_t)(joy_data[i].output.rumble_stop_at - now);
      if (d < 1) d = 1;
      if (!have || d < soonest) { soonest = d; have = TRUE; }
    }
  }

  timer_cancel();

  if (have) {
    _kernel_oserror *e = _swix(OS_CallAfter, _INR(0,2), soonest, rumble_timer, g_module_pw);
    if (e)
      debug_printf("timer_schedule: OS_CallAfter failed: %s\n", e->errmess);
    else
      timer_pending = TRUE;
  }
}


uint32_t rumble_capabilities(uint32_t slot)
{
  struct joydata_struct *j = &joy_data[slot];

  if (!j->in_use || j->device == NULL || j->device->rumble == NULL)
    return 0;

  if (j->device->output_transport == JOY_OUTPUT_ENDPOINT && j->output.endpoint == 0)
    return 0;

  return Joystick_Cap_Rumble;
}


_kernel_oserror *rumble_send(uint32_t slot, const uint8_t *buf, uint32_t len)
{
  if (len == 0 || len > RUMBLE_MAX_PACKET)
    return geterror(Error_BadNoParms);

  _kernel_oserror *e = output_open(slot);
  if (e) return e;

  uint32_t not_written = 0;
  e = usb_write((usb_device_t*)joy_data[slot].device_name, joy_data[slot].output.fp, buf, len, &not_written);
  if (e) {
    // the stream may have died with the device; the next send reopens it
    output_close(slot);
    return e;
  }
  if (not_written != 0) {
    debug_printf("rumble_send: slot %u: %d of %u bytes not written\n", slot, not_written, len);
    return geterror(Error_SendFail);
  }

  return NULL;
}


_kernel_oserror *rumble_send_report(uint32_t slot, uint8_t report_id, const uint8_t *buf, uint32_t len)
{
  if (len > RUMBLE_MAX_PACKET)
    return geterror(Error_BadNoParms);

  uint8_t data[RUMBLE_MAX_PACKET + 1];
  uint32_t n = 0;

  if (report_id != 0)
    data[n++] = report_id;
  memcpy(data + n, buf, len);
  n += len;

  if (n == 0)
    return geterror(Error_BadNoParms);

  struct joydata_struct *j = &joy_data[slot];
  _kernel_oserror *e = usb_hid_set_report((usb_device_t*)j->device_name, j->interface, UHID_OUTPUT_REPORT, report_id, data, (uint16_t)n);
  if (e)
    debug_printf("rumble_send_report: slot %u: report %u failed: %s\n", slot, report_id, e->errmess);
  return e;
}


// Packet layouts are in doc.Rumble; these are the offsets the strengths and
// the other variable bytes go to.
#define XB360_PKT_LEN      8
#define XB360_LOW          3
#define XB360_HIGH         4

#define XB360WL_PKT_LEN    12
#define XB360WL_LOW        5
#define XB360WL_HIGH       6

#define PSX_DATA_LEN       3
#define PSX_SMALL          1     // bit 0 runs the small motor
#define PSX_BIG            2

#define PS3_REPORT_ID      0x01
#define PS3_PKT_LEN        49    // the report id and 48 bytes
#define PS3_DURATION_MAX   0xFF
#define PS3_RIGHT_DURATION 2
#define PS3_RIGHT_MOTOR    3     // on or off
#define PS3_LEFT_DURATION  4
#define PS3_LEFT_MOTOR     5
#define PS3_LED            10
#define PS3_FIXED_FIRST    11
#define PS3_FIXED_GROUPS   4

#define PS4_REPORT_ID      0x05
#define PS4_PKT_LEN        32
#define PS4_EFFECTS        1
#define PS4_EFFECT_RUMBLE  0x01  // motors only, the lightbar is left alone
#define PS4_RIGHT          4
#define PS4_LEFT           5


_kernel_oserror *rumble_xbox360(uint32_t slot)
{
  struct joy_output_struct *o = &joy_data[slot].output;
  uint8_t pkt[XB360_PKT_LEN] = { 0x00, 0x08, 0x00 };

  pkt[XB360_LOW]  = (uint8_t)(o->rumble_low >> 8);
  pkt[XB360_HIGH] = (uint8_t)(o->rumble_high >> 8);
  return rumble_send(slot, pkt, sizeof(pkt));
}


_kernel_oserror *rumble_xbox360wireless(uint32_t slot)
{
  struct joy_output_struct *o = &joy_data[slot].output;
  uint8_t pkt[XB360WL_PKT_LEN] = { 0x00, 0x01, 0x0F, 0xC0, 0x00 };

  pkt[XB360WL_LOW]  = (uint8_t)(o->rumble_low >> 8);
  pkt[XB360WL_HIGH] = (uint8_t)(o->rumble_high >> 8);
  return rumble_send(slot, pkt, sizeof(pkt));
}


_kernel_oserror *rumble_psx(uint32_t slot)
{
  struct joydata_struct *j = &joy_data[slot];
  uint8_t data[PSX_DATA_LEN] = { 0 };

  data[PSX_SMALL] = (j->output.rumble_high != 0) ? 1 : 0;
  data[PSX_BIG]   = (uint8_t)(j->output.rumble_low >> 8);
  return rumble_send_report(slot, j->has_report_id ? j->report_id : 0, data, sizeof(data));
}


_kernel_oserror *rumble_ps3(uint32_t slot)
{
  struct joy_output_struct *o = &joy_data[slot].output;
  uint8_t pkt[PS3_PKT_LEN] = { 0 };

  pkt[0] = PS3_REPORT_ID;
  pkt[1] = 0x01;
  pkt[PS3_RIGHT_DURATION] = PS3_DURATION_MAX;
  pkt[PS3_RIGHT_MOTOR]    = (o->rumble_high != 0) ? 1 : 0;
  pkt[PS3_LEFT_DURATION]  = PS3_DURATION_MAX;
  pkt[PS3_LEFT_MOTOR]     = (uint8_t)(o->rumble_low >> 8);
  pkt[PS3_LED]            = (uint8_t)(0x02 << (slot & 3));

  for (int g=0; g<PS3_FIXED_GROUPS; g++) {
    uint8_t *p = pkt + PS3_FIXED_FIRST + 5*g;
    p[0] = 0xFF;
    p[1] = 0x27;
    p[2] = 0x10;
    p[3] = 0x00;
    p[4] = 0x32;
  }

  // rumble_send_report puts the report id in front itself
  return rumble_send_report(slot, PS3_REPORT_ID, pkt + 1, sizeof(pkt) - 1);
}


_kernel_oserror *rumble_ps4(uint32_t slot)
{
  struct joy_output_struct *o = &joy_data[slot].output;
  uint8_t pkt[PS4_PKT_LEN] = { 0 };

  pkt[0] = PS4_REPORT_ID;
  pkt[PS4_EFFECTS] = PS4_EFFECT_RUMBLE;
  pkt[PS4_RIGHT]   = (uint8_t)(o->rumble_high >> 8);
  pkt[PS4_LEFT]    = (uint8_t)(o->rumble_low >> 8);
  return rumble_send(slot, pkt, sizeof(pkt));
}


_kernel_oserror *rumble_set(uint32_t slot, uint32_t low, uint32_t high, uint32_t duration_ms)
{
  struct joydata_struct *j = &joy_data[slot];
  struct joy_output_struct *o = &j->output;

  if (!(rumble_capabilities(slot) & Joystick_Cap_Rumble))
    return geterror(Error_NoRumble);

  if (low == 0 && high == 0)
    debug_printf("rumble_set: slot %u: stop\n", slot);
  else
    debug_printf("rumble_set: slot %u: low %u, high %u, %u ms\n", slot, low, high, duration_ms);

  uint16_t old_low = o->rumble_low, old_high = o->rumble_high;
  o->rumble_low  = (low  > 0xFFFF) ? 0xFFFF : (uint16_t)low;
  o->rumble_high = (high > 0xFFFF) ? 0xFFFF : (uint16_t)high;

  int stopping = (o->rumble_low == 0 && o->rumble_high == 0);

  _kernel_oserror *e = j->device->rumble(slot);
  if (e) {
    debug_printf("rumble_set: slot %u: failed: %s\n", slot, e->errmess);
    o->rumble_low = old_low;
    o->rumble_high = old_high;

    if (stopping && (old_low != 0 || old_high != 0)) {
      o->rumble_stop_tries++;
      o->rumble_stop_at = deadline_cs(STOP_RETRY_CS);
      timer_schedule();
    }
    return e;
  }

  o->rumble_stop_tries = 0;

  if (!stopping) {
    o->rumble_stop_repeats = 0;
    o->rumble_stop_at = (duration_ms != 0) ? deadline_cs((duration_ms + 9) / 10) : 0;
  }
  else if (j->device->quirks & JOY_QUIRK_LOSSY_OUTPUT) {
    o->rumble_stop_repeats = STOP_SENDS - 1;
    o->rumble_stop_at = deadline_cs(STOP_SPACING_CS);
  }
  else {
    o->rumble_stop_repeats = 0;
    o->rumble_stop_at = 0;
  }

  timer_schedule();
  return NULL;
}


// Act on every slot whose deadline has passed: stop a running rumble (a failed
// stop is tried again), or resend the stop of a device that can drop it. Then
// re-arm for the rest.
static void expire_due(void)
{
  uint32_t now = now_cs();

  for (uint32_t i=0; i<Joystick_MaxSlots; i++) {
    struct joydata_struct *j = &joy_data[i];
    struct joy_output_struct *o = &j->output;

    if (o->rumble_stop_at == 0 || (int32_t)(now - o->rumble_stop_at) < 0)
      continue;
    o->rumble_stop_at = 0;

    if (o->rumble_low != 0 || o->rumble_high != 0) {
      if (o->rumble_stop_tries >= STOP_TRIES) {
        debug_printf("expire_due: slot %u: gave up stopping after %u tries\n", i, o->rumble_stop_tries);
        o->rumble_low = o->rumble_high = 0;
        o->rumble_stop_tries = 0;
      }
      else {
        rumble_set(i, 0, 0, 0);
      }
    }
    else if (o->rumble_stop_repeats != 0 && (rumble_capabilities(i) & Joystick_Cap_Rumble)) {
      o->rumble_stop_repeats--;
      debug_printf("expire_due: slot %u: sending the stop again, %u more\n", i, o->rumble_stop_repeats);
      _kernel_oserror *e = j->device->rumble(i);
      if (e)
        debug_printf("expire_due: slot %u: repeated stop failed: %s\n", i, e->errmess);
      if (o->rumble_stop_repeats != 0)
        o->rumble_stop_at = deadline_cs(STOP_SPACING_CS);
    }
  }

  timer_schedule();
}


void rumble_detach(uint32_t slot)
{
  struct joy_output_struct *o = &joy_data[slot].output;

  output_close(slot);
  o->rumble_low = o->rumble_high = 0;
  o->rumble_stop_at = 0;
  o->rumble_stop_tries = 0;
  o->rumble_stop_repeats = 0;
  timer_schedule();
}


void rumble_shutdown(void)
{
  timer_cancel();

  for (uint32_t i=0; i<Joystick_MaxSlots; i++) {
    struct joy_output_struct *o = &joy_data[i].output;
    if (o->rumble_low != 0 || o->rumble_high != 0)
      rumble_set(i, 0, 0, 0);
    output_close(i);
    o->rumble_stop_at = 0;
    o->rumble_stop_tries = 0;
    o->rumble_stop_repeats = 0;
  }

  // rumble_set above can arm the timer again
  timer_cancel();
}


_kernel_oserror *rumble_timer_handler(_kernel_swi_regs *r, void *pw)
{
  IGNORE(r);
  timer_pending = FALSE;
  _swix(OS_AddCallBack, _INR(0,1), rumble_callback, pw);
  return NULL;
}


_kernel_oserror *rumble_callback_handler(_kernel_swi_regs *r, void *pw)
{
  IGNORE(r);
  IGNORE(pw);
  expire_due();
  return NULL;
}
