// 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);
OSCFunc.trace(false);
(

// To send OSC
~n = NetAddr("127.0.0.1", 57120);

// Initialize the the receiver via SLIP decoder
~receiverPath = "/dev/ttyACM0";
//~baudRate = 115200;
~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],
  // Leva/desna roka
  [3, 1],
  [4, 1],
  // Leva/desna rama
  [5, 1],
  [6, 1]
  // Testni
  //[7, 1]
  /*
  [6, 4]
  */
  // Test
  //[2, nil],
  //[7, 2]
];

// Senzorji
~razmerja.do({ |par|
  var s = AHRSensor.new(par[0]);
  ~senzorji.put(par[0], s);
});
// Starsi
~razmerja.do({ |par|
  if (par[1] != nil) {
    ~senzorji[par[0]].parent = ~senzorji[par[1]];
  };
});
/*
~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));

~decoder = SLIPDecoder.new(~receiverPath);
~w = Window.new("Utopia || C²", Rect(300, ~senzorji.size * 60, 900, ~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;
    ~startStopBtn.string_("Stop")
  }, {
    // Else stop the decoder
    ~decoder.stop;
    ~startStopBtn.string_("Start")
  };
  );
};

~startStopBtn = Button().string_("Start").action_(~startStopDecoder);
~ttyInput = TextField().string_(~receiverPath).keyDownAction_({ |input, char| if (char == $\r, { ~startStopDecoder.value; }); });
~btnRefAll = Button().string_("ref all").action_({
  ~senzorji.values.do({ | s | s.setRefQuat(); });
});

~w.layout_(
  GridLayout.rows(
    // HEADER; serial path, buttons
    [
      StaticText().string_("Serial path: "),
      [~ttyInput, columns: 7],
      ~startStopBtn,
      // Gumb za referencni quat na vseh
      ~btnRefAll
    ],
    // Sensor rows
    *~elementi.flatten
  )
);

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

// Automatic reflow
// does NOT work with layouts!
//~w.view.decorator = FlowLayout(~w.view.bounds);

// Pucamo stare listenerje
OSCdef.freeAll;

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

  // Quat listener
  q = OSCdef.new((\quat ++ s.id).asSymbol, { |msg, time, addr, recvPort|
    var q;
    // We get X Y Z W, but supercollider has W X Y Z!
    q = Quaternion.new(msg[1], msg[2], msg[3], msg[4]);

    // Count quat events
    Routine {
      var quatD;

      // Stetje dogodkov
      senzor.eps = senzor.eps + 1;

      // Nov quaternion!
      senzor.setQuat(q);
      senzor.refreshGuiQuat;

      // razlika od parenta
      //quatD = q;
      quatD = senzor.relQuat;

      senzor.updateEuler(quatD);
      //senzor.rotateVector(quatD);

      senzor.refreshGuiEuler;
      //senzor.refreshGuiVector;

      // Posljemo kot OSC, za lazje mapiranje v midi
      ~n.sendMsg("/euler/" ++ senzor.id, senzor.euler[0], senzor.euler[1], senzor.euler[2]);
      //~n.sendMsg("/vector/" ++ senzor.id, senzor.vecF.x, senzor.vecF.y, senzor.vecF.z, senzor.vecT.x, senzor.vecT.y, senzor.vecT.z);
    }.play(AppClock);

  }, oscHeader ++ "/quat");

  // Acceleration listener
  q = OSCdef.new((\acc ++ s.id).asSymbol, { |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;

      // Poslji sumo
      ~n.sendMsg("/acc/" ++ senzor.id, senzor.accel[0], senzor.accel[1], senzor.accel[2], senzor.accelSum);

      senzor.refreshGuiAccel;
    }.play(AppClock);
  }, oscHeader ++ "/acc");

  // Battery listener
  q = OSCdef.new((\bat ++ s.id).asSymbol, { |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;

/**********
 * SOUND! *
 *********/

~e = Env([1, 0.2, 0]);
s.boot;
{[SinOsc.ar(90), SinOsc.ar(122.3)]}.play;// * EnvGen.kr(~e, doneAction: Done.freeSelf)}.play;