/*
 * Copyright (c) 2004-2007 Marcus Overhagen <marcus@overhagen.de>
 *
 * Permission is hereby granted, free of charge, to any person
 * obtaining a copy of this software and associated documentation
 * files (the "Software"), to deal in the Software without restriction, 
 * including without limitation the rights to use, copy, modify, 
 * merge, publish, distribute, sublicense, and/or sell copies of 
 * the Software, and to permit persons to whom the Software is 
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be 
 * included in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES 
 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 
 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, 
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR 
 * OTHER DEALINGS IN THE SOFTWARE.
 */

#include <stdio.h>
#include <fcntl.h>
#include <unistd.h>
#include <stdlib.h>
#include <string.h>

#include "TransportStreamDemux.h"
#include "Packet.h"
#include "PacketQueue.h"

#define TRACE_TS_DEMUX
#ifdef TRACE_TS_DEMUX
  #define TRACE printf
#else
  #define TRACE(a...)
#endif

#define CLOCK_TO_USEC(clck)             (bigtime_t((clck) + 13) / 27)
#define USEC_TO_CLOCK(usec)             (bigtime_t((usec) * 27))


TransportStreamDemux::TransportStreamDemux(
        PacketQueue *vid_queue, PacketQueue *aud_queue, 
        PacketQueue *vid_queue2, PacketQueue *aud_queue2,
        PacketQueue *mpeg_ts_queue)
 :      fCount(0)
 ,      fSystemTime(0)
 ,      fPerformanceTime(0)
 ,      fLastEndTime(0)
 ,      fVidPid(-1)
 ,      fAudPid(-1)
 ,      fPcrPid(-1)
 ,      fVidQueue(vid_queue)
 ,      fVidQueue2(vid_queue2)
 ,      fAudQueue(aud_queue)
 ,      fAudQueue2(aud_queue2)
 ,      fMpegTsQueue(mpeg_ts_queue)
 ,      fVidPacket(new Packet)
 ,      fAudPacket(new Packet)
 ,      fVidPacketValid(false)
 ,      fAudPacketValid(false)
{
}


TransportStreamDemux::~TransportStreamDemux()
{
        delete fVidPacket;
        delete fAudPacket;
}


void
TransportStreamDemux::SetPIDs(int vid_pid, int aud_pid, int pcr_pid)
{
        fVidPacket->MakeEmpty();
        fAudPacket->MakeEmpty();
        fVidPacketValid = false;
        fAudPacketValid = false;
        fVidPid = vid_pid; 
        fAudPid = aud_pid;
        fPcrPid = pcr_pid;
}


void
TransportStreamDemux::ProcessPacket(const mpeg_ts_packet *pkt, bigtime_t start_time)
{
        fCount++;
        
//      printf("ProcessPacket %lld\n", fCount);
        
        if (pkt->SyncByte() != 0x47) {
//              fCountInvalid++;
                printf("packet %lld sync byte error "
                           "%02x %02x %02x %02x %02x %02x %02x %02x "
                           "%02x %02x %02x %02x %02x %02x %02x %02x\n", fCount,
                pkt->header[0], pkt->header[1], pkt->header[2], pkt->header[3],
                pkt->data[0], pkt->data[1], pkt->data[2], pkt->data[3],
                pkt->data[4], pkt->data[5], pkt->data[6], pkt->data[7],
                pkt->data[8], pkt->data[9], pkt->data[10], pkt->data[11]);
                return;
        }

//      if (pkt->TransportError()) {
//              fCountInvalid++;
//              printf("invalid packet %lld (transport_error_indicator)\n", fCount);
//              return;
//      }

    int pid = pkt->PID();
    
    if (pid == 0) {
        ProcessPAT(pkt);
        return;
    }

    if (pid == fPcrPid) {
        ProcessPCR(pkt, start_time);
    }

    if (pid == fAudPid) {
        ProcessAUD(pkt);
    }
    
    if (pid == fVidPid) {
        ProcessVID(pkt);
    }
}


void
TransportStreamDemux::ProcessPAT(const mpeg_ts_packet *pkt)
{
}


void
TransportStreamDemux::ProcessPCR(const mpeg_ts_packet *pkt, bigtime_t start_time)
{
        if (!(pkt->AdaptationFieldControl() & 0x2)) // adaptation field present?
                return;
        if (pkt->data[0] < 7) // long enough?
                return;
        if (!(pkt->data[1] & 0x10)) // PCR present?
                return;
        int64 pcr;
        pcr = (uint64(pkt->data[2]) << 25) 
                | (uint32(pkt->data[3]) << 17)
                | (uint32(pkt->data[4]) << 9)
                | (uint32(pkt->data[5]) << 1)
                | (pkt->data[6] >> 7);
        pcr *= 300;
        pcr += (pkt->data[6] & 1) | pkt->data[7];
//      printf("    pcr = %lld\n", pcr);

//      bigtime_t now = system_time();
//      printf("system_time %.3f, PCR-time %.3f, delta %.06f, PCR %lld\n", now / 1e6, CLOCK_TO_USEC(pcr) / 1e6, (CLOCK_TO_USEC(pcr) - now) / 1e6, pcr);

//      atomic_set64(&fCurrentTime, CLOCK_TO_USEC(pcr));

        fTimeSourceLocker.Lock();
        fSystemTime = start_time;
        fPerformanceTime = CLOCK_TO_USEC(pcr);
        fTimeSourceLocker.Unlock();
}


