/* Copyright (C) 2024 Kevin Koster, OmberTech. All rights reserved.

 This software may be distributed and modified under the terms of the GNU
 General Public License version 2 (GPL2) as published by the Free Software
 Foundation and appearing in the file GPL2.TXT included in the packaging of
 this file. Please note that GPL2 Section 2[b] requires that all works based
 on this software must also be made publicly available under the terms of
 the GPL2 ("Copyleft").
*/

#include "HIDJoyParser.h"


//#define PRINTREPORT
//#define PRINTINPUTS

uint8_t hatValue[3];
float hatScale[3];
bool has_report_id;
uint8_t joy_report_id;

enum DPADEnum {
        DPAD_UP = 0x0,
        DPAD_UP_RIGHT = 0x1,
        DPAD_RIGHT = 0x2,
        DPAD_RIGHT_DOWN = 0x3,
        DPAD_DOWN = 0x4,
        DPAD_DOWN_LEFT = 0x5,
        DPAD_LEFT = 0x6,
        DPAD_LEFT_UP = 0x7,
        DPAD_OFF = 0xF,
};

bool HIDJoyParser::checkDpad(ButtonEnum b) {
        DPADEnum dpad = (DPADEnum)(0xF & hid_simple_joysicks->values.hat);
        switch (b) {
                case UP:
                        return    dpad == DPAD_LEFT_UP
                               || dpad == DPAD_UP
                               || dpad == DPAD_UP_RIGHT;
                case RIGHT:
                        return    dpad == DPAD_UP_RIGHT
                               || dpad == DPAD_RIGHT
                               || dpad == DPAD_RIGHT_DOWN;
                case DOWN:
                        return    dpad == DPAD_RIGHT_DOWN
                               || dpad == DPAD_DOWN
                               || dpad == DPAD_DOWN_LEFT;
                case LEFT:
                        return    dpad == DPAD_DOWN_LEFT
                               || dpad == DPAD_LEFT
                               || dpad == DPAD_LEFT_UP;
                default:
                        return false;
        }
}

bool HIDJoyParser::getButtonPress(ButtonEnum b) {
        if (b <= LEFT) // Dpad
                return checkDpad(b);
        else
                return hid_simple_joysicks->values.buttons & (1UL << pgm_read_byte(&HIDJOY_BUTTONS[(uint8_t)b]));
}

bool HIDJoyParser::getButtonClick(ButtonEnum b) {
        uint32_t mask = 1UL << pgm_read_byte(&HIDJOY_BUTTONS[(uint8_t)b]);
        bool click = buttonClickState.val & mask;
        buttonClickState.val &= ~mask; // Clear "click" event
        return click;
}

uint8_t HIDJoyParser::getAnalogHat(AnalogHatEnum a) {
     return hatValue[(uint8_t)a];
}

void HIDJoyParser::Reset() {
        uint8_t i;

        initDone = false;
        for (i = 0; i < 4; i++) {
          hatValue[i] = 127; // Centre value
        }
        oldButtonState.val = 0;
        oldButtonState.dpad = GAMEPAD_HAT_CENTERED;
        buttonClickState.dpad = 0;
        oldDpad = 0;
        oldHat = 0;
        has_report_id = false;
        joy_report_id = 0;
};

#define MAX_REPORT  4
void HIDJoyParser::Init() {
    /* See hid1_11.pdf pg. 21 (PDF pg. 31) and mouse examples on pg. 70 (PDF pg. 80) */
    uint8_t *desc_report = NULL; // HID Report Descriptor - Describes format of interface's HID Reports
    uint16_t desc_len;
    uint8_t report_count;
    tuh_hid_report_info_t reports[MAX_REPORT];
    tuh_hid_report_info_t *report;
    int32_t logical_range;

#ifdef DEBUG_USB_HOST
    puts("Joy Init");
#endif
    Reset();

    desc_report = getHIDReportDescriptor(&desc_len);
    if (desc_report == NULL) {
#ifdef DEBUG_USB_HOST
      puts("(HIDJoyParser::Init) Null report descriptor pointer");
#endif
      return;
    }
    report_count = tuh_hid_parse_report_descriptor(reports, MAX_REPORT, desc_report, desc_len);
#ifdef DEBUG_USB_HOST
    printf("(HIDJoyParser::Init) HID has %u report/s\n", report_count);
#endif
    for (uint8_t i = 0; i < report_count; ++i) {
      report = &reports[i];
      has_report_id = report_count > 1 || (report[0].report_id > 0);
#ifdef DEBUG_USB_HOST
      printf("(HIDJoyParser::Init) HID report usage_page=%d, usage=%d, has_report_id=%d\n", report->usage_page, report->usage, has_report_id);
#endif
      if (report->usage_page == HID_USAGE_PAGE_DESKTOP
          && (report->usage == HID_USAGE_DESKTOP_JOYSTICK || report->usage == HID_USAGE_DESKTOP_GAMEPAD)) {
           tuh_hid_joystick_parse_report_descriptor(desc_report, desc_len, 0, 0); // TODO: Parse while still reading report, like other descriptors?
           if (has_report_id)
             joy_report_id = report[i].report_id;
           break;
      }
    }
    free (desc_report); // Is this OK? Think so, tUSB can re-use its buffer after running the report descriptor parser.

    /* Set Analogue Input Scaling Value */
    logical_range = hid_simple_joysicks->axis_x1.logical_max - hid_simple_joysicks->axis_x1.logical_min;
    if (logical_range == 255 || logical_range == 0)
      hatScale[0] = 1;
    else
      hatScale[0] = (float)logical_range / 255;

    logical_range = hid_simple_joysicks->axis_y1.logical_max - hid_simple_joysicks->axis_y1.logical_min;
    if (logical_range == 255 || logical_range == 0)
      hatScale[1] = 1;
    else
      hatScale[1] = (float)logical_range / 255;

    logical_range = hid_simple_joysicks->axis_x2.logical_max - hid_simple_joysicks->axis_x2.logical_min;
    if (logical_range == 255 || logical_range == 0)
      hatScale[2] = 1;
    else
      hatScale[2] = (float)logical_range / 255;

    logical_range = hid_simple_joysicks->axis_y2.logical_max - hid_simple_joysicks->axis_y2.logical_min;
    if (logical_range == 255 || logical_range == 0)
      hatScale[3] = 1;
    else
      hatScale[3] = (float)logical_range / 255;

    initDone = true;
}

