using System.Numerics;
using System.Runtime.Intrinsics;
using OpenTK;
using OpenTK.Windowing.Desktop;
using Q01_Cálculos;

//  Convenio de signos:
//  X: Eje horizontal de la pantalla (izquierda a derecha positivo)
//  Z: Eje vertical de la pantalla (abajo a arriba positivo)
//  Y: Eje perpendicular a la pantalla (saliente positivo) 
 

// Definimos una palanca en punto neutro, utilizando las medidas que 
// hemos tomado para definir el volumen, la masa y el centro de gravedad
// de los componentes del ensamblaje que componen la palanca.

// La masa la estimaremos como el volumen del componente por la densidad
// de su material.

// Creamos una instancia de la clase Assembly para unir las piezas
// Para simplificar el programa, usamos técnicas de programación orientada a objetos (OOP)
// que nos permiten crear clases que describen los componentes del sistema, y las operaciones
// que se pueden realizar con ellos. Estas son "Assembly" y "Component".
Assembly Palanca = new() { Name = "Palanca de cambio" };

// Describimos físicamente los componentes, estimando su masa a partir del
// volumen y la densidad del material, y su centro de masas en base a sólidos
// tridimensionales sencillos:

// Perimero definimos las densidades de los materiales
const float
    DensAcero = 7800f,    // kg/m3
    DensAbs = 1110f;     // kg/m3 

// Aproximamos la parte superior del pomo como una esfera
float diametro_esfera = 4.5e-2f; //m
float VolPomo1 = 4 / 3 * MathF.PI * MathF.Pow(diametro_esfera / 2, 3);
Palanca.components.Add(new Component()
{
    Name = "Parte Superior Pomo",
    Mass = VolPomo1 * DensAbs,
    CenterOfMass = new(0, -diametro_esfera / 2, 30e-2f)
});
float LongitudPalanca = 30e-2f; //m

// Aproximamos la parte inferior del pomo como un cilindro hueco por donde
// pasarán la varilla y su cubierta
float diametro_cilindro = 2.5e-2f;          // m
float diametro_interno_cilindro = 1.5e-2f;  // m
float altura_cilindro = 1.5e-2f;            // m
float VolPomo2 = MathF.PI * (MathF.Pow(diametro_cilindro / 2, 2) - MathF.Pow(diametro_interno_cilindro / 2, 2)) * altura_cilindro;
Palanca.components.Add(new Component()
{
    Name = "Base Pomo",
    Mass = VolPomo2 * DensAbs,
    CenterOfMass = new(0, 0, LongitudPalanca - ((diametro_esfera / 2) - (altura_cilindro / 2)))
});

// Aproximamos la cubierta de la varilla como un cilindro hueco
float DiametroExterno2 = 1.5e-2f; //m
float DiametroInterno2 = 0.8e-2f; //m
float VolPalancaExterna = MathF.PI * (MathF.Pow(DiametroExterno2 / 2, 2) - MathF.Pow(DiametroInterno2 / 2, 2)) * LongitudPalanca;
Palanca.components.Add(new Component()
{
    Name = "Cubierta plástica de la varilla",
    Mass = VolPalancaExterna * DensAbs,
    CenterOfMass = new(0, 0, LongitudPalanca / 2)
});

// Aproximamos la varilla como un cilindro macizo
float DiametroExterno1 = 1.5e-2f; //m
float VolPalancaInterna = MathF.PI * MathF.Pow(DiametroExterno1 / 2, 2) * LongitudPalanca;
Palanca.components.Add(new Component()
{
    Name = "Varilla interna de la palanca",
    Mass = VolPalancaInterna * DensAcero,
    CenterOfMass = new(0, 0, LongitudPalanca / 2)
});

// Definimos las posicíones que hemos medido en el plano como tuplas (plano X Y)
(float, float) primera = (-6.5e-2f, -4.5e-2f);
(float, float) tercera = (0, -4.5e-2f);
(float, float) quinta = (4.5e-2f, -4.5e-2f);
(float, float) segunda = (-6.5e-2f, 10e-2f);
(float, float) cuarta = (0, 10e-2f);
(float, float) reversa = (4.5e-2f, 10e-2f);

Console.WriteLine("================================================");
Console.WriteLine("== Datos del sistema                          ==");
Console.WriteLine("================================================");

// Mostramos las propiedades físicas de la palanca por pantalla
Console.Write(Palanca.Summary());

// La fuerza que hace la palanca en reposo sobre la base es el peso de la palanca:
var fuerza_base = Palanca.Weight;
// Como está en reposo, no se trasalda, de modo que el sumatorio de fuerzas debe ser cero.
// Por ello, la reacción será el vector nulo menos la fuerza sobre la base (el peso de la palanca).
var reaccion_base = Vector3.Zero - fuerza_base;

