// Dependencies:
//  - SLIPDecoder (https://git.kompot.si/g1smo/SLIPDecoder)
//  - MathLib (https://depts.washington.edu/dxscdoc/Help/Browse.html#Libraries%3EMathLib)

(

// Initialize the the receiver via SLIP decoder
~receiverPath = "/dev/ttyUSB0";
~baudRate = 115200;

OSCFunc.trace(true); // debug osc
OSCFunc.trace(false);

/*******
 * GUI *
 ******/

// Indeksirani so z IDjem
~senzorji = Dictionary.new;

// Dodaj senzorje
((1..5)).do({ |id|
  ~senzorji.put(id, AHRSensor.new(id));
});

// Olimex test
~senzorji.put(9, AHRSensor.new(9));

//~senzorji.postln;

~decoder = SLIPDecoder.new(~receiverPath);
~w = Window.new("Utopia || C²", Rect(300, 300, 600, ~senzorji.size * 20),true);
~ttyInput = TextField().string_(~receiverPath);

~elementi = ~senzorji.values.sort({ |a, b| a.id < b.id;}).collect({|s| s.getGui;});

~w.layout_(
  VLayout(
	HLayout(
      // HEADER; serial path, buttons
	  StaticText().string_("Serial path: "),
	  ~ttyInput,
	  Button().string_("Start").action_({ |butt|
		if ((~decoder.running.not), {
		  // If not running, start decoder
          ~decoder.trace(true); // debug slip decoder
		  ~decoder = SLIPDecoder.new(~ttyInput.string, ~baudRate);
		  ~decoder.start;
		  butt.string_("Stop")
		}, {
		  // Else stop the decoder
		  ~decoder.stop;
		  butt.string_("Start")
		});
	  })
	),
    [],
    // Sensor rows
    GridLayout.rows(
      *~elementi.flatten
    )
  )
);

// On window close stop the decoder
~w.onClose_({ |w|
  ~decoder.stop;
});
~w.front;

// OSC listeners for sensors
~senzorji.collect({ |s|
  var oscHeader = "/ww/" ++ s.id,
  senzor = ~senzorji[s.id];

  // Quat listener
  q = OSCdef.new((\quat ++ s.id), { |msg, time, addr, recvPort|
    var q;
    q = Quaternion.new(msg[4], msg[1], msg[2], msg[3]);

    // Count quat events
    Routine {
      senzor.eps = senzor.eps + 1;
      senzor.quat = q;
      senzor.refreshGuiQuat;

      senzor.updateEuler(q);
      senzor.refreshGuiEuler;
      senzor.euler.postln;

    }.play(AppClock);
    
  }, oscHeader ++ "/quat");

  // Acceleration listener
  q = OSCdef.new((\acc ++ s.id), { |msg, time, addr, recvPort|
    var a;
    a = msg.at((1..3));
    Routine {
      senzor.accel = a;
      senzor.refreshGuiAccel;
    }.play(AppClock);
  }, oscHeader ++ "/acc");

  // Battery listener
  q = OSCdef.new((\acc ++ s.id), { |msg, time, addr, recvPort|
    var b = msg[1];
// Since we get a battery reading every second, use this as the events/s refresh
    Routine {
      senzor.battery = b;
      senzor.refreshGuiBat;
    }.play(AppClock);
  }, oscHeader ++ "/bat");

});

// Events per second counter
AppClock.clear;
AppClock.sched(0, {
  ~senzorji.collect({ |s|
    s.refreshGuiEps;
    s.eps = 0;
    // Do this every second
  });
  1;
});
)


~decoder.trace;
~decoder.stop;
~decoder.start;

OSCFunc.freeAll;

~decoder.rate;

/**********
 * SOUND! *
 *********/
 
~e = Env([1, 0.2, 0]);
{[SinOsc.ar(50), SinOsc.ar(52.3)] * EnvGen.kr(~e, doneAction: Done.freeSelf)}.play;