/*

* Some game thing

*/

#include <unistd.h>

#include <stdio.h>

#include <stdlib.h>

#include <math.h>

#include <psmoveapi/psmove.h>

int main(void)

{

PSMove *moves[7];

int last_xs[7];

int last_ys[7];

int last_zs[7];

float decay = 0.009;

float increase = 0.004;

// amount to get to brightness

float brightness = 0.49;

int told_everyone = 0;

int dist_thresh;

char *env_dist_thresh = getenv("DIST_THRESH");

if (env_dist_thresh == NULL) {

dist_thresh = 1000;

}

else {

dist_thresh = atoi(env_dist_thresh);

}

int i, j, move_count;

int rcolors[][3] = {

{254, 0, 0}, {255, 0, 0}, {255, 7, 0}, {255, 7, 0}, {255, 13, 0},

{255, 20, 0}, {255, 20, 0}, {255, 26, 0}, {255, 26, 0}, {255, 33, 0},

{255, 33, 0}, {255, 39, 0}, {255, 39, 0}, {255, 46, 0}, {255, 46, 0},

{255, 52, 0}, {255, 59, 0}, {255, 59, 0}, {255, 65, 0}, {255, 65, 0},

{255, 72, 0}, {255, 72, 0}, {255, 78, 0}, {255, 78, 0}, {255, 85, 0},

{255, 85, 0}, {255, 91, 0}, {255, 97, 0}, {255, 97, 0}, {255, 103, 0},

{255, 103, 0}, {255, 109, 0}, {255, 109, 0}, {255, 116, 0}, {255, 116, 0},

{255, 122, 0}, {255, 122, 0}, {255, 128, 0}, {255, 134, 0}, {255, 134, 0},

{255, 140, 0}, {255, 140, 0}, {255, 146, 0}, {255, 146, 0}, {255, 153, 0},

{255, 153, 0}, {255, 159, 0}, {255, 159, 0}, {255, 165, 0}, {255, 171, 0},

{255, 171, 0}, {255, 177, 0}, {255, 177, 0}, {255, 183, 0}, {255, 183, 0},

{255, 190, 0}, {255, 190, 0}, {255, 196, 0}, {255, 196, 0}, {255, 202, 0},

{255, 208, 0}, {255, 208, 0}, {255, 213, 0}, {255, 213, 0}, {255, 218, 0},

{255, 218, 0}, {255, 224, 0}, {255, 224, 0}, {255, 229, 0}, {255, 234, 0},

{255, 234, 0}, {255, 239, 0}, {255, 239, 0}, {255, 245, 0}, {255, 245, 0},

{255, 250, 0}, {255, 250, 0}, {255, 255, 0}, {255, 255, 0}, {250, 255, 0},

{246, 255, 0}, {246, 255, 0}, {241, 255, 0}, {241, 255, 0}, {236, 255, 0},

{236, 255, 0}, {230, 255, 0}, {230, 255, 0}, {224, 255, 0}, {224, 255, 0},

{218, 255, 0}, {212, 255, 0}, {212, 255, 0}, {206, 255, 0}, {206, 255, 0},

{200, 255, 0}, {200, 255, 0}, {194, 255, 0}, {194, 255, 0}, {188, 255, 0},

{188, 255, 0}, {181, 255, 0}, {174, 255, 0}, {174, 255, 0}, {167, 255, 0},

{167, 255, 0}, {160, 255, 0}, {160, 255, 0}, {153, 255, 0}, {153, 255, 0},

{146, 255, 0}, {146, 255, 0}, {139, 255, 0}, {132, 255, 0}, {132, 255, 0},

{125, 255, 0}, {125, 255, 0}, {118, 255, 0}, {118, 255, 0}, {111, 255, 0},

{111, 255, 0}, {103, 255, 0}, {103, 255, 0}, {96, 255, 0}, {88, 255, 0},

{88, 255, 0}, {81, 255, 0}, {81, 255, 0}, {73, 255, 0}, {73, 255, 0},

{66, 255, 0}, {66, 255, 0}, {58, 255, 0}, {50, 255, 0}, {50, 255, 0},

{43, 255, 0}, {43, 255, 0}, {35, 255, 0}, {35, 255, 0}, {28, 255, 0},

{28, 255, 0}, {20, 255, 0}, {20, 255, 0}, {13, 255, 0}, {5, 255, 0},

{5, 255, 0}, {0, 255, 0}, {0, 255, 0}, {0, 255, 4}, {0, 255, 4},

{0, 255, 9}, {0, 255, 9}, {0, 255, 13}, {0, 255, 13}, {0, 255, 18},

{0, 255, 22}, {0, 255, 22}, {0, 255, 27}, {0, 255, 27}, {0, 255, 31},

{0, 255, 31}, {0, 255, 36}, {0, 255, 36}, {0, 255, 42}, {0, 255, 42},

{0, 255, 47}, {0, 255, 52}, {0, 255, 52}, {0, 255, 58}, {0, 255, 58},

{0, 255, 63}, {0, 255, 63}, {0, 255, 68}, {0, 255, 68}, {0, 255, 74},

{0, 255, 74}, {0, 255, 79}, {0, 255, 84}, {0, 255, 84}, {0, 255, 90},

{0, 255, 90}, {0, 255, 95}, {0, 255, 95}, {0, 255, 100}, {0, 255, 100},

{0, 255, 106}, {0, 255, 106}, {0, 255, 111}, {0, 255, 117}, {0, 255, 117},

{0, 255, 122}, {0, 255, 122}, {0, 255, 128}, {0, 255, 128}, {0, 255, 134},

{0, 255, 134}, {0, 255, 140}, {0, 255, 145}, {0, 255, 145}, {0, 255, 151},

{0, 255, 151}, {0, 255, 157}, {0, 255, 157}, {0, 255, 163}, {0, 255, 163},

{0, 255, 168}, {0, 255, 168}, {0, 255, 174}, {0, 255, 180}, {0, 255, 180},

{0, 255, 185}, {0, 255, 185}, {0, 255, 191}, {0, 255, 191}, {0, 255, 197},

{0, 255, 197}, {0, 255, 203}, {0, 255, 203}, {0, 255, 208}, {0, 255, 214},

{0, 255, 214}, {0, 255, 220}, {0, 255, 220}, {0, 255, 226}, {0, 255, 226},

{0, 255, 231}, {0, 255, 231}, {0, 255, 237}, {0, 255, 237}, {0, 255, 242},

{0, 255, 246}, {0, 255, 246}, {0, 255, 251}, {0, 255, 251}, {0, 255, 255},

{0, 255, 255}, {0, 249, 255}, {0, 249, 255}, {0, 242, 255}, {0, 242, 255},

{0, 236, 255}, {0, 230, 255}, {0, 230, 255}, {0, 223, 255}, {0, 223, 255},

{0, 217, 255}, {0, 217, 255}, {0, 211, 255}, {0, 211, 255}, {0, 204, 255},

{0, 204, 255}, {0, 198, 255}, {0, 192, 255}, {0, 192, 255}, {0, 185, 255},

{0, 185, 255}, {0, 179, 255}, {0, 179, 255}, {0, 173, 255}, {0, 173, 255},

{0, 166, 255}, {0, 160, 255}, {0, 160, 255}, {0, 153, 255}, {0, 153, 255},

{0, 147, 255}, {0, 147, 255}, {0, 140, 255}, {0, 140, 255}, {0, 133, 255},

{0, 133, 255}, {0, 127, 255}, {0, 120, 255}, {0, 120, 255}, {0, 113, 255},

{0, 113, 255}, {0, 107, 255}, {0, 107, 255}, {0, 100, 255}, {0, 100, 255},

{0, 93, 255}, {0, 93, 255}, {0, 87, 255}, {0, 80, 255}, {0, 80, 255},

{0, 73, 255}, {0, 73, 255}, {0, 67, 255}, {0, 67, 255}, {0, 60, 255},

{0, 60, 255}, {0, 53, 255}, {0, 53, 255}, {0, 47, 255}, {0, 40, 255},

{0, 40, 255}, {0, 33, 255}, {0, 33, 255}, {0, 27, 255}, {0, 27, 255},

{0, 20, 255}, {0, 20, 255}, {0, 13, 255}, {0, 13, 255}, {0, 7, 255},

{0, 0, 255}, {0, 0, 255}, {6, 0, 255}, {6, 0, 255}, {11, 0, 255},

{11, 0, 255}, {17, 0, 255}, {17, 0, 255}, {22, 0, 255}, {22, 0, 255},

{28, 0, 255}, {33, 0, 255}, {33, 0, 255}, {39, 0, 255}, {39, 0, 255},

{44, 0, 255}, {44, 0, 255}, {50, 0, 255}, {50, 0, 255}, {55, 0, 255},

{61, 0, 255}, {61, 0, 255}, {67, 0, 255}, {67, 0, 255}, {72, 0, 255},

{72, 0, 255}, {78, 0, 255}, {78, 0, 255}, {83, 0, 255}, {83, 0, 255},

{89, 0, 255}, {94, 0, 255}, {94, 0, 255}, {100, 0, 255}, {100, 0, 255},

{105, 0, 255}, {105, 0, 255}, {111, 0, 255}, {111, 0, 255}, {116, 0, 255},

{116, 0, 255}, {122, 0, 255}, {127, 0, 255}, {127, 0, 255}, {133, 0, 255},

{133, 0, 255}, {139, 0, 255}, {139, 0, 255}, {144, 0, 255}, {144, 0, 255},

{150, 0, 255}, {150, 0, 255}, {155, 0, 255}, {161, 0, 255}, {161, 0, 255},

{166, 0, 255}, {166, 0, 255}, {172, 0, 255}, {172, 0, 255}, {177, 0, 255},

{177, 0, 255}, {183, 0, 255}, {183, 0, 255}, {188, 0, 255}, {194, 0, 255},

{194, 0, 255}, {200, 0, 255}, {200, 0, 255}, {205, 0, 255}, {205, 0, 255},

{211, 0, 255}, {211, 0, 255}, {216, 0, 255}, {216, 0, 255}, {222, 0, 255},

{227, 0, 255}, {227, 0, 255}, {233, 0, 255}, {233, 0, 255}, {238, 0, 255},

{238, 0, 255}, {244, 0, 255}, {244, 0, 255}, {249, 0, 255}, {255, 0, 255},

{255, 0, 255}, {255, 0, 249}, {255, 0, 249}, {255, 0, 243}, {255, 0, 243},

{255, 0, 237}, {255, 0, 237}, {255, 0, 230}, {255, 0, 230}, {255, 0, 224},

{255, 0, 218}, {255, 0, 218}, {255, 0, 212}, {255, 0, 212}, {255, 0, 206},

{255, 0, 206}, {255, 0, 200}, {255, 0, 200}, {255, 0, 193}, {255, 0, 193},

{255, 0, 187}, {255, 0, 181}, {255, 0, 181}, {255, 0, 175}, {255, 0, 175},

{255, 0, 169}, {255, 0, 169}, {255, 0, 163}, {255, 0, 163}, {255, 0, 157},

{255, 0, 157}, {255, 0, 150}, {255, 0, 144}, {255, 0, 144}, {255, 0, 138},

{255, 0, 138}, {255, 0, 132}, {255, 0, 132}, {255, 0, 126}, {255, 0, 126},

{255, 0, 120}, {255, 0, 120}, {255, 0, 113}, {255, 0, 107}, {255, 0, 107},

{255, 0, 101}, {255, 0, 101}, {255, 0, 95}, {255, 0, 95}, {255, 0, 89},

{255, 0, 89}, {255, 0, 83}, {255, 0, 83}, {255, 0, 77}, {255, 0, 71},

{255, 0, 71}, {255, 0, 65}, {255, 0, 65}, {255, 0, 59}, {255, 0, 59},

{255, 0, 53}, {255, 0, 53}, {255, 0, 48}, {255, 0, 42}, {255, 0, 42},

{255, 0, 36}, {255, 0, 36}, {255, 0, 30}, {255, 0, 30}, {255, 0, 24},

{255, 0, 24}, {255, 0, 18}, {255, 0, 18}, {255, 0, 12}, {255, 0, 6},

{255, 0, 6}, {255, 0, 0}, {255, 0, 0}

};

for (i = 7; i >= 0; i--) {

last_xs[i] = 0;

last_ys[i] = 0;

last_zs[i] = 0;

}

move_count = psmove_count_connected();

printf("Connected controllers: %d\n", move_count);

if (move_count < 1) {

printf("You must have at least 1 player!\n");

exit(1);

}

for (i = move_count-1; i >= 0; i--) {

moves[i] = psmove_connect_by_id(i);

if (moves[i] == NULL) {

printf("Could not connect to controller\n");

exit(1);

}

else {

/* Enable rate limiting for LED updates */

psmove_set_rate_limiting(moves[i], 1);

}

}

// reset

for (i = move_count-1; i >= 0; i--) {

psmove_set_leds(moves[i], 0, 0, 0);

psmove_set_rumble(moves[i], 0);

psmove_update_leds(moves[i]);

}

int colors[] = {0, 0, 0, 0, 0, 0, 0};

int trigger_downs[] = {0, 0, 0, 0, 0, 0, 0};

int max_color = (sizeof(rcolors) / sizeof(int)) / 3;

while (!(psmove_get_buttons(moves[0]) & Btn_PS)) {

for (i = move_count-1; i >= 0; i--) {

PSMove *move = moves[i];

int res = psmove_poll(move);

if (res) {

int x, y, z;

psmove_get_gyroscope(move, &x, &y, &z);

int diff_x = x - last_xs[i];

int diff_y = y - last_ys[i];

int diff_z = z - last_zs[i];

// update last pos

last_xs[i] = x;

last_ys[i] = y;

last_zs[i] = z;

float distance = sqrt((diff_x * diff_x) + (diff_y * diff_y) + (diff_z * diff_z));

if (distance > dist_thresh) {

colors[i] = colors[i] + 1;

if (colors[i] >= max_color) { colors[i] = 0; }

psmove_set_leds(move, rcolors[colors[i]][0] * brightness, rcolors[colors[i]][1] * brightness, rcolors[colors[i]][2] * brightness);

psmove_update_leds(move);

}

}

usleep(10000);

}

}

for (i = move_count-1; i >= 0; i--) {

psmove_disconnect(moves[i]);

}

return 0;

}