#include <stdio.h>
#include <stdint.h>
#include <unistd.h>
#include <string.h>
#include <errno.h>
#include <termios.h>

#include <sys/types.h>
#include <sys/socket.h>
#include <sys/ioctl.h>
#include <net/if.h>
#include <linux/can.h>
#include <linux/can/raw.h>
#include <vector>

#include <stdlib.h>
#include <sys/mman.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <pthread.h>

#include <linux/input.h>
#include <linux/uinput.h>

#include "abi-ipc.h"
#include "ABICommands.h"

#define DEFAULT_ABI_RX_DEVICE 	"/dev/mocca0"
#define DEFAULT_ABI_TX_DEVICE 	"/dev/mocca0"
#define UINPUT_DEVICE	 	"/dev/uinput"

using namespace K2L::Automotive::ABI_IPC;
using namespace K2L::Automotive::ABI;

#include "HVAC_Bin_jitter10.c"

class AbiMainHandler : public CABIListener
{
private:
    AbiUsbIpc *abiIpc;
    IABI *abi;
    bool isConnected;
    unsigned int connectionLabel;
    Bus canBuses[6] = { CAN1, CAN2, CAN3, CAN4, CAN5, CAN6};
    Bus xcanBuses[6] = { XCAN1, XCAN2, XCAN3, XCAN4, XCAN5, XCAN6};
    Bus linBuses[6] = { LIN1, LIN2, LIN3, LIN4, LIN5, LIN6};
    int busCount = 6;
    int socketId = -1;
    struct can_frame txCanFrame;
    struct sockaddr_can addr;
    struct ifreq ifr;

    pthread_t receivingThread;
    pthread_t sendingThread;
    BYTE busToSend = 0xFF;

    pthread_mutex_t mutex;
    BYTE currentState[8];
    BYTE nextState[8];
    bool sendNextState = false;

	int uinputFileDescriptor = -1;
    
    virtual void OnCanRxMessage(IABI* pAbi, void* pContext, unsigned long long timeStamp, Bus bus, unsigned int messageId, bool extended, BYTE* payload, int payloadLength)
    {
        if (messageId == 0x25B && payloadLength >= 8)
        {
            if (payload[6] & 0x01)
            {
                printf("MEDIA\n");
                SendKey(KEY_MEDIA);
            }
            if (payload[7] & 0x01)
            {
                printf("MAP\n");
                SendKey(KEY_FN_F11);
            }
            if (payload[5] & 0x08)
            {
                printf("COM\n");
                SendKey(KEY_PHONE);
            }
            if (payload[6] & 0x08)
            {
                printf("NAV\n");
                SendKey(KEY_FN_F12);
            }
            if (payload[4] & 0x20)
            {
                printf("BACK\n");
                SendKey(KEY_BACK);
            }
            if (payload[5] & 0x01)
            {
                printf("OPTION\n");
                SendKey(KEY_OPTION);
            }
            if (payload[3] & 0x01)
            {
                printf("iDRIVE (ENTER)\n");
                SendKey(KEY_ENTER);
            }
            uint8_t code = (payload[3] >> 4);
            if (code == 1)
            {
                printf("iDRIVE (UP)\n");
                SendKey(KEY_UP);
            }
            if (code == 7)
            {
                printf("iDRIVE (DOWN)\n");
                SendKey(KEY_DOWN);
            }
            if (code == 0xA)
            {
                printf("iDRIVE (LEFT)\n");
                SendKey(KEY_LEFT);
            }
            if (code == 0x4)
            {
                printf("iDRIVE (RIGHT)\n");
                SendKey(KEY_RIGHT);
            }
        }
        else
        {
            if (CAN1 <= bus && bus <= CAN6)
            {
                printf("CAN%i: RX %04X Std", bus - CAN1 + 1, messageId, payloadLength);
            }
            if (XCAN1 <= bus && bus <= XCAN6)
            {
                printf("CAN%i: RX %04X Ext", bus - XCAN1 + 1, messageId, payloadLength);
            }
            printf(":");
            for(int i = 0; i < payloadLength; i++)
            {
                printf(" %2X", payload[i]);
            }
            printf("\n");
        }
    }

