Move (Gem)

s32 gemInit(const gemAttribute* attr);

typedef struct _gem_attribute {
    u32 version;                        // Set to MOVE_VERSION (2)
    u32 max;                            // Max controllers (1-4)
    void* memory ATTRIBUTE_PRXPTR;      // Memory pointer (NULL for auto)
    Spurs* spurs ATTRIBUTE_PRXPTR;      // SPURS instance
    u8 spu_priorities[8];               // SPU priority levels
} gemAttribute;

#include <io/move.h>
#include <sysmodule/sysmodule.h>
#include <spurs/spurs.h>

// Load required modules
SysLoadModule(SYSMODULE_CAM);
SysLoadModule(SYSMODULE_GEM);

// Initialize SPURS (required for Move)
Spurs spurs;
// ... SPURS initialization code ...

// Configure Move
gemAttribute gem_attr;
memset(&gem_attr, 0, sizeof(gemAttribute));
gem_attr.version = MOVE_VERSION;
gem_attr.max = 4;  // Support up to 4 controllers
gem_attr.spurs = &spurs;
gem_attr.memory = NULL;  // Auto-allocate

// Initialize Move
if (gemInit(&gem_attr) != 0) {
    printf("Failed to initialize Move\n");
    return -1;
}

s32 gemEnd();

s32 gemGetMemorySize(s32 max);

s32 gemPrepareCamera(s32 maxexposure, f32 quality);

// Prepare camera with auto exposure and high quality
gemPrepareCamera(500, 0.9f);

s32 gemGetCameraState(gemCameraState* state);

typedef struct _gem_cam_state {
    s32 exposure;               // Current exposure value
    f32 exposure_time;          // Exposure time in seconds
    f32 gain;                   // Camera gain
    f32 pitch_angle;            // Camera pitch angle
    f32 pitch_angle_estimate;   // Estimated pitch angle
} gemCameraState;

s32 gemEnableCameraPitchAngleCorrection(s32 enable);

s32 gemGetInfo(gemInfo* info);

typedef struct _gem_info {
    u32 max;                // Maximum controllers supported
    u32 connected;          // Number of connected controllers
    u32 status[MAX_MOVES];  // Status for each controller
    u32 port[MAX_MOVES];    // Port number for each controller
} gemInfo;

gemInfo info;
gemGetInfo(&info);

printf("Max controllers: %d, Connected: %d\n", info.max, info.connected);

for (int i = 0; i < MAX_MOVES; i++) {
    if (info.status[i]) {
        printf("Move %d connected on port %d\n", i, info.port[i]);
    }
}

s32 gemTrackHues(const u32* req_hues, u32* res_hues);

s32 gemGetTrackerHue(u32 num, u32* hue);

s32 gemIsTrackableHue(u32 hue);

s32 gemGetAllTrackableHues(u8* hues);

s32 gemSetRGB(u32 num, f32 r, f32 g, f32 b);

s32 gemGetRGB(u32 num, f32* r, f32* g, f32* b);

s32 gemForceRGB(u32 num, f32 r, f32 g, f32 b);

// Set to red
gemForceRGB(0, 1.0f, 0.0f, 0.0f);

// Set to green
gemForceRGB(0, 0.0f, 1.0f, 0.0f);

// Set to blue
gemForceRGB(0, 0.0f, 0.0f, 1.0f);

// Set to purple
gemForceRGB(0, 0.5f, 0.0f, 0.5f);

// Get current color
f32 r, g, b;
gemGetRGB(0, &r, &g, &b);
printf("Current color: R=%.2f G=%.2f B=%.2f\n", r, g, b);

s32 gemHSVtoRGB(f32 h, f32 s, f32 v, f32* r, f32* g, f32* b);

s32 gemSetRumble(u32 num, u8 intensity);

s32 gemGetRumble(u32 num, u8* intensity);

// Strong rumble
gemSetRumble(0, 255);
usleep(500000);  // 0.5 seconds

// Medium rumble
gemSetRumble(0, 128);
usleep(500000);

