Simulator/DroneSimulator/FormMain.cs

using System.Text;
using System.Numerics;
using System.Windows.Forms;
using static System.Net.Mime.MediaTypeNames;
using static System.Runtime.InteropServices.JavaScript.JSType;
using System.Security.Policy;
using System.Runtime.InteropServices;
using System.CodeDom;
using System.Linq;

namespace DroneSimulator
{
  public partial class Form_Main : Form
  {
    Screen2D screen2D = null;

    NetServerClients netServerClient = new NetServerClients();
    NetServerVisual netServerVisual = new NetServerVisual();

    List<Drone> AllDrones = new List<Drone>();

    public Form_Main()
    {
      InitializeComponent();

      RealMode.RealSimulation = checkBox_Mode_Real.Checked;
      numericUpDown_Acc_Update(null, null);
      numericUpDown_Gyr_Update(null, null);
      numericUpDown_Pos_Update(null, null);
      numericUpDown_Bar_Update(null, null);
      numericUpDown_Range_Update(null, null);
    }

    private void ClientConnectionCallback(object o)
    {
      NetServerClients.ConnectData data = (NetServerClients.ConnectData)o;

      Invoke((MethodInvoker)delegate
      {
        label_Clients_Num.Text = data.Count.ToString();
      });

      if (data.Connect)
      {
        Drone drone = new Drone(data.ID);
        drone.Create(1.0f, 0.5f, 1.0f);

        screen2D.CreateDrone(Color.Red, data.ID);

        AllDrones.Add(drone);
      }
      else
      {
        foreach (Drone drone in AllDrones)
        {
          if (drone.ID != data.ID) continue;
          drone.Close();

          screen2D.RemoveDrone(data.ID);

          AllDrones.Remove(drone);
          break;
        }
      }
    }

    private void ClientReceiveCallback(object o)
    {
      NetServerClients.ReceiveData data = (NetServerClients.ReceiveData)o;

      Drone? drone = null;

      foreach (Drone d in AllDrones)
      {
        if (d.ID != data.ID) continue;
        drone = d;
        break;
      }

      if (drone == null) return;

      List<byte[]?>? send = drone.DataStream(data.Buffer, data.Size);

      if (send == null) return;
      try
      {
        foreach (byte[]? b in send)
        {
          if (b != null) data.Client?.Send(b);
        }
      }
      catch { }
    }

    private void button_Client_Start_Click(object sender, EventArgs e)
    {
      var done = netServerClient.StartServer((int)numericUpDown_Clients_Port.Value, (int)numericUpDown_Clients_Limit.Value, ClientConnectionCallback, ClientReceiveCallback);
      switch (done)
      {
        case NetServerClients.ServerState.Error:
          {
            MessageBox.Show("Error to start clients server", "Error", MessageBoxButtons.OK, MessageBoxIcon.Error);
            break;
          }
        case NetServerClients.ServerState.Start:
          {
            button_Client_Start.Text = "Stop";
            button_Client_Start.BackColor = Color.LimeGreen;
            break;
          }
        case NetServerClients.ServerState.Stop:
          {
            label_Clients_Num.Text = "0";
            button_Client_Start.Text = "Start";
            button_Client_Start.BackColor = Color.Transparent;
            break;
          }
      }

      if (done != NetServerClients.ServerState.Start) return;

      pictureBox_2D.Image = null;

      screen2D = new Screen2D(DrawCallback);
    }

    private void exitToolStripMenuItem_Click(object sender, EventArgs e)
    {
      Close();
    }

    private void DrawCallback(Bitmap bmp)
    {
      Invoke((MethodInvoker)delegate
      {
        if (pictureBox_2D.Image == null) pictureBox_2D.Image = bmp;
        pictureBox_2D.Refresh();
      });
    }

    private void timer_Test_Tick(object sender, EventArgs e)
    {
      if (screen2D == null) return;

      foreach (Drone d in AllDrones)
      {
        screen2D.Move(d.ID, d.PosXYZ, d.GetOrientation());
      }

      screen2D.DrawScene();
    }
    private void Form_Main_FormClosing(object sender, FormClosingEventArgs e)
    {
      foreach (Drone d in AllDrones) d.Close();
    }

    private void VisualConnectionCallback(object o)
    {
      NetServerVisual.ConnectData data = (NetServerVisual.ConnectData)o;

      Invoke((MethodInvoker)delegate
      {
        label_Clients_Num.Text = data.Count.ToString();
      });

      if (data.Connect)
      {
        //---
      }
      else
      {
        //---
      }
    }

