//
// Created by 29019 on 2019/5/4.
//

#include "rtp.h"
#include "librtsp.h"

int Pack264(unsigned char *frame,int size, Vector **outbuf){
	int i = 0;
    static short seq = 0;
    if((0 == frame) || (0 == outbuf)){
        return 0;
    }
    // fill rtsp interleaved header
    RtspInterleavedFrameHeader rtspInterleavedFrameHeader;
    rtspInterleavedFrameHeader.magic = 0x24;
    rtspInterleavedFrameHeader.channel = 0;
    //fill rtp header
    CRtpHeader header;
    header.version = 2;
    header.cc = 0;
    header.m = 0;
    header.p = 0;
    header.pt = 96;
    header.ts = htonl(1201233060);
    header.x = 0;
    header.ssrc = htonl(0x055aaee0);
    if((frame[0] != 0x00)&&(frame[1] != 0x00)&&(frame[2] != 0x00) && (frame[3] != 0x01)){
        return -1;
    }
    int packCnt = (size - 5) /MAX_RTP_LEN;
	printf("%d package pack\r\n",packCnt);
    if(packCnt == 0){
        header.seq = htons(seq++);
        outbuf[0]->data = (unsigned char *)malloc(sizeof(FuIdentifier) + sizeof(CRtpHeader) + sizeof(rtspInterleavedFrameHeader)
                                            + size - 5);
        outbuf[0]->length =  size - 5 + sizeof(FuIdentifier) + sizeof(CRtpHeader) + sizeof(rtspInterleavedFrameHeader);
        FuIdentifier identifier;
        identifier.FUType = frame[4]&0x1f; //sps or pps
        identifier.NRI = (frame[4]&0xe0)>>5;
        identifier.F = 0;
        rtspInterleavedFrameHeader.length = htons(size - 5 + sizeof(FuIdentifier) + sizeof(CRtpHeader) );

        int index = 0;
        memcpy(outbuf[0]->data,&rtspInterleavedFrameHeader,sizeof(rtspInterleavedFrameHeader));
        index += sizeof(rtspInterleavedFrameHeader);
        memcpy(outbuf[0]->data + index,&header,sizeof(header));
        index += sizeof(header);
        memcpy(outbuf[0]->data + index,&identifier,sizeof(FuIdentifier));
        index += sizeof(identifier);
        memcpy(outbuf[0]->data + index,frame + 5,size - 5);
        return 1;  // return one package
    }else{

        if(packCnt % MAX_RTP_LEN != 0){
            packCnt +=1;
        }
        CH264NalUnit nalUnit;
        nalUnit.Identifier.FUType = 28;  // fu-a
        nalUnit.Identifier.NRI = (frame[4]&0xe0)>>5;
        nalUnit.Identifier.F = 0;
        nalUnit.Header.NALUnitType = (frame[4]&0x1f); // idr picture or non-idr picture
        nalUnit.Header.Forbid = 0;
        for(i = 0;i < packCnt;i++) {
            header.seq = htons(seq++);
            if(i == packCnt - 1) {
                if(packCnt % MAX_RTP_LEN == 0){
                   (*outbuf + i)->data = (unsigned char *)malloc(sizeof(CH264NalUnit) + sizeof(CRtpHeader) + sizeof(rtspInterleavedFrameHeader)
                    + MAX_RTP_LEN);
                    (*outbuf + i)->length = MAX_RTP_LEN + sizeof(CH264NalUnit) + sizeof(CRtpHeader) + sizeof(rtspInterleavedFrameHeader);

                    rtspInterleavedFrameHeader.length = htons(MAX_RTP_LEN + sizeof(CH264NalUnit) + sizeof(CRtpHeader));
                }else{
                    (*outbuf + i)->data = (unsigned char *)malloc(sizeof(CH264NalUnit) + sizeof(CRtpHeader) + sizeof(rtspInterleavedFrameHeader)
                    + (size - 5)%MAX_RTP_LEN);
                    (*outbuf + i)->length = (size - 5)%MAX_RTP_LEN + sizeof(CH264NalUnit) + sizeof(CRtpHeader) + sizeof(rtspInterleavedFrameHeader);
                    rtspInterleavedFrameHeader.length = htons((size - 5)%MAX_RTP_LEN + sizeof(CH264NalUnit) + sizeof(CRtpHeader));
                }
                nalUnit.Header.End = 1;
                nalUnit.Header.Start = 0;

                int index = 0;
                memcpy((*outbuf + i)->data,&rtspInterleavedFrameHeader,sizeof(rtspInterleavedFrameHeader));
                index += sizeof(rtspInterleavedFrameHeader);
                memcpy((*outbuf + i)->data + index,&header,sizeof(header));
                index += sizeof(header);
                memcpy((*outbuf + i)->data + index,&nalUnit,sizeof(nalUnit));
                index += sizeof(nalUnit);
                memcpy((*outbuf + i)->data + index,frame + 5 + i*MAX_RTP_LEN,(size - 5)%MAX_RTP_LEN);
            }else if(i == 0){
                nalUnit.Header.End = 0;
                nalUnit.Header.Start = 1;
                (*outbuf + i)->data = (unsigned char *)malloc(sizeof(FuIdentifier) + sizeof(CRtpHeader) + sizeof(rtspInterleavedFrameHeader)
                                                    + MAX_RTP_LEN);
                (*outbuf + i)->length = MAX_RTP_LEN + sizeof(CH264NalUnit) + sizeof(CRtpHeader) + sizeof(rtspInterleavedFrameHeader);
                rtspInterleavedFrameHeader.length = htons(MAX_RTP_LEN + sizeof(CH264NalUnit) + sizeof(CRtpHeader));
                int index = 0;
                memcpy((*outbuf + i)->data,&rtspInterleavedFrameHeader,sizeof(rtspInterleavedFrameHeader));
                index += sizeof(rtspInterleavedFrameHeader);
                memcpy((*outbuf + i)->data + index,&header,sizeof(header));
                index += sizeof(header);
                memcpy((*outbuf + i)->data + index,&nalUnit,sizeof(nalUnit));
                index += sizeof(nalUnit);
                memcpy((*outbuf + i)->data + index,frame + 5 + i*MAX_RTP_LEN,MAX_RTP_LEN);
            }else{
                (*outbuf + i)->data = (unsigned char *)malloc(sizeof(FuIdentifier) + sizeof(CRtpHeader) + sizeof(rtspInterleavedFrameHeader)
                                                    + MAX_RTP_LEN);
                (*outbuf + i)->length = MAX_RTP_LEN  + sizeof(CH264NalUnit) + sizeof(CRtpHeader) + sizeof(rtspInterleavedFrameHeader);

                nalUnit.Header.End = 0;
                nalUnit.Header.Start = 0;
                rtspInterleavedFrameHeader.length = htons(MAX_RTP_LEN + sizeof(CH264NalUnit) + sizeof(CRtpHeader));
                int index = 0;
                memcpy((*outbuf + i)->data,&rtspInterleavedFrameHeader,sizeof(rtspInterleavedFrameHeader));
                index += sizeof(rtspInterleavedFrameHeader);
                memcpy((*outbuf + i)->data + index,&header,sizeof(header));
                index += sizeof(header);
                memcpy((*outbuf + i)->data + index,&nalUnit,sizeof(nalUnit));
                index += sizeof(nalUnit);
                memcpy((*outbuf + i)->data + index,frame + 5 + i*MAX_RTP_LEN,MAX_RTP_LEN);
            }
        }
        return packCnt;
    }
}


