#include <android/log.h>
#include "VN_JStreamer.h"

#define DEBUG_TAG "VN_JStreamer"

#define AV_OPT_SET(obj,name,val,opt)	(av_opt_set(obj,name,val,opt)==0)
#define AV_OPT_SET_INT(obj,name,val)	(av_opt_set_int(obj,name,val,0)==0)

using namespace videonext::media;

/* NAL unit types */
enum {
    NAL_SLICE=1,
    NAL_DPA,
    NAL_DPB,
    NAL_DPC,
    NAL_IDR_SLICE,
    NAL_SEI,
    NAL_SPS,
    NAL_PPS,
    NAL_AUD,
    NAL_END_SEQUENCE,
    NAL_END_STREAM,
    NAL_FILLER_DATA,
    NAL_SPS_EXT,
    NAL_AUXILIARY_SLICE=19
};

static void tune_encoder(AVCodecContext *ctx);

VN_JStreamer::VN_JStreamer()
    : av_picture_(0)
    , en_codec_(0), en_ctx_(0), out_buf_(0)
    , video_width_(0), video_height_(0)
{
}
  
VN_JStreamer::~VN_JStreamer()
{
   if (en_ctx_) 
   {
      avcodec_close(en_ctx_);
      av_free(en_ctx_);
   }
   
   if (av_picture_)
   {
       // Restore original pointers
       av_picture_->data[0] = av_picture_data_[0];
       av_picture_->data[1] = av_picture_data_[1];
       av_picture_->data[2] = av_picture_data_[2];

       avpicture_free((AVPicture*)av_picture_);
       av_free(av_picture_); 
   }

   if (out_buf_)
       free(out_buf_);
}


int VN_JStreamer::init(const std::string &rtsp_url, 
                       int video_width, int video_height)
{
    video_width_  = video_width;
    video_height_ = video_height;

    // Preparing ffmpeg
    avcodec_register_all();
	
    av_picture_ = avcodec_alloc_frame();
	
    //avpicture_fill((AVPicture *)avPictureYUV420, (uint8_t*)baseAddrYUV, PIX_FMT_YUV420P,
    //			   K_CORESURFACE_WIDTH, K_CORESURFACE_HEIGHT);
	
    avpicture_alloc((AVPicture *)av_picture_, 
                    PIX_FMT_YUV420P, 
                    video_width,
                    video_height);
  		
    // store original pointers. We will restore them in destructor
    av_picture_data_[0] = av_picture_->data[0];
    av_picture_data_[1] = av_picture_->data[1];
    av_picture_data_[2] = av_picture_->data[2];

    av_picture_->pts = 0;
		
    // init encoder
    en_codec_ = avcodec_find_encoder(CODEC_ID_H264);
    if (!en_codec_) 
    {
        __android_log_print(ANDROID_LOG_ERROR, DEBUG_TAG, "Codec not found");
        return -1;
    }
		
    en_ctx_ = avcodec_alloc_context3(en_codec_);		
//    avcodec_get_context_defaults2(en_ctx_, AVMEDIA_TYPE_VIDEO);
		
    tune_encoder(en_ctx_);
		
    en_ctx_->width        = video_width;  
    en_ctx_->height       = video_height;
    en_ctx_->pix_fmt      = PIX_FMT_YUV420P;
    en_ctx_->flags       |= CODEC_FLAG_GLOBAL_HEADER;

    if (avcodec_open2(en_ctx_, en_codec_, 0) < 0) 
    {
        __android_log_print(ANDROID_LOG_ERROR, DEBUG_TAG, "Could't open codec");
        return -2;
    }
	       
    out_buf_ = (uint8_t*)malloc(65536);
    if (!out_buf_) 
    {
        __android_log_print(ANDROID_LOG_ERROR, DEBUG_TAG, "malloc() failed");
        return -3;
    }
                
    // init streaming
    ResultStatus result;
    initRTSPStream(&result, rtsp_url);

    if (result.errorCode != 0)
    {
        __android_log_print(ANDROID_LOG_ERROR, DEBUG_TAG, "initStream() failed: %d (%s)", result.errorCode, result.errorString.c_str());
    }

    return result.errorCode;
}

