drone video, stack smash version

#include <stdio.h>
#include <stdint.h>

#include <sys/socket.h>
#include <sys/time.h>
#include <netinet/in.h>
#include <netdb.h>
#include <arpa/inet.h>
#include <fcntl.h>

#define DRONE_IP_ADDR "192.168.1.1"
#define DRONE_VID_STREAM_PORT "5555"

extern "C" {
#include <libavcodec/avcodec.h>
#include <libswscale/swscale.h>
#include <libavformat/avformat.h>
}

#define VIDEO_BUFFER_SIZE 40000
#define PaVE_HEADER_LENGTH 68

typedef struct { //PaVE
uint8_t signature[4]; /* "PaVE" - used to identify the start of
 frame */
uint8_t version; /* Version code */
uint8_t video_codec; /* Codec of the following frame */
uint16_t header_size; /* Size of the parrot_video_encapsulation_t */
uint32_t payload_size; /* Amount of data following this PaVE */
uint16_t encoded_stream_width; /* ex: 640 */
uint16_t encoded_stream_height; /* ex: 368 */
uint16_t display_width; /* ex: 640 */
uint16_t display_height; /* ex: 360 */
uint32_t frame_number; /* Frame position inside the current stream
 */
uint32_t timestamp; /* In milliseconds */
uint8_t total_chuncks; /* Number of UDP packets containing the
 current decodable payload - currently unused */
uint8_t chunck_index; /* Position of the packet - first chunk is #0
 - currenty unused*/
uint8_t frame_type; /* I-frame, P-frame -
 parrot_video_encapsulation_frametypes_t */
uint8_t control; /* Special commands like end
    -of-stream or
    advertised frames */
    uint32_t stream_byte_position_lw; /* Byte position of the current payload in
     the encoded stream - lower 32-bit word */
    uint32_t stream_byte_position_uw; /* Byte position of the current payload in
     the encoded stream - upper 32-bit word */
    uint16_t stream_id; /* This ID indentifies packets that should be
     recorded together */
    uint8_t total_slices; /* number of slices composing the current
     frame */
    uint8_t slice_index; /* position of the current slice in the frame
     */
    uint8_t header1_size; /* H.264 only : size of SPS inside payload -
     no SPS present if value is zero */
    uint8_t header2_size; /* H.264 only : size of PPS inside payload -
     no PPS present if value is zero */
    uint8_t reserved2[2]; /* Padding to align on 48 bytes */
    uint32_t advertised_size; /* Size of frames announced as advertised
     frames */
    uint8_t reserved3[12]; /* Padding to align on 64 bytes */
    //uint8_t reserved4[4]; // padding -- added b/c it was in the KIPR library code
}__attribute__ ((packed)) parrot_video_encapsulation_t;

typedef enum { //PaVE codec IDs
    CODEC_UNKNNOWN = 0,
    CODEC_VLIB,
    CODEC_P264,
    CODEC_MPEG4_VISUAL,
    CODEC_MPEG4_AVC
} parrot_video_encapsulation_codecs_t;

typedef enum { //PaVE frame types
    FRAME_TYPE_UNKNNOWN = 0, FRAME_TYPE_IDR_FRAME, /* headers followed by I-frame */
    FRAME_TYPE_I_FRAME, FRAME_TYPE_P_FRAME, FRAME_TYPE_HEADERS
} parrot_video_encapsulation_frametypes_t;