// Turn off rumble
gemSetRumble(0, 0);

s32 gemUpdateStart(const void* camera_frame, system_time_t timestamp);

s32 gemUpdateFinish();

s32 gemGetState(u32 num, u32 timeflag, system_time_t time, gemState* state);

typedef struct _gem_state {
    vec_float4 pos;             // Position (X, Y, Z, W)
    vec_float4 vel;             // Velocity
    vec_float4 accel;           // Acceleration
    vec_float4 quat;            // Orientation quaternion
    vec_float4 angvel;          // Angular velocity
    vec_float4 angaccel;        // Angular acceleration
    vec_float4 handle_pos;      // Handle position
    vec_float4 handle_vel;      // Handle velocity
    vec_float4 handle_accel;    // Handle acceleration
    gemPadData paddata;         // Button data
    gemExtPortData extportdata; // Extension port data (Navigation controller)
    system_time_t time;         // Timestamp
    f32 temperature;            // Controller temperature
    f32 camera_pitch_angle;     // Camera pitch angle
    u32 tracking;               // Tracking flags
} gemState;

typedef struct _gem_pad_data {
    u16 buttons;    // Button bitfield
    u16 ANA_T;      // Trigger analog value (0-65535)
} gemPadData;

// buttons bitfield values (same as pad buttons)
BTN_CROSS       // Cross button
BTN_CIRCLE      // Circle button  
BTN_SQUARE      // Square button
BTN_TRIANGLE    // Triangle button
BTN_START       // Start button
BTN_SELECT      // Select button
// Move-specific:
// Bit 8: Move button
// Bit 9: T button (trigger digital)

gemState state;

while (running) {
    // Update tracking
    gemUpdateStart(camera_frame, timestamp);
    gemUpdateFinish();
    
    // Get controller state
    gemGetState(0, STATE_CURRENT_TIME, 0, &state);
    
    // Check if tracking is active
    if (state.tracking & GEM_TRACKING_POSITION_TRACKED) {
        // Position is valid
        printf("Position: X=%.2f Y=%.2f Z=%.2f\n",
               state.pos[0], state.pos[1], state.pos[2]);
    }
    
    if (state.tracking & GEM_TRACKING_VISIBLE) {
        printf("Controller is visible\n");
    }
    
    // Check buttons
    if (state.paddata.buttons & (1 << 8)) {  // Move button
        printf("Move button pressed\n");
    }
    
    if (state.paddata.buttons & (1 << 9)) {  // Trigger
        printf("Trigger pressed\n");
    }
    
    // Read trigger analog value
    f32 trigger = state.paddata.ANA_T / 65535.0f;
    printf("Trigger: %.2f%%\n", trigger * 100.0f);
    
    // Read orientation
    printf("Orientation: X=%.2f Y=%.2f Z=%.2f W=%.2f\n",
           state.quat[0], state.quat[1], state.quat[2], state.quat[3]);
    
    // Temperature
    printf("Temperature: %.1f°C\n", state.temperature);
}

s32 gemGetInertialState(u32 num, u32 flag, system_time_t time, 
                        gemInertialState* inertial);

typedef struct _gem_inertial_state {
    vec_float4 accelerometer;       // Accelerometer data (X, Y, Z in G)
    vec_float4 gyro;                // Gyroscope data (X, Y, Z in rad/s)
    vec_float4 accelerometer_bias;  // Accelerometer bias
    vec_float4 gyro_bias;           // Gyroscope bias
    gemPadData pad;                 // Button data
    gemExtPortData ext;             // Extension port data
    system_time_t time;             // Timestamp
    s32 counter;                    // Sample counter
    f32 temperature;                // Temperature
} gemInertialState;

gemInertialState inertial;
gemGetInertialState(0, GEM_INERTIAL_LATEST, 0, &inertial);

// Read accelerometer (in G units)
printf("Accel: X=%.2f Y=%.2f Z=%.2f G\n",
       inertial.accelerometer[0],
       inertial.accelerometer[1],
       inertial.accelerometer[2]);