    void SendKey(unsigned int code)
    {
        if(this->uinputFileDescriptor < 0)
        {
            return;
        }

        struct input_event ev;

        memset(&ev, 0, sizeof(ev));

        ev.type = EV_KEY;
        ev.code = code;
        ev.value = 1;

        if (write(this->uinputFileDescriptor, &ev, sizeof(ev)) >= 0)
        {
            printf("Key: %X\n", code);
        }
    }

    virtual void OnLinRxMessage(IABI* pAbi, void* pContext, unsigned long long timeStamp, Bus bus, unsigned int sourceAddress,
                                unsigned int messageId, BYTE* payload, int payloadLength)
    {
        if (LIN1 <= bus && bus <= LIN6)
        {
            printf("LIN%i: RX %02X ", bus - LIN1 + 1, messageId, payloadLength);
        }
        printf(":");
        for(int i = 0; i < payloadLength; i++)
        {
            printf(" %2X", payload[i]);
        }
        printf("\n");
        if (this->busToSend == 0xFF)
        {
            this->busToSend = bus - LIN1 + 1;
            printf("Selected bus LIN%i\n", this->busToSend);
        }

        if (messageId == 0x2D)
        {
            // StatusAfterCalibration
            CheckResult("Identify", CABICommands::Identify(abi));
            unsigned long long result;
            CheckResult("Relay", CABICommands::SetRelay( abi, false, result));
        }
        if (messageId == 0x31 && payloadLength >= 8)
        {
            // Status
            if (pthread_mutex_lock(&mutex) == 0)
            {
                printf("Save state");
                for(int i = 0; i < payloadLength; i++)
                {
                    printf(" %2X", payload[i]);
                }
                printf("...\n");
                memcpy(currentState, payload, 8);
                if (memcmp(currentState, nextState, 8) == 0)
                {
                    sendNextState = false;
                }
                else if (sendNextState)
                {
                    printf("Next state is ready to send...\n" );
                    struct can_frame frame;
                    frame.can_id = 0x01;
                    frame.can_dlc = 8;
                    memcpy(&frame.data, this->nextState, 8);
                    sendto(socketId, &frame, sizeof(struct can_frame), 0, (struct sockaddr*)&addr, sizeof(addr));
                }
                pthread_mutex_unlock(&mutex);
            }
        }
    }

    static void *ReceiveThread( void *param )
    {
        if( NULL == param )
        {
            printf("ERROR: Virtual CAN receiving thread could not be started\n" );
            pthread_exit( 0 );
        }
        AbiMainHandler *that = ( AbiMainHandler * )param;

        printf("Virtual CAN receiving thread started...\n" );
        int nbytes = 0;
        struct can_frame frame;
        while( that->isConnected )
        {
            nbytes = read(that->socketId, &frame, sizeof(struct can_frame));
            if (nbytes < 0)
            {
                printf("ERROR: could not read CAN frame");
            }

            if (frame.can_id == 0x01 && frame.can_dlc >= 8)
            {
                printf("");
                printf("Send next state LIN%i: ", that->busToSend);
                for(int i = 0; i < 8; i++)
                {
                    printf(" %2X", that->nextState[i]);
                }
                printf("...\n");
                CheckResult("Send", CABICommands::SendLINMessage(that->abi, 150, that->busToSend, 0x30, frame.can_dlc, frame.data));
                if (pthread_mutex_lock(&(that->mutex)) == 0)
                {
                    memcpy(that->nextState, frame.data, 8);
                    that->sendNextState = true;
                    pthread_mutex_unlock(&(that->mutex));
                }
                continue;
            }

            printf("Virtual CAN: message received %x: ", frame.can_id );
            for(int i = 0; i < frame.can_dlc; i++)
            {
                printf(" %2X", frame.data[i]);
            }
            printf("\n");

            bool send = false;
            if (frame.can_dlc >= 8 && pthread_mutex_lock(&(that->mutex)) == 0)
            {
                if (that->sendNextState)
                {
                    printf("Save next state...\n");
                    memcpy(that->nextState, frame.data, 8);
                }
                else
                {
                    send = true;
                    memcpy(that->nextState, frame.data, 8);
                    that->sendNextState = true;
                }
                pthread_mutex_unlock(&(that->mutex));
            }

            if (send)
            {
                printf("Send new state...\n");
                CheckResult("Send", CABICommands::SendLINMessage(that->abi, 151, that->busToSend, frame.can_id, frame.can_dlc, frame.data));
            }
        }
        pthread_exit( 0 );
    }