int VN_JStreamer::process_video_frame(uint8_t *img, float gps_lat, float gps_long, float gps_alt)
{
    /*
      memcpy(avPictureYUV420->data[0], 
      //planes + planes->componentInfoY.offset, 
      __IOSurfaceGetBaseAddressOfPlane(surface, 0),
      avPictureYUV420->linesize[0] * __IOSurfaceGetHeight(surface));
    */
	
    av_picture_->data[0] = img;
	
    // Copying UV components
    unsigned i;
    unsigned size_of_plane  = (video_width_ / 2) * video_height_;
    unsigned char *ptr = img + (video_width_ * video_height_);
    unsigned pos[3] = {0};
    for (i = 0; i < size_of_plane; i++)
    {
        int idx = (i % 2) + 1;

        if (idx == 1)
            idx = 2;
        else
            idx = 1;

        av_picture_->data[idx][pos[idx]++] = ptr[i];
    }
	
    unsigned char *out_buf_ptr = out_buf_ + en_ctx_->extradata_size;
    unsigned size = avcodec_encode_video(en_ctx_, out_buf_ptr, 
                                         65536 - en_ctx_->extradata_size, 
                                         av_picture_);
    if (size <= 0 )
    {
        __android_log_print(ANDROID_LOG_ERROR, DEBUG_TAG, "Could't encode frame");
        //fprintf(stderr, "Extradata: %d, size: %d\n", en_ctx->extradata, en_ctx->extradata_size);
        return 0;
    }
	
    if (av_picture_->pts == 0)
    {
        // strip SEI NALU if exists
        if (out_buf_ptr[0] == 0x00 && out_buf_ptr[1] == 0x00 
            && out_buf_ptr[2] == 0x01 && out_buf_ptr[3] == NAL_SEI)
        {
            // Looking for NAL_IDR_SLICE
            //fprintf(stderr, "SEI found. Looking for IDR SLICE\n");

            unsigned char *idr_slice = 0;
            unsigned n = 0;
            do
            {        
                if (out_buf_ptr[n] == 0x00 && out_buf_ptr[n+1] == 0x00 && out_buf_ptr[n+2] == 0x00 
                    && out_buf_ptr[n+3] == 0x01  && (out_buf_ptr[n+4]&0x1F) == NAL_IDR_SLICE)
                {
                    idr_slice = &out_buf_ptr[n];
                    break;
                }
            }
            while (n++ < size - 4);
            
            if (idr_slice)
            {
                //fprintf(stderr, "IDR SLICE found. Size was: %d, new: %d\n", size, size - (idr_slice - out_buf_ptr));
                size = size - (idr_slice - out_buf_ptr);
                memmove(out_buf_ptr, idr_slice, size);
            }
        }       
	
    }
	
    if (out_buf_ptr[0] == 0x00 && out_buf_ptr[1] == 0x00 && out_buf_ptr[2] == 0x00 
        && out_buf_ptr[3] == 0x01  && (out_buf_ptr[4]&0x1F) == NAL_IDR_SLICE)
    {
        // Copying SPS/PPS to the front of frame
        memcpy(out_buf_, en_ctx_->extradata, en_ctx_->extradata_size);
        size += en_ctx_->extradata_size;
        out_buf_ptr = out_buf_;
    }
		
    av_picture_->pts++;
	
    // send frame
    videonext::media::DynaParams headers;	
    headers.insert(std::make_pair("Content-Type", "video/h264"));

    if (gps_lat && gps_long)
    {
        char hdr[64];
		
        snprintf(hdr, sizeof(hdr), "%f", gps_lat);
        headers.insert(std::make_pair("GEO_Latitude", hdr));
        
        snprintf(hdr, sizeof(hdr), "%f", gps_long);
        headers.insert(std::make_pair("GEO_Longitude", hdr));
        
        snprintf(hdr, sizeof(hdr), "%f", gps_alt);
        headers.insert(std::make_pair("GEO_Altitude", hdr));
    }
    ResultStatus result;
    postFrame(&result, out_buf_ptr, size, &headers);
    if (result.errorCode)
    {
        __android_log_print(ANDROID_LOG_ERROR, DEBUG_TAG, "Post error: %s\n", result.errorString.c_str());
        return -1;
    }    

    return 0;
}
 

