var noble = require('noble');
var net = require('net');

var PORT = 33333;
// var HOST = '127.0.0.1';
var HOST = '10.8.11.108';

var dgram = require('dgram');

var wave_service_uuid = '289b473c4d1e11e7b114b2f933d5fe66';
var midi_service_uuid = '03b80e5aede84b33a7516ce34ec4c700';

var clients = [];

var g_output_char;

var quaternion = [1, 0, 0, 0];
var accel = [0, 0, 0];
var gyro = [0, 0, 0];
var magnetometer = [0, 0, 0];

var click_count = [0, 0, 0];

var OPCODE_QUATERNION   = 0x01;
var OPCODE_RAW_DATA     = 0x02;
var OPCODE_TAP          = 0x03;
var OPCODE_HIT          = 0x04;
var OPCODE_CLICK        = 0x05;

var OPCODE_ACCELEROMETER = 0x01;
var OPCODE_GYROSCOPE = 0x02;
var OPCODE_MAGNETOMETER = 0x03;
var OPCODE_TIMESTAMP = 0x04;

var BUTTON_EVENT_DOWN = 0;
var BUTTON_EVENT_UP = 1;
var BUTTON_EVENT_LONG_PRESS = 2;
var BUTTON_EVENT_DOUBLE_CLICK = 3;

noble.on('stateChange', function(state) {
    console.log('BLE adapter state change: ', state);

    noble.startScanning([wave_service_uuid, midi_service_uuid]);
});

noble.on('discover', function(peripheral) {
    console.log('Found device with local name: ' + peripheral.advertisement.localName);
    console.log('Advertising the following service uuid\'s: ' + peripheral.advertisement.serviceUuids);

    noble.stopScanning();

    // Connect to the first peripheral we find
    peripheral.connect(function(error) {
        console.log('Connected to peripheral: ' + peripheral.uuid);

        peripheral.discoverServices(wave_service_uuid, function(error, services) {
            console.log('Discovered service:', services[0].uuid);

            var wave_service = services[0];

            wave_service.discoverCharacteristics(null, function(error, characteristics) {
                console.log('Discovered characteristics:');

                for(var i in characteristics) {
                    console.log('  ' + characteristics[i].uuid);
                }

                g_output_char = characteristics[0];

                // Set up a callback for new incoming data
                g_output_char.on('data', on_new_data);

                g_output_char.subscribe(function(error) {
                    if(error) {
                        console.log('Error subscribing to notifications for output characteristic');
                        return;
                    }

                    console.log('Enabled notifications for output characteristic');
                });

                wave_ready();
            });
        });
    });
});

function enable_quaternion(enable) {
    g_output_char.write(new Buffer([OPCODE_QUATERNION, enable]), true, function(error) {
        if(error) {
            console.log('Error requesting quaternion notifications: ' + error);
            return;
        }

        console.log('Requested quaternion notifications');
    });
}

function enable_raw_data(enable) {
    g_output_char.write(new Buffer([OPCODE_RAW_DATA, enable]), true, function(error) {
        if(error) {
            console.log('Error requesting raw data notifications: ' + error);
            return;
        }

        console.log('Requested raw data notifications');
    });
}

function enable_click(enable) {
    g_output_char.write(new Buffer([OPCODE_CLICK, enable]), true, function(error) {
        if(error) {
            console.log('Error requesting click notifications: ' + error);
            return;
        }

        console.log('Requested click notifications');
    });
}

