#include <unistd.h>
#include <fcntl.h>
#include <sys/stat.h>
#include <errno.h>
#include <string.h>
#include <stdlib.h>

#include <network.h>
#include <arpa/inet.h>

#include <pthread.h>

#include <sys/select.h>

#include <cyg/infra/diag.h>            // HAL polled output

//--------------------------------------------------------------------------

#define SHOW_RESULT( _fn, _res ) \
diag_printf("INFO: " #_fn "() returned %d %s\n", _res, _res<0?strerror(errno):"");

//--------------------------------------------------------------------------
// Thread stacks

char thread1_stack[PTHREAD_STACK_MIN*2];
char thread2_stack[PTHREAD_STACK_MIN*2];

//--------------------------------------------------------------------------
// Local variables

// Thread IDs
pthread_t thread1;
pthread_t thread2;

struct sockaddr_in sa;

//--------------------------------------------------------------------------

void *pthread_entry1(void *arg)
{
    int fd;
    struct sockaddr_in acc_addr;
    int addrsize;
    int err;

    int s;	/* TCP socket */
    
    diag_printf( "Thread1: calling socket()\n");        
    fd = socket( AF_INET, SOCK_STREAM, IPPROTO_TCP );
    if( fd < 0 ) SHOW_RESULT( socket, fd );

    diag_printf( "Thread1: calling bind()\n");
    err = bind( fd, (struct sockaddr *)&sa, sizeof(sa));
    if( err < 0 ) SHOW_RESULT( bind, err );    

    diag_printf( "Thread1: calling listen()\n");
    err = listen( fd, 3);
    if( err < 0 ) SHOW_RESULT( listen, err );    

    diag_printf( "Thread1: calling accept()\n");
    s = accept( fd, (struct sockaddr *)&acc_addr, &addrsize );
    if( s < 0 ) SHOW_RESULT( accept, s );    

    diag_printf( "Thread1: calling sleep(5)\n");
    sleep(5);

    {
        fd_set read_fds;
        struct timeval timeout;
        int net_ok;
        char buf[128];

        /* The "s" file descriptor is a connected TCP socket */

        FD_ZERO(&read_fds);
        FD_SET(s, &read_fds);
        timeout.tv_sec = timeout.tv_usec = 0;
        net_ok = true;
        while (net_ok &&
               (select(s+1, &read_fds, NULL, NULL, &timeout) > 0) &&
               FD_ISSET(s, &read_fds))
        {
            diag_printf("got data\n");
            net_ok = (read(s, buf, sizeof(buf)) > 0);

        }
        diag_printf("Finished flushing data\n");

    }

    while( 1 )
        sleep(1);
}

//==========================================================================

void *pthread_entry2(void *arg)
{
    int fd;
    int err;
    
    diag_printf( "Thread2: calling sleep(5)\n");
    sleep(5);
    
    diag_printf( "Thread2: calling socket()\n");
    fd = socket( AF_INET, SOCK_STREAM, IPPROTO_TCP );
    if( fd < 0 ) SHOW_RESULT( socket, fd );

    diag_printf( "Thread2: calling connect()\n");
    err = connect( fd, (struct sockaddr *)&sa, sizeof(sa));
    if( err < 0 ) SHOW_RESULT( connect, err );    

#if 1
    // Precharge the stream with some data:
    {       
        char buf[128];

        diag_printf( "Thread2: writing data to stream\n");

        err = write( fd, buf, sizeof(buf) );
        if( err != sizeof(buf) ) SHOW_RESULT( write, err );
        err = write( fd, buf, sizeof(buf) );
        if( err != sizeof(buf) ) SHOW_RESULT( write, err );
        err = write( fd, buf, sizeof(buf) );
        if( err != sizeof(buf) ) SHOW_RESULT( write, err );
        
    }
#endif
    
    while( 1 )
        sleep(1);
}


//==========================================================================
// main

int main( int argc, char **argv )
{
    void *retval;
    pthread_attr_t attr;
    struct sched_param schedparam;

    sa.sin_family = AF_INET;
    sa.sin_len = sizeof(sa);
    inet_aton("127.0.0.1", &sa.sin_addr);
    sa.sin_port = htons(1234);
    init_all_network_interfaces();
    
    // Create test threads

    {
        pthread_attr_init( &attr );

        schedparam.sched_priority = 5;
        pthread_attr_setinheritsched( &attr, PTHREAD_EXPLICIT_SCHED );
        pthread_attr_setschedpolicy( &attr, SCHED_RR );
        pthread_attr_setschedparam( &attr, &schedparam );
        pthread_attr_setstackaddr( &attr, (void *)&thread1_stack[sizeof(thread1_stack)] );
        pthread_attr_setstacksize( &attr, sizeof(thread1_stack) );

        pthread_create( &thread1,
                        &attr,
                        pthread_entry1,
                        (void *)0x12345671);
    }

    {
        pthread_attr_init( &attr );

        schedparam.sched_priority = 10;
        pthread_attr_setinheritsched( &attr, PTHREAD_EXPLICIT_SCHED );
        pthread_attr_setschedpolicy( &attr, SCHED_RR );
        pthread_attr_setschedparam( &attr, &schedparam );
        pthread_attr_setstackaddr( &attr, (void *)&thread2_stack[sizeof(thread2_stack)] );
        pthread_attr_setstacksize( &attr, sizeof(thread2_stack) );

        pthread_create( &thread2,
                        &attr,
                        pthread_entry2,
                        (void *)0x12345672);
    }
    
    // Now join with thread1
    diag_printf( "Main: calling pthread_join(thread1)\n");
    pthread_join( thread1, &retval );

    // And thread 2
    diag_printf( "Main: calling pthread_join(thread2)\n");
    pthread_join( thread2, &retval );

    diag_printf( "Main: exiting\n");
    
    return 0;
}