///////////// JNI methods implementation ///////////////////
JNIEXPORT jlong JNICALL Java_com_videonext_MediaStreamer_nativeInit(JNIEnv *jenv, jobject jobj, jstring url, jint width, jint height)
{
    const char *url_buf;
    url_buf = jenv->GetStringUTFChars(url, NULL);
    if (url_buf == NULL) 
        return 0; /* OutOfMemoryError already thrown */

    VN_JStreamer *streamer = new VN_JStreamer();
    
    if (streamer->init(url_buf, width, height) != 0)
    {
        delete streamer;
        streamer = 0;
    }

    jenv->ReleaseStringUTFChars(url, url_buf);

    return (jlong)streamer;
}

JNIEXPORT void JNICALL Java_com_videonext_MediaStreamer_nativeDestroy(JNIEnv *jenv, jobject jobj, jlong id)
{
    if (!id)
        return;

    VN_JStreamer *streamer = (VN_JStreamer*)id;

    delete streamer;
}

JNIEXPORT jint JNICALL Java_com_videonext_MediaStreamer_nativeSendVideo(JNIEnv *jenv, jobject jobj, jlong id, jbyteArray img,
                                                                        jfloat gpsLat, jfloat gpsLong, jfloat gpsAlt)
{
    if (!id)
        return -1;

    VN_JStreamer *streamer = (VN_JStreamer*)id;

    jbyte *arr = jenv->GetByteArrayElements(img, 0);

//    int size = jenv->GetArrayLength(img);
//    __android_log_print(ANDROID_LOG_ERROR, DEBUG_TAG, "Size: %d", size);
    

    int ret = streamer->process_video_frame((uint8_t*)arr, gpsLat, gpsLong, gpsAlt);

    jenv->ReleaseByteArrayElements(img, arr, 0);

    return ret;
}


static void tune_encoder(AVCodecContext *ctx)
{
	ctx->bit_rate = 200000;
	ctx->bit_rate_tolerance = 400000; 
        ctx->time_base= (AVRational){1,15};
        ctx->gop_size = 30; /* emit one intra frame every 30 frames */
        ctx->max_b_frames=0;

        if (!AV_OPT_SET(ctx->priv_data, "profile", "baseline", 0))
        {
            __android_log_print(ANDROID_LOG_ERROR, DEBUG_TAG, "Failed to set H264 profile to 'baseline'");
        }
        
        /* Apply intra-refresh */
        if (!AV_OPT_SET_INT(ctx->priv_data, "intra-refresh", 0))
        {
            __android_log_print(ANDROID_LOG_ERROR, DEBUG_TAG, "Failed to set x264 intra-refresh");
        }
        
        /* Misc x264 settings (performance, quality, latency, etc).
         * Let's just use the x264 predefined preset & tune.
         */
        if (!AV_OPT_SET(ctx->priv_data, "preset", "veryfast", 0)) 
        {
            __android_log_print(ANDROID_LOG_ERROR, DEBUG_TAG, "Failed to set x264 preset 'veryfast'");
        }
        
        if (!AV_OPT_SET(ctx->priv_data, "tune", "animation+zerolatency", 0)) 
        {
            __android_log_print(ANDROID_LOG_ERROR, DEBUG_TAG, "Failed to set x264 tune 'zerolatency'");
        }
}