void HIDJoyParser::Parse(uint8_t len, uint8_t *buf) {
      tusb_hid_simple_joysick_t *simple_joystick;
      tusb_hid_simple_joysick_values_t *values;

      if (initDone && len > 1 && buf && (has_report_id == false || buf[0] == joy_report_id)) {
#ifdef PRINTREPORT
        printf("(HIDJoyParser::Parse) ");
        for (uint8_t i = 0; i < len; i++) {
                printf("0x%X ", buf[i]);
        }
        puts("");
#endif
        simple_joystick = &hid_simple_joysicks[0]; // Only looking at one joystick from tUSB's joystick array

        // Parser doesn't expect a report ID byte, so jump past it
        if (has_report_id == true) {
          buf++;
          len--;
        }

        // Update input values from HID Report
        tusb_hid_simple_joysick_process_report (simple_joystick, buf, len);
        values = &simple_joystick->values;

        // Scale analogue inputs to 0-255 range according to report descriptor "Logical Min." and "Logical Max."
        if (values->x1 != oldJoyValues.x1) {
          oldJoyValues.x1 = values->x1;
          hatValue[0] = (uint8_t)((values->x1 - hid_simple_joysicks->axis_x1.logical_min) / hatScale[0]);
        }

        if (values->y1 != oldJoyValues.y1) {
          oldJoyValues.y1 = values->y1;
          hatValue[1] = (uint8_t)((values->y1 - hid_simple_joysicks->axis_y1.logical_min) / hatScale[1]);
        }

        if (values->x2 != oldJoyValues.x2) {
          oldJoyValues.x2 = values->x2;
          hatValue[2] = (uint8_t)((values->x2 - hid_simple_joysicks->axis_x2.logical_min) / hatScale[2]);
        }

        if (values->y2 != oldJoyValues.y2) {
          oldJoyValues.y2 = values->y2;
          hatValue[3] = (uint8_t)((values->y2 - hid_simple_joysicks->axis_y2.logical_min) / hatScale[3]);
        }

        values->buttons = (uint32_t)(values->buttons << 4); // Shift button state bits past D-pad buttons (UP, DOWN, LEFT, RIGHT)
        if (values->buttons != oldButtonState.val) { // Check if anything has changed
          buttonClickState.val = values->buttons & ~oldButtonState.val; // Update click state variable
          oldButtonState.val = values->buttons;
        }
        if (values->hat != oldHat) {
          oldHat = values->hat;
          // The DPAD buttons does not set the different bits, but set a value corresponding to the buttons pressed, we will simply set the bits ourself
          uint8_t newDpad = 0;
          
          if (checkDpad(UP))
                  newDpad |= 1 << UP;
          if (checkDpad(RIGHT))
                  newDpad |= 1 << RIGHT;
          if (checkDpad(DOWN))
                  newDpad |= 1 << DOWN;
          if (checkDpad(LEFT))
                  newDpad |= 1 << LEFT;
          if (newDpad != oldDpad) {
                  buttonClickState.dpad = newDpad & ~oldDpad; // Override values
                  oldDpad = newDpad;
          }
        }
#ifdef PRINTINPUTS
         tusb_hid_print_simple_joysick_report(simple_joystick);
         for (uint8_t psinput=0; psinput < 18; psinput++)
           printf("%d:%d  ", psinput, getButtonPress((ButtonEnum)psinput));
         puts ("");
#endif
      }
}