// Read gyroscope (in radians/second)
printf("Gyro: X=%.2f Y=%.2f Z=%.2f rad/s\n",
       inertial.gyro[0],
       inertial.gyro[1],
       inertial.gyro[2]);

s32 gemGetImageState(u32 num, gemImageState* state);

typedef struct _gem_img_state {
    system_time_t frame_time;   // Frame timestamp
    system_time_t time;         // State timestamp
    f32 u;                      // Image U coordinate (0-1)
    f32 v;                      // Image V coordinate (0-1)
    f32 r;                      // Sphere radius in image
    f32 projectionx;            // X projection
    f32 projectiony;            // Y projection
    f32 distance;               // Distance from camera
    u8 visible;                 // Visibility flag
    u8 r_valid;                 // Radius valid flag
} gemImageState;

s32 gemCalibrate(u32 num);

s32 gemInvalidateCalibration(u32 num);

s32 gemReset(u32 num);

s32 gemEnableMagnetometer(u32 num, s32 enable);

s32 gemFilterState(u32 num, u32 enable);

s32 gemSetYaw(u32 num, vec_float4 zdir);

s32 gemGetAccelerometerPositionInDevice(u32 num, vec_float4* pos);

s32 gemGetStatusFlags(u32 num, u64* flags);

s32 gemClearStatusFlags(u32 num, u64 mask);

typedef struct _gem_ext_port_data {
    u16 isconnected;            // Navigation controller connected
    
    // Digital buttons (same layout as DualShock)
    unsigned int BTN_LEFT : 1;
    unsigned int BTN_DOWN : 1;
    unsigned int BTN_RIGHT : 1;
    unsigned int BTN_UP : 1;
    unsigned int BTN_START : 1;
    unsigned int BTN_R3 : 1;
    unsigned int BTN_L3 : 1;
    unsigned int BTN_SELECT : 1;
    
    unsigned int BTN_SQUARE : 1;
    unsigned int BTN_CROSS : 1;
    unsigned int BTN_CIRCLE : 1;
    unsigned int BTN_TRIANGLE : 1;
    unsigned int BTN_R1 : 1;
    unsigned int BTN_L1 : 1;
    unsigned int BTN_R2 : 1;
    unsigned int BTN_L2 : 1;
    
    // Analog stick
    unsigned int ANA_R_H : 16;  // Right analog horizontal
    unsigned int ANA_R_V : 16;  // Right analog vertical
    unsigned int ANA_L_H : 16;  // Left analog horizontal
    unsigned int ANA_L_V : 16;  // Left analog vertical
    
    u8 data[5];                 // Additional data
} gemExtPortData;

s32 gemWriteExternalPort(u32 num, u8 data[EXTERNAL_PORT_DATA_SIZE]);

s32 gemPrepareVideoConvert(const gemVideoConvertAttribute* attr);

s32 gemConvertVideoStart(const void* frame);

s32 gemConvertVideoFinish();

s32 gemGetHuePixels(const void* frame, u32 hue, u8* pixels);

s32 gemGetEnvironmentLightingColor(f32* r, f32* g, f32* b);

#define MOVE_VERSION                    2       // API version
#define MAX_MOVES                       4       // Maximum controllers
#define EXTERNAL_PORT_DATA_SIZE         32      // Extension port data size

// Tracking flags
#define GEM_TRACKING_POSITION_TRACKED   1       // Position is tracked
#define GEM_TRACKING_VISIBLE            2       // Controller is visible

// Time flags
#define STATE_CURRENT_TIME              0       // Current state
#define STATE_LATEST_IMAGE_TIME         1       // Latest image time
#define STATE_SPECIFY_TIME              2       // Specific time

// Camera flags
#define GEM_AUTO_WHITE_BALANCE          1       // Auto white balance
#define GEM_GAMMA_BOOST                 2       // Gamma boost
#define GEM_COMBINE_PREVIOUS_INPUT_FRAME 4      // Combine previous frame
#define GEM_FILTER_OUTLIER_PIXELS       8       // Filter outliers

