AHRSensor {
  var <id,
  <parent,

  // Kalibracijski quaternion (kao nula)
  <>calibQuat,

  <>quat,
  <>euler,
  <>eulerD,
  <>accel,
  <>accelSum,

  <>battery,
  <>eps,

  // GUI elementi
  // Quaternion
  <gQw,
  <gQx,
  <gQy,
  <gQz,
  // Kalibriraj quaternion
  <gQc,
  // Eulerjevi koti
  <gEx,
  <gEy,
  <gEz,
  // Relativni eulerjevi koti
  <gEdx,
  <gEdy,
  <gEdz,
  // Pospeskomer
  <gAx,
  <gAy,
  <gAz,
  <gAs,
  // Baterija
  <gB,
  <gBi,
  // Dogodkov na sekundo
  <gEps;

  *new { |id, parent = nil|
    ^super.newCopyArgs(id, parent).init;
  }

  quat2euler { |quat|
    // Quaternion
    //var w = quat.a, x = quat.b, y = quat.c, z = quat.d,
    //var q = [quat.b, quat.c, quat.d, quat.a],
    var q = [quat.a, quat.b, quat.c, quat.d],
    // Euler angles; roll, pitch and yaw
    e = [0, 0, 0];

    /**/
    e[0] = atan2( (2 * q[1] * q[2]) - (2 * q[0] * q[3]), (2 * q[0]*q[0]) + (2 * q[1] * q[1]) - 1); // psi
    e[1] = asin( (2 * q[1] * q[3]) + (2 * q[0] * q[2])).neg; // theta
    e[2] = atan2( (2 * q[2] * q[3]) - (2 * q[0] * q[1]), (2 * q[0] * q[0]) + (2 * q[3] * q[3]) - 1); // phi
    /**/

    /*
    // Variables
    sinr_cosp, cosr_cosp, sinp, cosp, siny_cosp, cosy_cosp;
    // Conversion source:
    // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Source_code_2

    sinr_cosp = 2 * (w * x * y * z);
    cosr_cosp = 1 - (2 * (x * x * y * y));
    // roll (x-axis rotation)
    e[0] = atan2(sinr_cosp, cosr_cosp);

    sinp = sqrt(1 + (2 * ((w * y) - (x * z))));
    cosp = sqrt(1 - (2 * ((w * y) - (x * z))));
    // pitch (y-axis rotation)
    e[1] = 2 * atan2(sinp, cosp) - 0.5pi;

    siny_cosp = 2 * ((w * z) + (x * y));
    cosy_cosp = 1 - (2 * ((y * y) + (z * z)));
    // yaw (z-axis rotation)
    e[2] = atan2(siny_cosp, cosy_cosp);
    */

    /*
    x2, y2, z2, xx, xy, xz, yy, yz, zz, wx, wy, wz,
    matrix, m11, m12, m13, m21, m22, m23, m31, m32, m33;

    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
    ];

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

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

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

    ^e;
  }

  init {
	quat = Quaternion;
	calibQuat = Quaternion.new(1, 0, 0, 0);
	euler = [0, 0, 0];
	accel = [0, 0, 0];
	battery = 0;
    eps = 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);

    // Razlika v eulerju
	gEdx = StaticText().string_(0).stringColor_(cRed);
	gEdy = StaticText().string_(0).stringColor_(cGreen);
	gEdz = StaticText().string_(0).stringColor_(cBlue);

	//gAx = StaticText().string_(0).stringColor_(cRed);
	//gAy = StaticText().string_(0).stringColor_(cGreen);
	//gAz = StaticText().string_(0).stringColor_(cBlue);
    gAx = LevelIndicator();
    gAy = LevelIndicator();
    gAz = LevelIndicator();
    gAs = LevelIndicator();

	gQw = StaticText().string_(0).stringColor_(cPurple);
	gQx = StaticText().string_(0).stringColor_(cRed);
	gQy = StaticText().string_(0).stringColor_(cGreen);
	gQz = StaticText().string_(0).stringColor_(cBlue);

	gQc = Button().string_("cal").action_({ |butt|
      this.calibrateQuat;
    });

	gB = StaticText().string_(0).stringColor_(cRed);
	gBi = LevelIndicator();
	gEps = StaticText().string_(0);
  }

  getGui {
    var napis = "Sensor " ++ id, elementi;
    if (parent != nil) { napis = napis ++ " (parent " ++ parent ++ ")"};

	elementi = [
      [
        StaticText().font_(Font("OpenSans", 12, true)).string_(napis),
        nil,
        [StaticText().string_("bat: "), align: \right],
        gB,
        gBi,
        [StaticText().string_("events/s: "), align: \right],
        gEps,
        gQc
      ],
	  [
		StaticText().string_("quaternion: "),
		[StaticText().string_("w: "), align: \right],
		gQw,
		[StaticText().string_("x: "), align: \right],
		gQx,
		[StaticText().string_("y: "), align: \right],
		gQy,
		[StaticText().string_("z: "), align: \right],
		gQz,
	  ],
      [ 
		StaticText().string_("euler: "),
		[StaticText().string_("x: "), align: \right],
		gEx,
		[StaticText().string_("y: "), align: \right],
		gEy,
		[StaticText().string_("z: "), align: \right],
		gEz,
	  ],
	  [ 
		StaticText().string_("accel: "),
		[StaticText().string_("x: "), align: \right],
		gAx,
		[StaticText().string_("y: "), align: \right],
		gAy,
		[StaticText().string_("z: "), align: \right],
		gAz,
		[StaticText().string_("sum: "), align: \right],
        gAs
	  ],
      if (parent != nil) {
        [ 
          StaticText().string_("euler d: "),
          [StaticText().string_("x: "), align: \right],
          gEdx,
          [StaticText().string_("y: "), align: \right],
          gEdy,
          [StaticText().string_("z: "), align: \right],
          gEdz,
       ];
      }
	];
    ^elementi;
  }

  calibrateQuat {
    calibQuat = quat;
  }

  updateEuler { |newQuat|
    euler = this.quat2euler(newQuat);
  }

  updateEulerD { |newQuat|
    eulerD = this.quat2euler(newQuat);
  }

  refreshGuiQuat {
    // Stevilo decimalk
    var prec = 3;
	gQw.string_(quat.a.asStringPrec(prec));
	gQx.string_(quat.b.asStringPrec(prec));
	gQy.string_(quat.c.asStringPrec(prec));
	gQz.string_(quat.d.asStringPrec(prec));
  }

  refreshGuiEuler {
    // Stevilo decimalk
    var prec = 3;
	gEx.string_(euler[0].asStringPrec(prec));
	gEy.string_(euler[1].asStringPrec(prec));
	gEz.string_(euler[2].asStringPrec(prec));
  }
  refreshGuiEulerD {
    // Stevilo decimalk
    var prec = 3;
	gEdx.string_(eulerD[0].asStringPrec(prec));
	gEdy.string_(eulerD[1].asStringPrec(prec));
	gEdz.string_(eulerD[2].asStringPrec(prec));
  }

  refreshGuiAccel {
    // Stevilo decimalk
    //var prec = 2;
	//gAx.string_(accel[0].asStringPrec(prec));
	//gAy.string_(accel[1].asStringPrec(prec));
	//gAz.string_(accel[2].asStringPrec(prec));
    var from = -50, to = 50;
    [gAx, gAy, gAz].do({|el, i|
      el.value_(accel[i].linlin(from, to, 0, 1));
    });
    gAs.value_(accelSum.linlin(0, 100, 0, 1));
  }

  refreshGuiBat {
    // Stevilo decimalk
    var prec = 2;
	gB.string_(battery.asStringPrec(prec));
    gBi.value_(battery.linlin(3.6, 4.2, 0, 1));
  }

  refreshGuiEps {
    // Stevilo decimalk
    var prec = 2;
	gEps.string_(eps);
  }

  refreshGui {
    this.refreshGuiQuat;
    this.refreshGuiEuler;
    this.refreshGuiAccel;
    this.refreshGuiBat;
  }
}