void
TransportStreamDemux::ProcessAUD(const mpeg_ts_packet *pkt)
{
        // flush old packet
        if (pkt->PayloadUnitStart()) {
                if (fAudPacketValid) {
                        Packet *clone = new Packet(*fAudPacket);
                        status_t err = fAudQueue->Insert(fAudPacket);
                        if (err != B_OK) {
                                delete fAudPacket;
                                if (err == B_WOULD_BLOCK) {
                                        printf("fAudQueue->Insert failed (would block)\n");
                                }
                        }
                        err = fAudQueue2->Insert(clone);
                        if (err != B_OK) {
                                delete clone;
                                if (err == B_WOULD_BLOCK) {
                                        printf("fAudQueue2->Insert failed (would block)\n");
                                }
                        }
                        fAudPacket = new Packet;
                } else {
                        fAudPacket->MakeEmpty();
                        fAudPacketValid = true;
                }
        }
        
        int skip;
        switch (pkt->AdaptationFieldControl()) {
                case 0: // illegal
                        skip = 184;
                        break;
                case 1: // payload only
                        skip = 0;
                        break;
                case 2: // adaptation field only
                        skip = 184;
                        break;
                case 3: // adaptation field followed by payload
                        skip = pkt->data[0] + 1;
                        if (skip > 184)
                                skip = 184;
                        break;
                default:
                        skip = 0; // stupid compiler, impossible case
        }
        
        const uint8 *data = pkt->data + skip;
        int size = 184 - skip;
        
//      if (skip != 0)
//              printf("aud skip %d\n", skip);

        if (pkt->PayloadUnitStart()) {

                if (pkt->TransportError()) {
                        printf("error in audio packet %02x %02x %02x %02x\n", data[0], data[1], data[2], data[3]);
                        fAudPacketValid = false;
                        return;
                }

                if (data[0] || data[1] || data[2] != 0x01 || data[3] <= 0xbf || data[3] >= 0xf0) {
                        printf("invalid audio packet %02x %02x %02x %02x\n", data[0], data[1], data[2], data[3]);
                        fAudPacketValid = false;
                        return;
                }
                
                if (data[7] & 0x80) { // PTS
                        int64 pts;
                        int64 dts;
                
                        pts = (uint64((data[9] >> 1) & 0x7) << 30)
                                | (data[10] << 22) | ((data[11] >> 1) << 15)
                                | (data[12] << 7) | (data[13] >> 1);
                        pts *= 300;
                       
//                  printf("vid pts = %lld\n", pts);

                        if (data[7] & 0x40) { // DTS
                
                                dts = (uint64((data[14] >> 1) & 0x7) << 30)
                                        | (data[15] << 22) | ((data[16] >> 1) << 15)
                                        | (data[17] << 7) | (data[18] >> 1);
                                dts *= 300;

//                              printf("aud dts = %lld\n", dts);
                        } else {
                                dts = pts;
                        }
                        
                        fAudPacket->SetTimeStamp(CLOCK_TO_USEC(dts));
                }


//              printf("aud %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x\n",
//              data[0], data[1], data[2], data[3], data[4],
//              data[5], data[6], data[7], data[8], data[9]);
        }

        fAudPacket->AddData(data, size);
}


