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

Quarks.install("https://github.com/cappelnord/OSCRecorder.git");

OSCRecorderGUI();

NetAddr.langPort;
OSCFunc.trace(true);
(

// Initialize the the receiver via SLIP decoder
~receiverPath = "/dev/ttyACM0";
~baudRate = 230400;

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

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

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

// Dodaj senzorje
// Pari: parent/child; core nima parenta, ostali pa imajo "relativno rotacijo", ki je odvisna od jedra

~razmerja = [
  // Core
  [1, nil],
  //[6, 1],
  [6, 1],
  [3, 6],
  [7, nil]
].do({ |par|
  var s = AHRSensor.new(par[0], par[1]);
  ~senzorji.put(par[0], s);
  ~starsi.put(par[1], s);
});
/*
~senzorji.put(1, AHRSensor.new(1));
~senzorji.put(2, AHRSensor.new(2));
// Ostali (3 do 6)
((3..6)).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, ~senzorji.size * 60, 600, ~senzorji.size * 100),true);

// Midi naprava
MIDIClient.init;
MIDIClient.destinations;
~midi = MIDIOut.new(0);
~midi.connect(1);

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

~startStopDecoder = { |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")
  };
  );
};

~ttyInput = TextField().string_(~receiverPath).keyDownAction_({ |input, char| if (char == $\r, { ~startStopDecoder.value(~ttyInput); }); });

~w.layout_(
  VLayout(
	HLayout(
      // HEADER; serial path, buttons
	  StaticText().string_("Serial path: "),
	  ~ttyInput,
	  Button().string_("Start").action_(~startStopDecoder);
	),
    // 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;
    // We get X Y Z W, but supercollider has W X Y Z!
    q = Quaternion.new(msg[4], msg[1], msg[2], msg[3]);

    // Count quat events
    Routine {
      var quatD;

      senzor.eps = senzor.eps + 1;
      senzor.quat = q;
      senzor.refreshGuiQuat;

      //quatD = senzor.calibQuat.conjugate * q;
      quatD = senzor.calibQuat.reciprocal * q;
      senzor.updateEuler(quatD);
      senzor.refreshGuiEuler;

      // Relativni euler koti (glede na starsa)
      if (senzor.parent != nil) {
        senzor.updateEulerD(q);
        senzor.refreshGuiEulerD;
      };
    }.play(AppClock);

  }, oscHeader ++ "/quat");

  // Acceleration listener
  q = OSCdef.new((\acc ++ s.id), { |msg, time, addr, recvPort|
    var a;
    a = msg.at((1..3));
    Routine {
      var vectorSum = 0, xy = 0, xyz = 0;

      senzor.accel = a;

      // Vsota vektorjev:
      xy = sqrt((a[0] * a[0]) + (a[1] * a[1]));
      xyz = sqrt((xy * xy) + (a[2] * a[2]));
      senzor.accelSum = xyz;

      ~midi.control(s.id, 1, xyz);

      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;