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;

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

    NetServerClients netServerClient = new NetServerClients();

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

    public Form_Main()
    {
      InitializeComponent();
    }

    private void ConnectionCallback(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(0.5f, 10.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 ReceiveCallback(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;

      Drone.DataIn id = new Drone.DataIn();

      id.fromBytes(data.Buffer);

      drone.SetQadroPow(id.MotorUL, id.MotorUR, id.MotorDL, id.MotorDR);

      Drone.DataOut od = new Drone.DataOut();

      od.AccX= drone.Acc.X; od.AccY= drone.Acc.Y; od.AccZ= drone.Acc.Z;
      od.GyrX = drone.Gyr.X; od.GyrY = drone.Gyr.Y; od.GyrZ = drone.Gyr.Z;
      od.PosX= drone.PosXYZ.X; od.PosY = drone.PosXYZ.Y;
      od.LaserRange = drone.LaserRange;

      try { data.Client.Send(od.getBytes()); }
      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, ConnectionCallback, ReceiveCallback);
      switch (done)
      {
        case NetServerClients.ServerState.Error:
          {
            MessageBox.Show("Error to start server");
            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();
    }
  }
}