#!/usr/bin/perl -w
 
use strict;
use IO::Socket;
use File::Slurp;
use threads;

my $socket=new IO::Socket::INET->new(PeerPort=>12345,Proto=>'udp',PeerAddr=>'localhost');
print "This is a test Sending Server\n";

my $capxml = read_file("C:\\Users\\deadpickle\\Desktop\\UAS\\GRRUVI_1.60\\test xml\\capabilities.xml");

my $recv = threads->new(\&recvxml);
my $send = threads->new(\&sendxml);

while(1){
}

sub sendxml{
    my $socket=new IO::Socket::INET->new(PeerPort=>12345,Proto=>'udp',PeerAddr=>'localhost');
    while (1) {
        #send capabilities every 10 seconds
        $socket->send($capxml);
        print "Sending Capabilities\n";
        sleep 10;
    }
}

sub recvxml{
    my $xml;
    my $socket=new IO::Socket::INET->new(PeerPort=>56789,Proto=>'udp',PeerAddr=>'localhost');
    while(1) {
        print "Waiting for meassage\n";
        $socket->recv($xml,3000);
        print "Recieved Message: $xml\n";
    }
}

##</code><code>##

sub sendxml {
    $SIG{PIPE} = "IGNORE";
    $\="\n";

    ##variables
    my $subscribe;
    my $count = 29;
    my $waypoints;
    my $path;
    my $xmlcoords;
    
    #my $send_client = IO::Socket::INET->new( PeerAddr => $sendnode, Proto => 'tcp') or $error = 4;
    my $send_client = new IO::Socket::INET->new(LocalPort=>56789,Proto=>'udp') or $error = 3;  #for testing
            
    if ($send_client){
        print "$subflag $planflag $verify";
                    
        if ($subflag == 3) {
            $astream = 'Telemetry'; #for testing
            $subscribe = <<XML;
<?xml version="1.0" encoding="UTF-8"?>
<StreamSubscribe xsi:schemaLocation="http://StreamSubscribe.CUIntegration.com stream_subscription.xsd" xmlns="http://StreamSubscribe.CUIntegration.com" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance">
	<VehicleID>$vid</VehicleID>
	<System>
		<ID>Autopilot</ID>
		<Stream>$astream</Stream>
		<Action>Subscribe</Action>
	</System>
</StreamSubscribe>
XML
            print "Subscribe: $subscribe\n";
            #$send_client->send($subscribe);
            $send_client->send($subscribe,0,'localhost:56789');   #for testing
            $subflag = 4;
        }
                
        if ($verify == 4) {
            my @handlecoords = split(/\n/,$coords);
                        
            foreach my $kid(@handlecoords) {
                $count++;
                print "$kid\n";
                my @split = split(/,/,$kid);
                #$xmlcoords.="\n" if $count > 31;
                $xmlcoords.="   <Waypoint Latitude=\"$split[0]\" Longitude=\"$split[1]\" SequenceNumber=\"$count\"/>\n";
            }
                        
            $path = 'Loiter' if $count == 30;
            $path = 'Path' if $count >= 31;

            my $waypoints = <<XML;
<?xml version="1.0" encoding="UTF-8"?>
<Task xmlns="http://Task.CUIntegration.com" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:schemaLocation="http://Task.CUIntegration.com Task.xsd ">
  <ID>ID</ID>
  <Type>$path</Type>
  <Destination>$vid</Destination>
  $xmlcoords
</Task>
XML
            print "$waypoints\n";
            $send_client->send($waypoints);
            $count_waypoint = $count_waypoint + 0.1;
            $verify = 0;
        }
                    
        if ($planflag == 2) {
            $astream = 'Flight Plan';   #for testing
            $subscribe = <<XML;
<?xml version="1.0" encoding="UTF-8"?>
<StreamSubscribe xsi:schemaLocation="http://StreamSubscribe.CUIntegration.com stream_subscription.xsd" xmlns="http://StreamSubscribe.CUIntegration.com" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance">
	<VehicleID>$vid</VehicleID>
	<System>
		<ID>Autopilot</ID>
		<Stream>$astream</Stream>
		<Action>Subscribe</Action>
	</System>
</StreamSubscribe>
XML
            print "Subscribe: $subscribe\n";
            #$send_client->send($subscribe);
            $send_client->send($subscribe,0,'localhost:56789');   #for testing
            $planflag = 3;
            #exit;
        }
    }
    undef $send_client;
}