    virtual void OnLinTxMessage(IABI* pAbi, void* pContext, unsigned long long timeStamp, unsigned short handle, Bus bus, unsigned int targetAddress,
                                unsigned int messageId, BYTE* payload, int payloadLength)
    {
        if (LIN1 <= bus && bus <= LIN6)
        {
            printf("LIN%i: TX %02X ", bus - LIN1 + 1, messageId, payloadLength);
        }
        printf(":");
        for(int i = 0; i < payloadLength; i++)
        {
            printf(" %2X", payload[i]);
        }
        printf("\n");

        if (pthread_mutex_lock(&mutex) == 0)
        {
            memcpy(currentState, payload, 8);
            pthread_mutex_unlock(&mutex);
        }
    }

    static void CheckResult( const char *command, int result )
    {
        if( 0 == result )
            printf( "Command '%s' succeeded\n", command );
        else
            printf( "Command '%s' failed, with error code: %d\n", command, result );
    }

    bool SetupAbiDevice()
    {
        //Lets the Active LED twinkle fast
        CheckResult( "Identify", CABICommands::Identify( abi ) );
        
        CheckResult( "Delete all receivers", CABICommands::Delete( abi, 0 ) );
        unsigned long long result;
        CheckResult("Relay", CABICommands::SetRelay( abi, true, result));

        for(int i = 0; i < busCount; i++)
        {
            CheckResult( "Start CAN", CABICommands::StartCan( abi, canBuses[i], canTransceiverFast, canSpeed500) );
            
            CheckResult( "Add CAN receiver", CABICommands::AddReceiver( abi, 100 + i, canBuses[i], 0x0, 0x7FF ) );
            CheckResult( "Start CAN receiver", CABICommands::Start( abi, 100 + i ) );
            
            CheckResult( "Add extended CAN receiver", CABICommands::AddReceiver( abi, 110 + i, xcanBuses[i], 0x0, 0x1FFFFFFF ) );
            CheckResult( "Start extended CAN receiver", CABICommands::Start( abi, 110 + i ) );
        }
        
        for(int i = 0; i < busCount; i++)
        {
            CheckResult( "Start LIN", CABICommands::StartLin(abi, linBuses[i], sizeof(bin_data), (BYTE*)bin_data));

            CheckResult( "Add LIN receiver", CABICommands::AddReceiver( abi, 120 + i, linBuses[i], 0xFF, 0 ) );
            CheckResult( "Start LIN receiver", CABICommands::Start( abi, 120 + i ) );
        }
        
        this->socketId = socket(PF_CAN, SOCK_RAW, CAN_RAW);
        if (this->socketId < 0)
        {
            printf("ERROR: socket could not be created\n");
            return false;
        }
        
        strcpy(ifr.ifr_name, "vcan0" );
        if(ioctl(this->socketId, SIOCGIFINDEX, &ifr))
        {
            printf("ERROR: ioctl failed\n");
            return false;
        }
        
        addr.can_family = AF_CAN;
        addr.can_ifindex = ifr.ifr_ifindex;

        if (bind(this->socketId, (struct sockaddr *)&addr, sizeof(addr)) < 0)
        {
            printf("ERROR: bind failed\n");
            return false;
        }

        int recv_own_msgs = 1; /* 0 = disabled (default), 1 = enabled */
        setsockopt(this->socketId, SOL_CAN_RAW, CAN_RAW_RECV_OWN_MSGS,
                       &recv_own_msgs, sizeof(recv_own_msgs));
        
        return true;
    }

public:
    AbiMainHandler() : isConnected( false ), connectionLabel( 0xFFFFFFFF )
    {
        abiIpc = new AbiUsbIpc();
        abi = CABIFactory::CreateAbi();
        pthread_mutex_init(&mutex, NULL);
		this->uinputFileDescriptor = open(UINPUT_DEVICE, O_WRONLY | O_NONBLOCK);
		if(this->uinputFileDescriptor < 0)
		{
            printf("ERROR: Cannot open UInput!\n");
		}
        else if(ioctl(this->uinputFileDescriptor, UI_SET_EVBIT, EV_KEY) >= 0
                && ioctl(this->uinputFileDescriptor, UI_SET_KEYBIT, KEY_MEDIA) >= 0
                && ioctl(this->uinputFileDescriptor, UI_SET_KEYBIT, KEY_PHONE) >= 0
                && ioctl(this->uinputFileDescriptor, UI_SET_KEYBIT, KEY_MENU) >= 0
                && ioctl(this->uinputFileDescriptor, UI_SET_KEYBIT, KEY_BACK) >= 0
                && ioctl(this->uinputFileDescriptor, UI_SET_KEYBIT, KEY_OPTION) >= 0
                && ioctl(this->uinputFileDescriptor, UI_SET_KEYBIT, KEY_FN_F11) >= 0
                && ioctl(this->uinputFileDescriptor, UI_SET_KEYBIT, KEY_FN_F12) >= 0
                && ioctl(this->uinputFileDescriptor, UI_SET_KEYBIT, KEY_SCROLLDOWN) >= 0
                && ioctl(this->uinputFileDescriptor, UI_SET_KEYBIT, KEY_SCROLLUP) >= 0
                && ioctl(this->uinputFileDescriptor, UI_SET_KEYBIT, KEY_UP) >= 0
                && ioctl(this->uinputFileDescriptor, UI_SET_KEYBIT, KEY_LEFT) >= 0
                && ioctl(this->uinputFileDescriptor, UI_SET_KEYBIT, KEY_RIGHT) >= 0
                && ioctl(this->uinputFileDescriptor, UI_SET_KEYBIT, KEY_DOWN) >= 0
                && ioctl(this->uinputFileDescriptor, UI_SET_KEYBIT, KEY_ENTER) >= 0)
		{
            struct uinput_user_dev uidev;

            memset(&uidev, 0, sizeof(uidev));

            snprintf(uidev.name, UINPUT_MAX_NAME_SIZE, "uinput-sample");
            uidev.id.bustype = BUS_USB;
            uidev.id.vendor  = 0x1234;
            uidev.id.product = 0xfedc;
            uidev.id.version = 1;

            if (write(this->uinputFileDescriptor, &uidev, sizeof(uidev)) >= 0
                    && ioctl(this->uinputFileDescriptor, UI_DEV_CREATE) >= 0)
            {
                printf("UInput ready...\n");
            }
            else
            {
                printf("ERROR: cannot create UInput device...\n");
            }
		}
        else
        {
            printf("ERROR: UInput failed!\n");
        }
    }