    private void VisualReceiveCallback(object o)
    {
      NetServerVisual.ReceiveData data = (NetServerVisual.ReceiveData)o;

      foreach (Drone d in AllDrones)
      {
        Drone.DataVisual v = d.GetVisual();

        try { data.Client.Send(Drone.getBytes(v)); }
        catch { }
      }
    }

    private void button_Visual_Start_Click(object sender, EventArgs e)
    {
      var done = netServerVisual.StartServer((int)numericUpDown_Visual_Port.Value, (int)numericUpDown_Visual_Limit.Value, VisualConnectionCallback, VisualReceiveCallback);
      switch (done)
      {
        case NetServerVisual.ServerState.Error:
          {
            MessageBox.Show("Error to start visual server", "Error", MessageBoxButtons.OK, MessageBoxIcon.Error);
            break;
          }
        case NetServerVisual.ServerState.Start:
          {
            button_Visual_Start.Text = "Stop";
            button_Visual_Start.BackColor = Color.LimeGreen;
            break;
          }
        case NetServerVisual.ServerState.Stop:
          {
            label_Visual_Num.Text = "0";
            button_Visual_Start.Text = "Start";
            button_Visual_Start.BackColor = Color.Transparent;
            break;
          }
      }
    }

    private void numericUpDown_Bar_Update(object sender, EventArgs e)
    {
      try { RealMode.Barometer.Pressure = uint.Parse(textBox_Bar_Pressure.Text); }
      catch
      {
        RealMode.Barometer.Pressure = 102258;
        MessageBox.Show("Pressure invalid format", "Barometer error", MessageBoxButtons.OK, MessageBoxIcon.Error);
      }


      RealMode.Barometer.Freq = (uint)numericUpDown_Bar_Freq.Value;
      RealMode.Barometer.Noise = (float)numericUpDown_Bar_Noise.Value;
      RealMode.Barometer.Lateness = (float)numericUpDown_Bar_Laten.Value;
      RealMode.Barometer.Enable = checkBox_Bar_Enable.Checked;
    }

    private void checkBox_Mode_Real_CheckedChanged(object sender, EventArgs e)
    {
      RealMode.RealSimulation = checkBox_Mode_Real.Checked;
    }

    private void numericUpDown_Acc_Update(object sender, EventArgs e)
    {
      RealMode.Accelerometer.Freq = (uint)numericUpDown_Acc_Freq.Value;
      RealMode.Accelerometer.Noise = (float)numericUpDown_Acc_Noise.Value;
      RealMode.Accelerometer.Lateness = (float)numericUpDown_Acc_Laten.Value;

      RealMode.Accelerometer.ScaleLeft = (float)numericUpDown_Acc_Scale_Left.Value;
      RealMode.Accelerometer.ScaleRight = (float)numericUpDown_Acc_Scale_Rigth.Value;
    }

    private void numericUpDown_Gyr_Update(object sender, EventArgs e)
    {
      RealMode.Gyroscope.Freq = (uint)numericUpDown_Gyr_Freq.Value;
      RealMode.Gyroscope.Noise = (float)numericUpDown_Gyr_Noise.Value;
      RealMode.Gyroscope.Lateness = (float)numericUpDown_Gyr_Laten.Value;

      RealMode.Gyroscope.Shift.X = (float)numericUpDown_Gyr_Shift_X.Value;
      RealMode.Gyroscope.Shift.Y = (float)numericUpDown_Gyr_Shift_Y.Value;
      RealMode.Gyroscope.Shift.Z = (float)numericUpDown_Gyr_Shift_Z.Value;
    }

    private void numericUpDown_Pos_Update(object sender, EventArgs e)
    {
      RealMode.Position.Freq = (uint)numericUpDown_Pos_Freq.Value;
      RealMode.Position.Noise = (float)numericUpDown_Pos_Noise.Value;
      RealMode.Position.Lateness = (float)numericUpDown_Pos_Laten.Value;
      RealMode.Position.Enable = checkBox_Pos_Enable.Checked;
    }

    private void numericUpDown_Range_Update(object sender, EventArgs e)
    {
      RealMode.Range.Freq = (uint)numericUpDown_Range_Freq.Value;
      RealMode.Range.Noise = (float)numericUpDown_Range_Noise.Value;
      RealMode.Range.Lateness = (float)numericUpDown_Range_Laten.Value;
      RealMode.Range.Enable = checkBox_Range_Enable.Checked;

      RealMode.Range.MaxHeight = (float)numericUpDown_Range_Max.Value;
    }
  }
}