// El momento sobre la base será el producto vectorial de la posición del centro de masas
// por el peso de la palanca.
var momento_base = Vector3.Cross(Palanca.CenterOfMass, Palanca.Weight);
// Y como tampoco rota, significa que el sumatorio de momentos se cancela.
// Por ello, el momento de reacción que hace la base sobre la palanca será el vector nulo menos
// el momento que hace la palanca sobre la base.
var momento_reaccion = Vector3.Zero - momento_base;

Console.WriteLine();
Console.WriteLine("===============================================================================");
Console.WriteLine("== Cálculos de fuerzas y momentos                   :  Escalares / <x,y,z>   ==");
Console.WriteLine("===============================================================================");
Console.WriteLine($"Fuerza que ejerce la barra sobre la base            : {fuerza_base} N");
Console.WriteLine($"Fuerza resistente que hace la base sobre la palanca : {reaccion_base} N");
Console.WriteLine($"Momento que hace la barra sobre la base             : {momento_base * 100} N·cm");
Console.WriteLine($"Momento que hace la base sobre la palanca           : {momento_reaccion * 100} N·cm");
Console.WriteLine();
Console.WriteLine("Momentos que hace la palanca cuando engrana una marcha:");
Console.WriteLine($"Centro de masa 1a : {CentroDeMasasRotado(Palanca, LongitudPalanca, primera)}");
Console.WriteLine($"Centro de masa 2a : {CentroDeMasasRotado(Palanca, LongitudPalanca, segunda)}");
Console.WriteLine($"Centro de masa 3a : {CentroDeMasasRotado(Palanca, LongitudPalanca, tercera)}");
Console.WriteLine($"Centro de masa 4a : {CentroDeMasasRotado(Palanca, LongitudPalanca, cuarta)}");
Console.WriteLine($"Centro de masa 5a : {CentroDeMasasRotado(Palanca, LongitudPalanca, quinta)}");
Console.WriteLine($"Centro de masa Reversa : {CentroDeMasasRotado(Palanca, LongitudPalanca, reversa)}");
Console.WriteLine();

//Simulation simulación = new Simulation(new GameWindowSettings() { UpdateFrequency = 20, RenderFrequency = 20 }, new NativeWindowSettings() { Size = new OpenTK.Mathematics.Vector2i(500, 500) });
//simulación.Run();


// Obtiene el vector de longitud determinada que corta los planos X e Y por 
// los puntos especificados
Vector3 Position(float X, float Y, float vectorLength)
{
    // Calculamos la longitud del segmento de la base de nuestro vector resultado como
    // la hipotenusa del triangulo de catetos X,Y (usando Pitágoras):
    float _base = MathF.Sqrt(MathF.Pow(X, 2) + MathF.Pow(Y, 2));
    // Calculamos la componente Z como el cateto restante del triangulo cuya hipotenusa
    // es el módulo de nuestro vector, con la base calculada en el paso anterior.
    float _z = MathF.Sqrt(MathF.Pow(vectorLength, 2) - MathF.Pow(_base, 2));
    // Construimos y devolvemos un vector con el valor Z hallado.
    return new Vector3(X, Y, _z);
}

// Obtiene el vector que representa el centro de masas de la palanca cuando se mueve en el plano XY
// hacia la posición de la marcha.
Vector3 CentroDeMasasRotado(IRigidBody componente, float longitud, (float, float) posicionesMarcha)
{
    Vector3 rcm_rotado;

    // Calculamos el vector de la posición de la palanca con la marcha metida.
    var posiciónDestino = Position(posicionesMarcha.Item1, posicionesMarcha.Item2, longitud);
    // Calculamos el eje de la rotación que se debe producir para que el vector de posición
    // realice el movimiento para engranar la marcha.
    // El eje de rotación lo podemos definir como el vector unitario perpendicular al vector rotado
    // respecto al vector original. Para ello podemos usar el producto vectorial (producto en cruz)
    // normalizado, para convertirlo en vector unitario.
    var ejeDeRotación = Vector3.Normalize(Vector3.Cross(componente.CenterOfMass, posiciónDestino));

    // El siguiente caso es determinar el ángulo que rotó el vector para orientarse a la posición
    // final. Para ello usamos una formula derivada de la definición de producto escalar.
    // El producti escalar se puede calcular como = a * b * el coseno del angulo que forman, de modo que:
    var ángulo = MathF.Acos(
        Vector3.Dot(componente.CenterOfMass, posiciónDestino) /
        (componente.CenterOfMass.Length() * posiciónDestino.Length()));

    // A continuación, crearemos un quaternión a partir del eje y el ángulo de rotación hallados:
    Quaternion rotación = Quaternion.CreateFromAxisAngle(ejeDeRotación, ángulo);

    // Y finalmente aplicamos la transformación vectorial del centro de masas para aplicar la misma
    // rotación que ha sufrido el vector que representa la orientación de la palanca de cambio.
    rcm_rotado = Vector3.Transform(componente.CenterOfMass, rotación);

    // Devolvemos el centro de masas rotado como resultado
    return rcm_rotado;
}