void
TransportStreamDemux::ProcessVID(const mpeg_ts_packet *pkt)
{
        // flush old packet
//      if (pkt->PayloadUnitStart() || pkt->TransportError()) {
        if (pkt->PayloadUnitStart()) {
                if (fVidPacketValid) {
                        Packet *clone = new Packet(*fVidPacket);
                        status_t err = fVidQueue->Insert(fVidPacket);
                        if (err != B_OK) {
                                delete fVidPacket;
                                if (err == B_WOULD_BLOCK) {
                                        printf("fVidQueue->Insert failed (would block)\n");
                                }
                        }
                        err = fVidQueue2->Insert(clone);
                        if (err != B_OK) {
                                delete clone;
                                if (err == B_WOULD_BLOCK) {
                                        printf("fVidQueue2->Insert failed (would block)\n");
                                }
                        }
                        fVidPacket = new Packet;
                } else {
                        fVidPacket->MakeEmpty();
                        fVidPacketValid = true;
                }
        }
        
//      if (pkt->TransportError()) {
//              printf("transport error\n");
//              fVidPacketValid = false;
//              return;
//      }
        
        int skip;
        switch (pkt->AdaptationFieldControl()) {
                case 0: // illegal
                        skip = 184;
                        break;
                case 1: // payload only
                        skip = 0;
                        break;
                case 2: // adaptation field only
                        skip = 184;
                        break;
                case 3: // adaptation field followed by payload
                        skip = pkt->data[0] + 1;
                        if (skip > 184)
                                skip = 184;
                        break;
                default:
                        skip = 0; // stupid compiler, impossible case
        }
        
        const uint8 *data = pkt->data + skip;
        int size = 184 - skip;
        
//      if (skip != 0)
//              printf("vid skip %d\n", skip);

        if (pkt->PayloadUnitStart()) {

                if (pkt->TransportError()) {
                        printf("error in video packet %02x %02x %02x %02x\n", data[0], data[1], data[2], data[3]);
                        fVidPacketValid = false;
                        return;
                }
                
                if (data[0] || data[1] || data[2] != 0x01 || data[3] <= 0xbf || data[3] >= 0xf0) {
                        printf("invalid video packet %02x %02x %02x %02x\n", data[0], data[1], data[2], data[3]);
                        fVidPacketValid = false;
                        return;
                }
                
                if (data[7] & 0x80) { // PTS
                        int64 pts;
                        int64 dts;
                
                        pts = (uint64((data[9] >> 1) & 0x7) << 30)
                                | (data[10] << 22) | ((data[11] >> 1) << 15)
                                | (data[12] << 7) | (data[13] >> 1);
                        pts *= 300;
                       
//                  printf("vid pts = %lld\n", pts);

                        if (data[7] & 0x40) { // DTS
                
                                dts = (uint64((data[14] >> 1) & 0x7) << 30)
                                        | (data[15] << 22) | ((data[16] >> 1) << 15)
                                        | (data[17] << 7) | (data[18] >> 1);
                                dts *= 300;

//                              printf("vid dts = %lld\n", dts);
//                              printf("dts = %lld, pts = %lld, delta %lld ### dts = %lld, pts = %lld, delta %lld\n", 
//                                              dts, pts, pts - dts, CLOCK_TO_USEC(dts), CLOCK_TO_USEC(pts), CLOCK_TO_USEC(pts - dts));
                        } else {
                                dts = pts;
                        }

//                      printf("clocks: dts = %14Ld, pts = %14Ld, delta %8Ld ### usec: dts = %14Ld, pts = %14Ld, delta %8Ld\n", 
//                                      dts, pts, pts - dts, CLOCK_TO_USEC(dts), CLOCK_TO_USEC(pts), CLOCK_TO_USEC(pts - dts));
                        
                        fVidPacket->SetTimeStamp(CLOCK_TO_USEC(dts));
                }
                
//              printf("vid %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x\n",
//              data[0], data[1], data[2], data[3], data[4],
//              data[5], data[6], data[7], data[8], data[9]);
        }

        fVidPacket->AddData(data, size);
}


inline void
TransportStreamDemux::ProcessData(const void *data, int size, bigtime_t start_time, bigtime_t delta)
{
        const uint8 *d = (const uint8 *)data;
        if (d[0] != 0x47 && size > 376 && d[188] != 0x47 && d[376] != 0x47) {
                printf("TransportStreamDemux::ProcessData: input sync error: "
                           "%02x %02x %02x %02x %02x %02x %02x %02x "
                           "%02x %02x %02x %02x %02x %02x %02x %02x\n",
                           d[0], d[1], d[2], d[3], d[4], d[5], d[6], d[7], 
                           d[8], d[9], d[10], d[11], d[12], d[13], d[14], d[15]);
                return;
        }

        const mpeg_ts_packet *pkt = (const mpeg_ts_packet *)data;
        int count = size / 188;
        for (int i = 0; i < count; i++) {
                ProcessPacket(pkt++, start_time + (i * delta) / count);
        }
}


void
TransportStreamDemux::AddData(Packet *packet)
{
        bigtime_t end_time = packet->TimeStamp();
        
        if (fLastEndTime == 0) {
                #define DEFAULT_DELTA 36270
                // need something at the start, 36270 usec is the packet length
                // in Germany, and should be ok for other countries, too
                fLastEndTime = end_time - DEFAULT_DELTA;
        }

        bigtime_t delta = end_time - fLastEndTime;
        
        // sanity check
        if (delta > (3 * DEFAULT_DELTA)) {
                printf("TransportStreamDemux::ProcessData: delta %.6f is too large\n", delta / 1E6);
                fLastEndTime = end_time - DEFAULT_DELTA;
                delta = DEFAULT_DELTA;
        }

        ProcessData(packet->Data(), packet->Size(), fLastEndTime, delta);
        
        packet->SetTimeStamp(fLastEndTime);

        fLastEndTime = end_time;

        status_t err = fMpegTsQueue->Insert(packet);
        if (err != B_OK) {
                delete packet;
                if (err == B_WOULD_BLOCK) {
                        printf("fMpegTsQueue->Insert failed (would block)\n");
                }
        }
}