void printPaVE(parrot_video_encapsulation_t PaVE) {
    printf("\n---------------------------\n");

    printf("Codec : %s\n",
            (PaVE.video_codec == CODEC_MPEG4_VISUAL) ?
                    "MP4" :
                    ((PaVE.video_codec == CODEC_MPEG4_AVC) ? "H264" : "Unknown"));

    printf("StreamID : %d \n", PaVE.stream_id);
    printf("Timestamp : %d ms\n", PaVE.timestamp);
    printf("Encoded dims : %d x %d\n", PaVE.encoded_stream_width,
            PaVE.encoded_stream_height);
    printf("Display dims : %d x %d\n", PaVE.display_width, PaVE.display_height);
    ////printf ("Header size  : %d (PaVE size : %d)\n", PaVE.header_size, sizeof (parrot_video_encapsulation_t));
    printf("Header size : %d\n", PaVE.header_size);
    printf("Payload size : %d\n", PaVE.payload_size);
    printf("Size of SPS inside payload : %d\n", PaVE.header1_size);
    printf("Size of PPS inside payload : %d\n", PaVE.header2_size);
    printf("Slices in the frame : %d\n", PaVE.total_slices);
    printf("Frame Type / Number : %s : %d : slide %d/%d\n",
            (PaVE.frame_type == FRAME_TYPE_P_FRAME) ?
                    "P-Frame" :
                    ((PaVE.frame_type == FRAME_TYPE_I_FRAME) ?
                            "I-Frame" : "IDR-Frame"), PaVE.frame_number,
            PaVE.slice_index + 1, PaVE.total_slices);

    printf("---------------------------\n\n");
}

int receive(int socketNumber, unsigned char *buffer, int requestSize) {
    int lengthReceived = -1;
    //lengthReceived = recv(socketNumber,buffer,requestSize,MSG_WAITALL);
    lengthReceived = recv(socketNumber, buffer, requestSize, 0);
    if (lengthReceived < 0) {
        printf("failed to receive assumed PaVE packet\n");
    } else {
        printf("asked for %i bytes, received packet of %i bytes\n", requestSize,
                lengthReceived);
    }
    return lengthReceived;
}

void fetch_and_decode(int socketNumber,AVCodecContext *codecContext, AVFrame *picture) {
    parrot_video_encapsulation_t PaVE;
    unsigned char part[VIDEO_BUFFER_SIZE];
    int partLength = -1;
    partLength = receive(socketNumber, part, VIDEO_BUFFER_SIZE);
    if (partLength < 0) {
        printf("did not receive video data\n");
        return;
    }
    //memcpy(&PaVE, part, sizeof(parrot_video_encapsulation_t));
    memcpy(&PaVE, part, 76);
    if (strncmp((const char*) PaVE.signature, "PaVE", 4) != 0) {
        printf("PaVE not synchronized, skipping iteration\n");
        return;
    } else {
        printf("PaVE synchronized. YIPEEEEEEEEEEEEEEEEEEEEEEEE\n");
        printPaVE(PaVE);
    }

    uint32_t read = 0;
    unsigned char payload[VIDEO_BUFFER_SIZE];

    memcpy(payload, part + PaVE.header_size, partLength - PaVE.header_size);
    read += partLength - PaVE.header_size;

    double lastTime = clock();

    while (read < PaVE.payload_size && (clock() - lastTime) < 0.1) {
        int payloadLength = -1;
        printf("gathering payload...\n");
        payloadLength = receive(socketNumber, payload + read,
                PaVE.payload_size - read);
        read += payloadLength;
        lastTime = clock();
    }

    printf("payload complete, attempting to decode frame\n");

    AVPacket avPkt;
    av_init_packet(&avPkt);
    avPkt.data = NULL;
    avPkt.size = 0;

    avPkt.data = payload;
    avPkt.size = PaVE.payload_size;

    printf("avPkt.size = %d\n",avPkt.size);

    int done = 0;
    int ret = -1;
    ret = avcodec_decode_video2(codecContext, picture, &done, &avPkt);
    if (done == 0 || ret < 0) {
        printf("could not decode frame\n");
        return;
    }
    else {
        printf("\"done\" integer = %d\n",done);
        printf("avcodec_decode_video2 returned %d\n",ret);
    }

    avPkt.data = NULL;
    avPkt.size = 0;
    av_free_packet(&avPkt);
}

