AHRSensor {
  var <id,
  <>calibrationQuat,
  <>quat,
  <>euler,
  <>accel,

  <battery,

  <gEx,
  <gEy,
  <gEz;

  *new { |id|
    ^super.newCopyArgs(id).init;
  }

  quat2euler { |quat|
    var w = quat.a, x = quat.b, y = quat.c, z = quat.d,

    x2 = x + x,  y2 = y + y,  z2 = z + z,
    xx = x * x2, xy = x * y2, xz = x * z2,
    yy = y * y2, yz = y * z2, zz = z * z2,
    wx = w * x2, wy = w * y2, wz = w * z2,

    matrix = [
        1 - ( yy + zz ), xy + wz,         xz - wy,         0,
        xy - wz,         1 - ( xx + zz ), yz + wx,         0,
        xz + wy,         yz - wx,         1 - ( xx + yy ), 0,
        0,               0,               0,               1
    ];

    var m11 = matrix[0], m12 = matrix[4], m13 = matrix[8];
    var m21 = matrix[1], m22 = matrix[5], m23 = matrix[9];
    var m31 = matrix[2], m32 = matrix[6], m33 = matrix[10];

    var euler = [0, 0, 0];

    euler[1] = asin(m13.clip(- 1, 1));

    if ((m13.abs < 0.9999999), {
        euler[0] = atan2(m23.neg, m33);
        euler[2] = atan2(m12.neg, m11);
    }, {
        euler[0] = atan2(m32, m22);
        euler[2] = 0;
    });

    euler.postln;
    euler;
  }

  init {
	quat = Quaternion;
	calibrationQuat = Quaternion;
	euler = [0, 0, 0];
	accel = [0, 0, 0];
	battery = 0;

	this.guiInit;
  }

  guiInit {
	// Barve
	var cRed, cGreen, cBlue, cPurple, cI;
	// Intenziteta
	cI = 0.8;

	cRed = Color.new(cI, 0, 0);
	cGreen = Color.new(0, cI, 0);
	cBlue = Color.new(0, 0, cI);
	cPurple = Color.new(cI, 0, cI);

	gEx = StaticText().string_(0).stringColor_(cRed);
	gEy = StaticText().string_(0).stringColor_(cGreen);
	gEz = StaticText().string_(0).stringColor_(cBlue);
  }

  getGui {
	^VLayout(
      StaticText().string_("Sensor " ++ id),
	  HLayout(
		StaticText().string_("euler: "),
		[StaticText().string_("x: "), align: \left],
		[gEx, align: \left],
		StaticText().string_("y: "),
		gEy,
		StaticText().string_("z: "),
		gEz,
	  )
	);
  }

  updateEuler { |newQuat|
    var quatDiff = newQuat / quat;
    euler += quat2euler(quatDiff);
  }

  refreshGui {
	gEx.string_(euler[0]);
	gEy.string_(euler[1]);
	gEz.string_(euler[2]);
  }
}