within BondGraph;

package _3D
  connector BondPort "Bond graph 3D multibond power port"
    Real e[3] "Effort vector";
    flow Real f[3] "Flow vector";
    annotation(
      Icon(graphics = {Rectangle(lineColor = {170, 0, 0}, fillColor = {170, 0, 0}, fillPattern = FillPattern.Solid, extent = {{-60, 60}, {60, -60}})}));
  end BondPort;

  model J1 "Bond graph 3D 1-junction (common flow, efforts sum to zero)"
    parameter Integer N(min = 1) = 2 "# of power ports";
    parameter Real s[N] = fill(1.0, N) "Bond orientation signs used in the effort balance";
    BondPort P[N] "Power ports" annotation(
      Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(extent = {{-10, -10}, {10, 10}})));
    Real f[3];
  equation
// Efforts sum to zero, with signs from bond directions
    for j in 1:3 loop
      sum(s[i]*P[i].e[j] for i in 1:N) = 0;
    end for;
// Flows are all equal
    for i in 2:N loop
      P[i].f = P[i - 1].f;
    end for;
    f = P[1].f;
    annotation(
      Icon(graphics = {Text(extent = {{-100, 100}, {100, -100}}, textString = "1", textStyle = {TextStyle.Bold, TextStyle.UnderLine, TextStyle.Italic}), Text(origin = {50, 80}, textColor = {0, 0, 255}, extent = {{-50, 20}, {50, -20}}, textString = "%name", textStyle = {TextStyle.Italic})}));
  end J1;

  model J0 "Bond graph 3D 0-junction (common effort, flows sum to zero)"
    parameter Integer N(min = 1) = 2 "# of power ports";
    parameter Real s[N] = fill(1.0, N) "Bond orientation signs used in the effort balance";
    BondPort P[N] "Power ports" annotation(
      Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(extent = {{-10, -10}, {10, 10}})));
    Real e[3];
  equation
// Flows sum to zero, with signs from bond directions
    for j in 1:3 loop
      sum(s[i]*P[i].f[j] for i in 1:N) = 0;
    end for;