    ~AbiMainHandler()
    {
        CheckResult( "Delete all receivers", CABICommands::Delete( abi, 0 ) );

        for(int i = 0; i < busCount; i++)
        {
            CheckResult( "Stop LIN", CABICommands::StopLin(abi, linBuses[i]));
        }
        
        if(this->socketId != -1)
        {
            close(this->socketId);
            printf("Socket closed\n");
        }

		if(this->uinputFileDescriptor >= 0)
		{
            if (ioctl(this->uinputFileDescriptor, UI_DEV_DESTROY) >= 0)
            {
                printf("UInput device destroyed\n");
            }
			close(this->uinputFileDescriptor);
            printf("UInput closed\n");
		}
    }

    bool Connect( const char *cdevRx, const char *cdevTx )
    {
        if( isConnected )
        {
            isConnected = false;
            abiIpc->Disconnect();
        }
        bool isSingleDevice = ( 0 == strcmp( cdevRx, cdevTx ) );
        bool success = isSingleDevice ? abiIpc->Connect( cdevRx ) : abiIpc->Connect( cdevRx, cdevTx );
        if( success )
        {
            int err = abi->Init( abiIpc );
            if( 0 == err )
            {
                abi->Start( this );
                isConnected = SetupAbiDevice();
                if(  pthread_create( &this->receivingThread, NULL, ReceiveThread, this ) )
                {
                    printf( "Failed to create receive thread\n" );
                    return false;
                }
            }
        }

        return isConnected;
    }    
};

int main( int argc, char *argv[] )
{
    AbiMainHandler mainHandler;
    bool success = mainHandler.Connect( DEFAULT_ABI_RX_DEVICE, DEFAULT_ABI_TX_DEVICE );
    
    if(success)
    {
        printf( "OptoLyzer MOCCA compact connected...\n");
        printf("Press any key to finish...\n");
        getchar();
    }
    else
    {
        printf( "ERROR: ABI could not started...\n", success );
    }
    
    return 0;
}