int Pack264AndSend(unsigned char *frame,int size,RtspClient * p){
	int i = 0;
    static short seq = 0;
	char *sendBuf;
	int sendLength = 0;
	int length = MAX_RTP_LEN + sizeof(CH264NalUnit) + sizeof(CRtpHeader) 
		+ sizeof(RtspInterleavedFrameHeader);
    if(0 == frame || NULL == p) {
        return 0;
    }
	sendBuf = malloc(MAX_RTP_LEN + sizeof(CH264NalUnit) + sizeof(CRtpHeader) 
		+ sizeof(RtspInterleavedFrameHeader));
    // fill rtsp interleaved header
    RtspInterleavedFrameHeader rtspInterleavedFrameHeader;
    rtspInterleavedFrameHeader.magic = 0x24;
    rtspInterleavedFrameHeader.channel = 0;
    //fill rtp header
    CRtpHeader header;
    header.version = 2;
    header.cc = 0;
    header.m = 0;
    header.p = 0;
    header.pt = 96;
    header.ts = htonl(1201233060);
    header.x = 0;
    header.ssrc = htonl(0x055aaee0);
    if((frame[0] != 0x00)&&(frame[1] != 0x00)&&(frame[2] != 0x00) && (frame[3] != 0x01)){
        return -1;
    }
    int packCnt = (size - 5) /MAX_RTP_LEN;
	printf("%d package pack\r\n",packCnt);
    if(packCnt == 0) {
        header.seq = htons(seq++);
        sendLength =  size - 5 + sizeof(FuIdentifier) + sizeof(CRtpHeader) + sizeof(rtspInterleavedFrameHeader);
        FuIdentifier identifier;
        identifier.FUType = frame[4]&0x1f; //sps or pps
        identifier.NRI = (frame[4]&0xe0)>>5;
        identifier.F = 0;
        int index = 0;
        memcpy(sendBuf,&rtspInterleavedFrameHeader,sizeof(rtspInterleavedFrameHeader));
        index += sizeof(rtspInterleavedFrameHeader);
        memcpy(sendBuf + index,&header,sizeof(header));
        index += sizeof(header);
        memcpy(sendBuf + index,&identifier,sizeof(FuIdentifier));
        index += sizeof(identifier);
        memcpy(sendBuf + index,frame + 5,size - 5);
		SendRtpPackage(p,sendBuf,sendLength);		
        return 1;  // return one package
    }else{
        if(packCnt % MAX_RTP_LEN != 0){
            packCnt +=1;
        }
        CH264NalUnit nalUnit;
        nalUnit.Identifier.FUType = 28;  // fu-a
        nalUnit.Identifier.NRI = (frame[4]&0xe0)>>5;
        nalUnit.Identifier.F = 0;
        nalUnit.Header.NALUnitType = (frame[4]&0x1f); // idr picture or non-idr picture
        nalUnit.Header.Forbid = 0;
        for(i = 0;i < packCnt;i++) {
            header.seq = htons(seq++);
            if(i == packCnt - 1) {
	                if(packCnt % MAX_RTP_LEN == 0){
	                    sendLength = MAX_RTP_LEN + sizeof(CH264NalUnit) + sizeof(CRtpHeader) + sizeof(RtspInterleavedFrameHeader);
	                    rtspInterleavedFrameHeader.length = htons(MAX_RTP_LEN + sizeof(CH264NalUnit) + sizeof(CRtpHeader));
	                }else{
	                    sendLength = (size - 5)%MAX_RTP_LEN + sizeof(CH264NalUnit) + sizeof(CRtpHeader) + sizeof(RtspInterleavedFrameHeader);
	                    rtspInterleavedFrameHeader.length = htons((size - 5)%MAX_RTP_LEN + sizeof(CH264NalUnit) + sizeof(CRtpHeader));
	                }
	                nalUnit.Header.End = 1;
	                nalUnit.Header.Start = 0;

	                int index = 0;
	                memcpy(sendBuf,&rtspInterleavedFrameHeader,sizeof(rtspInterleavedFrameHeader));
	                index += sizeof(rtspInterleavedFrameHeader);
	                memcpy(sendBuf + index,&header,sizeof(header));
	                index += sizeof(header);
	                memcpy(sendBuf + index,&nalUnit,sizeof(nalUnit));
	                index += sizeof(nalUnit);
	                memcpy(sendBuf + index,frame + 5 + i*MAX_RTP_LEN,(size - 5)%MAX_RTP_LEN);
					SendRtpPackage(p,sendBuf,sendLength);
            	}else if(i == 0) {
	                nalUnit.Header.End = 0;
	                nalUnit.Header.Start = 1;
	                sendLength = MAX_RTP_LEN + sizeof(CH264NalUnit) + 
						sizeof(CRtpHeader) + sizeof(rtspInterleavedFrameHeader);
	                rtspInterleavedFrameHeader.length = htons(MAX_RTP_LEN + sizeof(CH264NalUnit) + sizeof(CRtpHeader));
	                int index = 0;
	                memcpy(sendBuf,&rtspInterleavedFrameHeader,sizeof(rtspInterleavedFrameHeader));
	                index += sizeof(rtspInterleavedFrameHeader);
	                memcpy(sendBuf + index,&header,sizeof(header));
	                index += sizeof(header);
	                memcpy(sendBuf + index,&nalUnit,sizeof(nalUnit));
	                index += sizeof(nalUnit);
	                memcpy(sendBuf + index,frame + 5 + i*MAX_RTP_LEN,MAX_RTP_LEN);
					SendRtpPackage(p,sendBuf,sendLength);
            }else{
                sendLength = MAX_RTP_LEN  + sizeof(CH264NalUnit) + sizeof(CRtpHeader) + sizeof(rtspInterleavedFrameHeader);
                nalUnit.Header.End = 0;
                nalUnit.Header.Start = 0;
                rtspInterleavedFrameHeader.length = htons(MAX_RTP_LEN + sizeof(CH264NalUnit) + sizeof(CRtpHeader));
                int index = 0;
                memcpy(sendBuf,&rtspInterleavedFrameHeader,sizeof(rtspInterleavedFrameHeader));
                index += sizeof(rtspInterleavedFrameHeader);
                memcpy(sendBuf + index,&header,sizeof(header));
                index += sizeof(header);
                memcpy(sendBuf + index,&nalUnit,sizeof(nalUnit));
                index += sizeof(nalUnit);
                memcpy(sendBuf + index,frame + 5 + i*MAX_RTP_LEN,MAX_RTP_LEN);
				SendRtpPackage(p,sendBuf,sendLength);
            }
        }
		free(sendBuf);
        return packCnt;
		
    }
}