// Efforts are all equal
    for i in 2:N loop
      P[i].e = P[1].e;
    end for;
    e = P[1].e;
    annotation(
      Icon(graphics = {Text(extent = {{-100, 100}, {100, -100}}, textString = "0", textStyle = {TextStyle.Bold, TextStyle.UnderLine, TextStyle.Italic}), Text(origin = {50, 80}, textColor = {0, 0, 255}, extent = {{-50, 20}, {50, -20}}, textString = "%name", textStyle = {TextStyle.Italic})}),
      Diagram(graphics));
  end J0;

  partial model OnePortPassive "One-port passive 3D multibond element"
    BondPort p "Generic power port" annotation(
      Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(extent = {{-10, -10}, {10, 10}})));
  end OnePortPassive;

  partial model OnePortEnergetic "One-port 3D multibond storage element"
    extends OnePortPassive annotation(
      Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {0, 80}, extent = {{-10, -10}, {10, 10}})));
    Real state[3] "Conserved quantity";
  end OnePortEnergetic;

  model C "Bond graph 3D C element"
    extends OnePortEnergetic(state(start = q0, each fixed = true));
    parameter Real c[3, 3] = [1, 0, 0; 0, 1, 0; 0, 0, 1] "Capacitance matrix inverse denominator form";
    parameter Real q0[3] = {0, 0, 0} "Initial stored quantity (charge)";
  equation
    der(state) = p.f;
    c*p.e = state;
    annotation(
      Icon(graphics = {Text(extent = {{-100, 100}, {100, -100}}, textString = "C", textStyle = {TextStyle.Bold, TextStyle.UnderLine, TextStyle.Italic}), Text(origin = {50, 80}, textColor = {0, 0, 255}, extent = {{-50, 20}, {50, -20}}, textString = "%name", textStyle = {TextStyle.Italic})}),
      Diagram(graphics));
  end C;

  model I "Bond graph 3D I element"
    extends OnePortEnergetic(state(start = p0, each fixed = true));
    parameter Real I[3, 3] = [1, 0, 0; 0, 1, 0; 0, 0, 1] "Inertance / inductance / mass matrix";
    parameter Real p0[3] = {0, 0, 0} "Initial stored quantity (momentum / flux)";
  equation
    der(state) = p.e;
    I*p.f = state;
    annotation(
      Icon(graphics = {Text(extent = {{-100, 100}, {100, -100}}, textString = "I", textStyle = {TextStyle.Bold, TextStyle.UnderLine, TextStyle.Italic}), Text(origin = {50, 80}, textColor = {0, 0, 255}, extent = {{-50, 20}, {50, -20}}, textString = "%name", textStyle = {TextStyle.Italic})}),
      Diagram(graphics));
  end I;

  model R "Bond graph 3D resistor"
    extends OnePortPassive;
    parameter Real R[3, 3] = [1, 0, 0; 0, 1, 0; 0, 0, 1] "Resistance matrix";
  equation
    p.e = R*p.f;
    annotation(
      Icon(graphics = {Text(extent = {{-100, 100}, {100, -100}}, textString = "R", textStyle = {TextStyle.Bold, TextStyle.UnderLine, TextStyle.Italic}), Text(origin = {50, 80}, textColor = {0, 0, 255}, extent = {{-50, 20}, {50, -20}}, textString = "%name", textStyle = {TextStyle.Italic})}),
      Diagram(graphics));
  end R;

  model Se "Effort source"
    BondPort p annotation(
      Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {80, 0}, extent = {{-10, -10}, {10, 10}})));
    parameter Real e0[3] "Imposed effort";
  equation
    p.e = e0;
    annotation(
      Icon(graphics = {Text(origin = {-20, 0}, extent = {{-80, 100}, {80, -100}}, textString = "Se", textStyle = {TextStyle.Bold, TextStyle.UnderLine, TextStyle.Italic}), Text(origin = {50, 80}, textColor = {0, 0, 255}, extent = {{-50, 20}, {50, -20}}, textString = "%name", textStyle = {TextStyle.Italic})}),
      Diagram(graphics));
  end Se;

  model Sf "Flow source"
    BondPort p annotation(
      Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {80, 0}, extent = {{-10, -10}, {10, 10}})));
    parameter Real f0[3] "Imposed flow";
  equation
    p.f = f0;
    annotation(
      Icon(graphics = {Text(origin = {-20, 0}, extent = {{-80, 100}, {80, -100}}, textString = "Sf", textStyle = {TextStyle.Bold, TextStyle.UnderLine, TextStyle.Italic}), Text(origin = {50, 80}, textColor = {0, 0, 255}, extent = {{-50, 20}, {50, -20}}, textString = "%name", textStyle = {TextStyle.Italic})}),
      Diagram(graphics));
  end Sf;

  model TF "Bond graph 3D transformer"
    BondPort p1 "Port 1" annotation(
      Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {-80, 0}, extent = {{-10, -10}, {10, 10}})));
    BondPort p2 "Port 2" annotation(
      Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {80, 0}, extent = {{-10, -10}, {10, 10}})));
    parameter Real n[3, 3] = [1, 0, 0; 0, 1, 0; 0, 0, 1] "Transformer ratio matrix";
  equation
    p1.e = n*p2.e;
    transpose(n)*p1.f = p2.f;
    annotation(
      Icon(graphics = {Text(extent = {{-80, 100}, {80, -100}}, textString = "TF", textStyle = {TextStyle.Bold, TextStyle.UnderLine, TextStyle.Italic}), Text(origin = {50, 80}, textColor = {0, 0, 255}, extent = {{-50, 20}, {50, -20}}, textString = "%name", textStyle = {TextStyle.Italic})}),
      Diagram(graphics));
  end TF;

  model GY "Bond graph 3D gyrator"
    BondPort p1 "Port 1" annotation(
      Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {-80, 0}, extent = {{-10, -10}, {10, 10}})));
    BondPort p2 "Port 2" annotation(
      Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {80, 0}, extent = {{-10, -10}, {10, 10}})));
    parameter Real r[3, 3] = [1, 0, 0; 0, 1, 0; 0, 0, 1] "Gyrator modulus matrix";
  equation
    p1.e = r*p2.f;
    p2.e = transpose(r)*p1.f;
    annotation(
      Icon(graphics = {Text(extent = {{-80, 100}, {80, -100}}, textString = "GY", textStyle = {TextStyle.Bold, TextStyle.UnderLine, TextStyle.Italic}), Text(origin = {50, 80}, textColor = {0, 0, 255}, extent = {{-50, 20}, {50, -20}}, textString = "%name", textStyle = {TextStyle.Italic})}),
      Diagram(graphics));
  end GY;

  model mSe "Bond graph modulated effort source"
    BondPort p annotation(
      Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {80, 0}, extent = {{-10, -10}, {10, 10}})));
    Modelica.Blocks.Interfaces.RealInput e0 "Imposed effort" annotation(
      Placement(transformation(origin = {-8, -64}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {-30, -78}, extent = {{-8, -8}, {8, 8}}, rotation = 90)));
    Modelica.Blocks.Interfaces.RealInput e1 "Imposed effort" annotation(
      Placement(transformation(origin = {-8, -64}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {0, -78}, extent = {{-8, -8}, {8, 8}}, rotation = 90)));
    Modelica.Blocks.Interfaces.RealInput e2 "Imposed effort" annotation(
      Placement(transformation(origin = {-8, -64}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {30, -78}, extent = {{-8, -8}, {8, 8}}, rotation = 90)));
  equation
    p.e = {e0, e1, e2};
    annotation(
      Icon(graphics = {Text(origin = {-20, 0}, extent = {{-80, 100}, {80, -100}}, textString = "mSe", textStyle = {TextStyle.Bold, TextStyle.UnderLine, TextStyle.Italic}), Text(origin = {50, 80}, textColor = {0, 0, 255}, extent = {{-50, 20}, {50, -20}}, textString = "%name", textStyle = {TextStyle.Italic})}),
      Diagram(graphics));
  end mSe;

  model mSf "Bond graph modulated flow source"
    BondPort p annotation(
      Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {80, 0}, extent = {{-10, -10}, {10, 10}})));
    Modelica.Blocks.Interfaces.RealInput f0 "Imposed flow" annotation(
      Placement(transformation(origin = {-8, -64}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {-30, -78}, extent = {{-8, -8}, {8, 8}}, rotation = 90)));
    Modelica.Blocks.Interfaces.RealInput f1 "Imposed flow" annotation(
      Placement(transformation(origin = {-8, -64}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {0, -78}, extent = {{-8, -8}, {8, 8}}, rotation = 90)));
    Modelica.Blocks.Interfaces.RealInput f2 "Imposed flow" annotation(
      Placement(transformation(origin = {-8, -64}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {30, -78}, extent = {{-8, -8}, {8, 8}}, rotation = 90)));
  equation
    p.f = {f0, f1, f2};
    annotation(
      Icon(graphics = {Text(origin = {-20, 0}, extent = {{-80, 100}, {80, -100}}, textString = "mSf", textStyle = {TextStyle.Bold, TextStyle.UnderLine, TextStyle.Italic}), Text(origin = {50, 80}, textColor = {0, 0, 255}, extent = {{-50, 20}, {50, -20}}, textString = "%name", textStyle = {TextStyle.Italic})}),
      Diagram(graphics));
  end mSf;

  model mTF "Bond graph modulated transformer"
    BondPort p1 "Port 1" annotation(
      Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {-80, 0}, extent = {{-10, -10}, {10, 10}})));
    BondPort p2 "Port 2" annotation(
      Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {80, 0}, extent = {{-10, -10}, {10, 10}})));
    Modelica.Blocks.Interfaces.RealInput m[3, 3] "Modulation" annotation(
      Placement(transformation(origin = {-8, -64}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {0, -78}, extent = {{-8, -8}, {8, 8}}, rotation = 90)));
  equation
    p1.e = m*p2.e;
    transpose(m)*p1.f = p2.f;
    annotation(
      Diagram(graphics),
      Icon(graphics = {Text(extent = {{-60, 100}, {60, -100}}, textString = "mTF", textStyle = {TextStyle.Bold, TextStyle.UnderLine, TextStyle.Italic}), Text(origin = {50, 80}, textColor = {0, 0, 255}, extent = {{-50, 20}, {50, -20}}, textString = "%name", textStyle = {TextStyle.Italic})}));
  end mTF;

  model mGY "Bond graph modulated gyrator"
    BondPort p1 "Port 1" annotation(
      Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {-80, 0}, extent = {{-10, -10}, {10, 10}})));
    BondPort p2 "Port 2" annotation(
      Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {80, 0}, extent = {{-10, -10}, {10, 10}})));
    Modelica.Blocks.Interfaces.RealInput m[3, 3] "Modulation" annotation(
      Placement(transformation(origin = {-8, -64}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {0, -78}, extent = {{-8, -8}, {8, 8}}, rotation = 90)));
  equation
    p1.e = m*p2.f;
    p2.e = transpose(m)*p1.f;
    annotation(
      Diagram(graphics),
      Icon(graphics = {Text(extent = {{-60, 100}, {60, -100}}, textString = "mGY", textStyle = {TextStyle.Bold, TextStyle.UnderLine, TextStyle.Italic}), Text(origin = {50, 80}, textColor = {0, 0, 255}, extent = {{-50, 20}, {50, -20}}, textString = "%name", textStyle = {TextStyle.Italic})}));
  end mGY;

  package TransRotUtils
    model mTFrot3lin
      BondPort pR annotation(
        Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {-80, 0}, extent = {{-10, -10}, {10, 10}})));
      BondPort pT annotation(
        Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {80, 0}, extent = {{-10, -10}, {10, 10}})));
      parameter Real r_body[3] = {1, 0, 0} "Vector from rotational reference to translational point (body frame)";
    protected
      Real S[3, 3];
    equation