///////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////

int main() {

    printf("\n\n*********************** START ***********************\n\n");

    //printf("size of PaVE = %d\n",sizeof(parrot_video_encapsulation_t));

    int socketNumber;
    sockaddr_in myAddr;
    sockaddr_in droneAddr;

    // my sockaddr_in
    myAddr.sin_family = AF_INET;
    myAddr.sin_addr.s_addr = INADDR_ANY; // my IP address
    myAddr.sin_port = htons(atoi(DRONE_VID_STREAM_PORT));

    // the drone's sockaddr_in
    droneAddr.sin_family = AF_INET;
    droneAddr.sin_addr.s_addr = inet_addr(DRONE_IP_ADDR);
    droneAddr.sin_port = htons(atoi(DRONE_VID_STREAM_PORT));

    socketNumber = socket(AF_INET, SOCK_STREAM, 0);

    // bind the socket
    if (bind(socketNumber, (sockaddr*) &myAddr, sizeof(sockaddr_in)) < 0) {
        printf("failed to bind socket\n");
    }

    int result = -1;
    result = connect(socketNumber, (sockaddr*) &droneAddr, sizeof(sockaddr_in));
    if (result != 0) {
        printf("connection NOT established\n");
    }

    printf("booting...\n");
    // send one packet of "some bytes" to drone
    uint8_t gateKeyPkt[4] = { 0x01, 0x00, 0x00, 0x00 };
    int badCheck = -1;
    badCheck = sendto(socketNumber, (void*) gateKeyPkt, 4, 0,
            (sockaddr*) &droneAddr, sizeof(sockaddr_in));

    if (badCheck < 0) {
        printf("Failed to send basic packet\n");
    }

    AVCodec *codec = NULL;
    AVCodecContext *codecContext = NULL;
    AVFrame *picture = NULL;
    AVFrame *pictureBGR = NULL;
    SwsContext *convertContext = NULL;

    uint8_t *buffer = NULL;

    // set up codec
    avcodec_register_all();
    av_register_all();
    av_log_set_level(AV_LOG_DEBUG);
    codec = avcodec_find_decoder(CODEC_ID_H264);
    codec->id = CODEC_ID_H264;

    codecContext = avcodec_alloc_context3(codec);
    avcodec_open2(codecContext, codec, 0);
    codecContext->width = 640;
    codecContext->height = 360;
    codecContext->pix_fmt = PIX_FMT_YUV420P;
    codecContext->skip_frame = AVDISCARD_DEFAULT;
    codecContext->error_concealment = FF_EC_GUESS_MVS | FF_EC_DEBLOCK;
    codecContext->skip_loop_filter = AVDISCARD_DEFAULT;
    codecContext->workaround_bugs = FF_BUG_AUTODETECT;
    codecContext->debug = true;

    // allocate video frames and buffer
    picture = avcodec_alloc_frame();
    pictureBGR = avcodec_alloc_frame();
    buffer = (uint8_t*) av_mallocz(
            avpicture_get_size(PIX_FMT_BGR24, codecContext->width,
                    codecContext->height) * sizeof(uint8_t));

    // assign parts of buffer to image planes in BGR frame
    avpicture_fill((AVPicture*) pictureBGR, buffer, PIX_FMT_BGR24,
            codecContext->width, codecContext->height);

    // convert context
    convertContext = sws_getContext(codecContext->width, codecContext->height,
            codecContext->pix_fmt, codecContext->width, codecContext->height,
            PIX_FMT_BGR24, SWS_SPLINE, 0, 0, 0);

    for (int j = 0; j < 100; j++) {
        fetch_and_decode(socketNumber, codecContext, picture);
        printf("I escaped from the dreaded fetch_and_decode monster!\n");
    }

    avcodec_close(codecContext);
    av_free(codecContext);
    av_free(picture);
    av_free(pictureBGR);

    return 1;
}