#include <stdint.h>
#include <stdlib.h>
#include <unistd.h>
#include <fcntl.h>
#include <sys/mman.h>
#include "opencv2/opencv.hpp"
#include <iostream>
#include <thread>

#include "gip.h"

// **********************************************************************
//
// Frame-Interrupt Thread der HDMI-Ausgabe des IPs ZYNQ-BASE-HDMI
//
// Alle 60 Frames (und damit jede Sekunde) wird die Farbe einer
// der RGB-LEDs auf dem Zybo-Board geändert
//
// **********************************************************************
void blinkRGBLED()
{
    int fdZynqBase = open ("/dev/uio0", O_RDWR);
    PGIP_ZYNQ_BASE zynqBase= (PGIP_ZYNQ_BASE) mmap(NULL, 0x1000, PROT_READ|PROT_WRITE, MAP_SHARED, fdZynqBase, 0);

    int i = 0;
    int col = 0;
    int pending;
    int reenable=1;

    // Initialisierung:
    // Interrupts freigeben, INT-Status loeschen und Interrupt-verarbeitung im UIO-Treiber aktivieren
    zynqBase->InterruptStatus = 0;
    zynqBase->InterruptEnable = 1;
    write(fdZynqBase,&reenable,4);
    
    // Hier die eigentliche ISR:
    // INTs zaehlen und alle 60 INTs die Farbe einer der RGB-LEDs aendern
    while(1) {
        read(fdZynqBase,&pending,4);
        write(fdZynqBase,&reenable,4);
        zynqBase->InterruptStatus = 0;
        i++;
        if (i==60) {
            i=0;
            zynqBase->Board_IO &= 0x00FFFFFF;
            zynqBase->Board_IO |= 1<<(24+col);
            col = (col+1)%3;
        }
    }
}


// **********************************************************************
// 
// Thread zur Demonstration des Verhaltens eines Video-Streaming-IPs
// mit Ausgabe der Input- und Output-Bilder ueber OpenCV-Fenster
//
// Bei CIF-Aufloesung (352x288) wird annaehernd Echtzeit erreicht
// Bei Verwendung hoeherer Aufloesungen ist die Ausgabe, bedingt durch OpenCV, deutlich verlangsamt
//
// **********************************************************************