// Skew matrix such that S*x = r_body x x
      S = [0, -r_body[3], r_body[2]; r_body[3], 0, -r_body[1]; -r_body[2], r_body[1], 0];
// v = w x r_body = -S*w
      pT.f = transpose(S)*pR.f;
// tau = r_body x F = S*F
      pR.e = S*pT.e;
      annotation(
        Icon(graphics = {Text(extent = {{-70, 100}, {70, -100}}, textString = "rlTF", textStyle = {TextStyle.Bold, TextStyle.UnderLine, TextStyle.Italic})}));
    end mTFrot3lin;

    model rTF3D
      BondPort p1 "Port 1" annotation(
        Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {-80, 0}, extent = {{-10, -10}, {10, 10}})));
      BondPort p2 "Port 2" annotation(
        Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {80, 0}, extent = {{-10, -10}, {10, 10}})));
      Modelica.Blocks.Interfaces.RealInput phi "roll angle" annotation(
        Placement(transformation(origin = {-8, -64}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {-30, -78}, extent = {{-8, -8}, {8, 8}}, rotation = 90)));
      Modelica.Blocks.Interfaces.RealInput theta "pitch angle" annotation(
        Placement(transformation(origin = {-8, -64}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {0, -78}, extent = {{-8, -8}, {8, 8}}, rotation = 90)));
      Modelica.Blocks.Interfaces.RealInput psi "yaw angle" annotation(
        Placement(transformation(origin = {-8, -64}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {30, -78}, extent = {{-8, -8}, {8, 8}}, rotation = 90)));
    protected
      Real R[3, 3];
    equation