// Inertial flags
#define GEM_INERTIAL_LATEST             0       // Latest reading
#define GEM_INERTIAL_PREVIOUS           1       // Previous reading
#define GEM_INERTIAL_NEXT               2       // Next reading

#include <io/move.h>
#include <io/camera.h>
#include <sysmodule/sysmodule.h>
#include <spurs/spurs.h>
#include <stdio.h>

int main() {
    // Load modules
    SysLoadModule(SYSMODULE_CAM);
    SysLoadModule(SYSMODULE_GEM);
    
    // Initialize SPURS
    Spurs spurs;
    // ... SPURS initialization ...
    
    // Initialize Move
    gemAttribute gem_attr;
    memset(&gem_attr, 0, sizeof(gemAttribute));
    gem_attr.version = MOVE_VERSION;
    gem_attr.max = 4;
    gem_attr.spurs = &spurs;
    gem_attr.memory = NULL;
    
    if (gemInit(&gem_attr) != 0) {
        printf("Failed to initialize Move\n");
        return -1;
    }
    
    // Prepare camera
    gemPrepareCamera(500, 0.9f);
    
    // Get Move info
    gemInfo info;
    gemGetInfo(&info);
    printf("Connected controllers: %d\n", info.connected);
    
    if (info.connected == 0) {
        printf("No Move controllers connected\n");
        gemEnd();
        return -1;
    }
    
    // Track hues for connected controllers
    u32 req_hues[MAX_MOVES] = {0};
    u32 res_hues[MAX_MOVES] = {0};
    
    // Request automatic hue assignment
    for (int i = 0; i < info.connected; i++) {
        req_hues[i] = 0;  // 0 = auto-assign
    }
    
    gemTrackHues(req_hues, res_hues);
    
    // Set LED colors based on assigned hues
    for (int i = 0; i < info.connected; i++) {
        f32 r, g, b;
        gemHSVtoRGB(res_hues[i], 1.0f, 1.0f, &r, &g, &b);
        gemForceRGB(i, r, g, b);
        printf("Move %d: Hue=%d, RGB=(%.2f, %.2f, %.2f)\n",
               i, res_hues[i], r, g, b);
    }
    
    // Main loop
    gemState state;
    int frame = 0;
    
    while (frame < 1000) {  // Run for 1000 frames
        // Update tracking (assumes camera frame available)
        // gemUpdateStart(camera_frame, timestamp);
        gemUpdateFinish();
        
        // Read first controller
        gemGetState(0, STATE_CURRENT_TIME, 0, &state);
        
        // Check tracking
        if (state.tracking & GEM_TRACKING_POSITION_TRACKED) {
            // Position is tracked
            printf("Frame %d: Pos=(%.2f, %.2f, %.2f) ",
                   frame, state.pos[0], state.pos[1], state.pos[2]);
            
            // Check Move button
            if (state.paddata.buttons & (1 << 8)) {
                printf("MOVE ");
                // Vibrate on button press
                gemSetRumble(0, 255);
            } else {
                gemSetRumble(0, 0);
            }
            
            // Check trigger
            if (state.paddata.buttons & (1 << 9)) {
                printf("TRIGGER ");
            }
            
            // Trigger analog
            f32 trigger = state.paddata.ANA_T / 65535.0f;
            if (trigger > 0.1f) {
                printf("T=%.0f%% ", trigger * 100.0f);
            }
            
            printf("\n");
        } else {
            printf("Frame %d: Not tracking\n", frame);
        }
        
        // Check extension port (Navigation controller)
        if (state.extportdata.isconnected) {
            if (state.extportdata.BTN_L3) {
                printf("Navigation L3 pressed\n");
            }
        }
        
        frame++;
        usleep(16667);  // ~60 FPS
    }
    
    // Cleanup
    gemEnd();
    SysUnloadModule(SYSMODULE_GEM);
    SysUnloadModule(SYSMODULE_CAM);
    
    return 0;
}