function on_new_data(data, is_notification) {
    var opcode = data.readUInt8(0);

    switch(opcode) {
        case OPCODE_QUATERNION: {
            var q0 = data.readFloatLE(1);
            var q1 = data.readFloatLE(5);
            var q2 = data.readFloatLE(9);
            var q3 = data.readFloatLE(13);

            on_quaternion_data([q0, q1, q2, q3]);

            break;
        }

        case OPCODE_RAW_DATA: {
            var type = data.readUInt8(1);

            if(type != OPCODE_TIMESTAMP) {
                var x = data.readFloatLE(2);
                var y = data.readFloatLE(6);
                var z = data.readFloatLE(10);

                on_raw_data(type, [x, y, z]);
            } else {
                // console.log(data);
                // var ts = data.readUInt32LE(2);

                // on_raw_data(type, ts);z
            }

            break;
        }

        case OPCODE_CLICK: {
            var button_id = data.readUInt8(1);
            var event = data.readUInt8(2);

            on_click(button_id, event);

            break;
        }

        default:
            break;
    }
}

function on_quaternion_data(data) {
    // console.log('New quaternion data: ' + data[0] + ' ' + data[1] + ' ' + data[2] + ' ' + data[3]);

    quaternion = [data[0], data[1], data[2], data[3]];

    // clients.forEach(function (client) {
    //   // if (client === sender) return;
    //   var json = {q0 : data[0], q1 : data[1], q2 : data[2], q3 : data[3]};

    //   client.write(JSON.stringify(json));
    // });

    // var client = dgram.createSocket('udp4');

    // var buf = new Buffer(16);
    // buf.writeFloatLE(data[0], 0);
    // buf.writeFloatLE(data[1], 4);
    // buf.writeFloatLE(data[2], 8);
    // buf.writeFloatLE(data[3], 12);

    // client.send(buf, 0, buf.length, PORT, HOST, function(error, bytes) {
    //     if(error) throw error;

    //     client.close();
    // })
}

function on_raw_data(type, data) {
    switch(type) {
        case OPCODE_ACCELEROMETER:
            // console.log('New accelerometer data: ' + data[0] + ' ' + data[1] + ' ' + data[2]);
            accel = [data[0], data[1], data[2]];

            break;

        case OPCODE_GYROSCOPE:
            // console.log('New gyroscope data: ' + data[0] + ' ' + data[1] + ' ' + data[2]);
            gyro = [data[0], data[1], data[2]];

            break;

        case OPCODE_MAGNETOMETER:
            // console.log('New magnetometer data: ' + data[0] + ' ' + data[1] + ' ' + data[2]);

            magnetometer = [data[0], data[1], data[2]];
            break;

        case OPCODE_TIMESTAMP:
            // console.log('New timestamp: ' + data);
            break;

        default:
            break;

    }
}

function on_click(button_id, event) {
    // console.log("Received click from button " + button_id + ": " + event);

    if(event == BUTTON_EVENT_DOWN) {
        click_count[button_id-1]++;
    }
}

function wave_ready() {
    enable_click(true);

    enable_quaternion(true);
    enable_raw_data(true);

    setInterval(function(arg) {

        clients.forEach(function (client) {
        // if (client === sender) return;
            var json = {
                q0 : quaternion[0],
                q1 : quaternion[1],
                q2 : quaternion[2],
                q3 : quaternion[3],
                ax : accel[0],
                ay : accel[1],
                az : accel[2],
                b1 : click_count[0],
                b2 : click_count[1],
                b3 : click_count[2]
            };

            console.log(json);

            client.write(JSON.stringify(json));
        });
    }, 10);
}

net.createServer(function (socket) {

  socket.name = socket.remoteAddress + ":" + socket.remotePort
  clients.push(socket);

  // socket.write("Welcome " + socket.name + "\n");
  broadcast(socket.name + " joined the chat\n", socket);

  socket.on('data', function (data) {
    broadcast(socket.name + "> " + data, socket);
  });

  socket.on('end', function () {
    clients.splice(clients.indexOf(socket), 1);
    broadcast(socket.name + " left the chat.\n");
  });

  function broadcast(message, sender) {
    clients.forEach(function (client) {
      if (client === sender) return;
      client.write(message);
    });
    process.stdout.write(message)
  }

}).listen(PORT);

// Put a friendly message on the terminal of the server.
console.log("Chat server running at port " * PORT + "\n");