// ZYX rotation matrix from body to inertial frame.
      R = [cos(psi)*cos(theta), cos(psi)*sin(theta)*sin(phi) - sin(psi)*cos(phi), cos(psi)*sin(theta)*cos(phi) + sin(psi)*sin(phi); sin(psi)*cos(theta), sin(psi)*sin(theta)*sin(phi) + cos(psi)*cos(phi), sin(psi)*sin(theta)*cos(phi) - cos(psi)*sin(phi); -sin(theta), cos(theta)*sin(phi), cos(theta)*cos(phi)];
      p1.e = R*p2.e;
      transpose(R)*p1.f = p2.f;
      annotation(
        Diagram(graphics),
        Icon(graphics = {Text(extent = {{-60, 100}, {60, -100}}, textString = "rTF", textStyle = {TextStyle.Bold, TextStyle.Italic, TextStyle.UnderLine})}));
    end rTF3D;
  end TransRotUtils;

  model fsensor3d
    BondPort p annotation(
      Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {-52, 0}, extent = {{-10, -10}, {10, 10}})));
    Modelica.Blocks.Interfaces.RealOutput f0 "Flow output" annotation(
      Placement(transformation(origin = {-8, -64}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {54, 26}, extent = {{-8, -8}, {8, 8}})));
    Modelica.Blocks.Interfaces.RealOutput f1 "Flow output" annotation(
      Placement(transformation(origin = {-8, -64}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {54, 0}, extent = {{-8, -8}, {8, 8}})));
    Modelica.Blocks.Interfaces.RealOutput f2 "Flow output" annotation(
      Placement(transformation(origin = {-8, -64}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {54, -26}, extent = {{-8, -8}, {8, 8}})));
  equation
// Ideal flow sensor in bond-graph form: zero effort loading.
    p.e = {0, 0, 0};
    f0 = p.f[1];
    f1 = p.f[2];
    f2 = p.f[3];
    annotation(
      Icon(graphics = {Text(origin = {-10, 0}, extent = {{-50, 60}, {50, -60}}, textString = "f", textStyle = {TextStyle.Italic}), Ellipse(origin = {-2, 0}, lineThickness = 5, extent = {{-50, 50}, {50, -50}})}));
  end fsensor3d;
  annotation(
    Icon(graphics = {Text(origin = {50, 0}, extent = {{-50, 100}, {50, -100}}, textString = "R", textStyle = {TextStyle.Bold, TextStyle.Italic}), Line(origin = {-45.22, 20.19}, points = {{-58.7774, -20.1934}, {21.2226, -20.1934}, {-38.7774, 19.8066}}, thickness = 5), Line(origin = {-9.81, -8.19}, points = {{-10.1934, 48.1934}, {-10.1934, -31.8066}}, thickness = 5), Line(origin = {-78, 16}, points = {{-26, 0}, {30, 0}}, thickness = 5), Line(origin = {-78, -16}, points = {{-26, 0}, {30, 0}}, thickness = 5)}),
    uses(Modelica(version = "4.1.0")),
    Diagram(graphics));
end _3D;