int videoDemo()
{
    int currFrame = 0;  // Zaehler fuer aktuell verwendetes Bild 
    int videoNum  = 6;  // Auswahl der Videosequenz - aus den nachfolgend aufgefuehrten

    const std::string videoFiles[9] = {
                                 "/home/user/Videos/testseq/MobileCalendar",
                                 "/home/user/Videos/testseq/Football",
                                 "/home/user/Videos/testseq/acht_cam_",
                                 "/home/user/Videos/testseq/acht_bev_",
                                 "/home/user/Videos/testseq/BetesPasBetes_CIF",
                                 "/home/user/Videos/testseq/Football_CIF",
                                 "/home/user/Videos/testseq/MobileCalendar_CIF",
                                 "/home/user/Videos/testseq/FlowerGarden_CIF",
                                 "/home/user/Videos/testseq/LePoint_CIF",
                                };

    std::string fn = videoFiles[videoNum]+std::to_string(currFrame++)+".jpg";   // Filename
    cv::Mat imgJpg = cv::imread(fn,cv::IMREAD_GRAYSCALE);                       // Erstes Bild lesen

    char c = 0;
    int pending;
    int reenable=1;
 
    // Memory-Mapping durchfuehren
 
    int fdPrivateMemory = open ("/dev/uio16", O_RDWR);
    uint8_t* privMem  = (uint8_t*) mmap(NULL, 0x8000000, PROT_READ|PROT_WRITE, MAP_SHARED, fdPrivateMemory, 0); // Request 128 MB

    int fdMm2vs = open ("/dev/uio1", O_RDWR);
    PGIP_AXI_2D_MM2VS mmvs= (PGIP_AXI_2D_MM2VS) mmap(NULL, 0x1000, PROT_READ|PROT_WRITE, MAP_SHARED, fdMm2vs, 0);
  
    int fdFilter = open ("/dev/uio2", O_RDWR);
    int32_t* filter = (int32_t*) mmap(NULL, 0x1000, PROT_READ|PROT_WRITE, MAP_SHARED, fdFilter, 0);

    // Falls MMVS bereits aktiv ist, sollte er zunächst alle laufenden Requests zuende abarbeiten
    // Schaltet man zunaechst die Zufuehrung von Daten (MM2VS) ab, können auch die Streaming-FIFOs leerlaufen
    mmvs->MM2VS_Control = 0;
    usleep(100000);
    mmvs->VS2MM_Control = 0;
    usleep(100000);


    if (!imgJpg.empty()) {  // Konnte das 1. Bild erfolgreich gelesen werden? 
                            // Falls nein, brauchen wir gar nicht weitermachen und beenden den Thread
    
        cv::Mat imgGrayIn(imgJpg.size(),CV_8UC1,&privMem[0]);           // Speicher fuer Eingangsbild einer OpenCV-Mat zuordnen
        cv::Mat imgGrayOut(imgJpg.size(),CV_8UC1,&privMem[0x400000]);   // dto. fuer Ausgangsbild
    
        mmvs->InterruptEnable = (1<<1);         // VS2MM INT freigeben
        write(fdMm2vs,&reenable,4);

        mmvs->MM2VS_StartAddress = 0x30000000;  // Adressierungsparameter fuer Eingangsbild setzen
        mmvs->MM2VS_HorizontalBytes = imgJpg.cols;
        mmvs->MM2VS_Stride=imgJpg.cols;
        mmvs->MM2VS_VerticalLines=imgJpg.rows;
        mmvs->MM2VS_Control |= (0x0<<12);       // ARCACHE

        mmvs->VS2MM_StartAddress = 0x30400000;  // Adressierungsparameter fuer Ausgangsbild setzen
        mmvs->VS2MM_HorizontalBytes = imgJpg.cols;
        mmvs->VS2MM_Stride=imgJpg.cols;
        mmvs->VS2MM_VerticalLines=imgJpg.rows;
        mmvs->VS2MM_InterruptLine = 1;
        mmvs->VS2MM_Control |= (0x0<<12);       // AWCACHE
        
        mmvs->VS2MM_Control |= (1<<1);          // Synchronize with Start of Frame (e.g. TUSER=1)
    
        mmvs->InterruptStatus = 0;              // INT Status im MMVS loeschen
        
        mmvs->VS2MM_Control |= (1<<0);          // Start VS2MM
        mmvs->MM2VS_Control |= (1<<0);          // Start MM2VS
    
        // Filter Coefficients
        filter[0]= -1;
        filter[1]= -2;
        filter[2]= -1;
        filter[3]= 0;
        filter[4]= 0;
        filter[5]= 0;
        filter[6]= 1;
        filter[7]= 2;
        filter[8]= 1;
        filter[9]= 0;


        //
        // Schleife durch alle Bilder der Videodateien (Abbruch mit ESC-Taste)
        //
        
        while(c!=27) {  // Press  ESC on keyboard to exit
            fn = videoFiles[videoNum]+std::to_string(currFrame++)+".jpg";
            imgJpg = cv::imread(fn,cv::IMREAD_GRAYSCALE);
        
            if (imgJpg.empty()) { 
                currFrame = 0;
            } else {
                memcpy(imgGrayIn.data,imgJpg.data,imgJpg.cols*imgJpg.rows);
                    
                read(fdMm2vs,&pending,4);           // Auf VS2MM INT warten
                write(fdMm2vs,&reenable,4);         // INT wieder aktivieren
                mmvs->InterruptStatus = 0;
          
                cv::imshow( "In",  imgGrayIn );    // Display input image
                cv::imshow( "Out", imgGrayOut);    // Display output image
            
                //fn = videoFiles[videoNum]+std::to_string(currFrame)+"_out.jpg";
                //cv::imwrite(fn,imgGrayOut);
            
                c=(char)cv::waitKey(1);    // Press  ESC on keyboard to exit
            }
        }
        cv::destroyAllWindows();  // Close all OpenCV windows
    }
    
    // Memory-Streaming ausschalten (wir wollen schliesslich nicht unnoetig die anderen Komponenten ausbremsen)
    
    mmvs->MM2VS_Control = 0;
    usleep(100000);
    mmvs->VS2MM_Control = 0;
    usleep(100000);

    return 0;
}



// **********************************************************************
// 
// main()
//
// **********************************************************************

int main(int argc, char** argv)
{
    std::thread t1(blinkRGBLED);        // Threads anlegen
    std::thread t2(videoDemo);
    //t1.join();                        // Der Blink-Thread wird nie beendet, also brauchen wir darauf auch nicht warten
    t2.join();                          // Wenn der OpenCV-Thread beendet wird, sind wir fertig
    return 0;
}