#pragma once
#include "SensorDemo.h"
#pragma comment(lib, "Ws2_32.lib")
SensorDemo::~SensorDemo()
{
    // deletes the instances
    delete mySensor;
    delete communication;
    delete commSettings;
}
void SensorDemo::Connect() {
#ifdef ETHERNET
    if (port == -1) {
        cout << "Please enter sensor ip  (Default sensor's ip is 192.168.1.2)" << endl;
        cin >> ip;
        cout << "Please enter sensor port (default senor's port is 3050)" << endl;
        cin >> port;
    }
    commSettings = new EthernetSettings(ip, port, port);
    communication = new EthernetCommunication((EthernetSettings*)commSettings);
#else
    commSettings = new UsbSettings(0x2161, 0x0405, 0x81, 0x1);
    communication = new UsbCommunication((UsbSettings*)commSettings);
#endif

    try {
        mySensor = new RawDataSensor(communication);
        // Initialize the sensor and establishing the connection
        mySensor->Initialize();
    }
    catch (CommunicationException e) {
        cout << e.what();
    }
    cout << "Sensor connected" << endl;
}

void SensorDemo::Disconnect()
{   // disconnect the channel
    mySensor->GetCommunication()->Disconnect();   
    cout << "Sensor disconnected" << endl;
}

void SensorDemo::DisplaySensorVersion()
{
    // Get network information
    NetworkInformation netInfo = mySensor->GetNetworkInformation();

    // Get sensor version
    SoftwareVersion version = mySensor->GetVersion();
    
    // Display data
    cout << "Version: " << version.GetVersion() << endl;
    cout << "Revision: " << version.GetRevision() << endl;
    cout << "Can number: " << version.GetCanNumber() << endl;
    cout << "Hardware version: " << version.GetHardwareVersion() << endl;
    cout << "Part number: " << version.GetPartNumber() << endl;
    cout << "Prototype: " << version.GetPrototype() << endl;
    cout << "ProductIdentifier: " << version.GetProductIdentification() << endl;
    cout << endl;
    cout << "Sensor's IP: " << netInfo.GetIpAddress() << endl;
    cout << "Sensor's subnet mask: " << netInfo.GetSubnetMask() << endl;
    cout << "Sensor's gateway: " << netInfo.GetGateway() << endl;
    cout << "Sensor's port: " << netInfo.GetIpPort() << endl;
}

void SensorDemo::StartMdi()
{
    cout << "Starting MDI at sensor" << endl;
    mySensor->SendMdi();
    cout << "Started" << endl;
}

void SensorDemo::StopMdi()
{
    cout << "Stoping MDI at sensor" << endl;
    mySensor->StopMdi();
    cout << "Stoped" << endl;
}

void SensorDemo::ReadRawDataSync(int count)
{
    int i = 0;
    do {
        RawData data = mySensor->RetrieveRawData();

        // Do whatever you want with your data (plot, treat, display etc.)
        // For this example we will write that in a file.
        WriteDataToFile(data);

		try {
			if (data._scanCount > 0) {
				delete (data._indexes);
				delete(data._angles);
				delete(data._intensities);
				delete(data._distances);
				delete(data._timestamps);
			}
		}
		catch (exception e) {
			cout << "cannot free pointers" << endl;
		}
		
        i++;
    } while (i < count && !StopRawDataRead);

    cout << "Exit ReadRawDataSync" << endl;
}

void SensorDemo::ReadRawDataAsync(int count)
{
    thread rawDataThreadHandle;
    rawDataThreadHandle = thread(&SensorDemo::ReadRawDataSync, this, count);
    rawDataThreadHandle.join();
}

void SensorDemo::OperateRecorder()
{
    thread rawDataThreadHandle;
    rawDataThreadHandle = thread(&SensorDemo::ReadRawDataSync, this, 10000000); // RawData must be retrieven from the sensor to be able to record them

    FileDataRecorder recorder("RawdataRecord.dat");
    mySensor->SetDataRecorder(&recorder);
    string command;
    system("cls");
    do {
        cout << endl;
        cout << "Please enter a command (/!\\ commands are case sensitive)" << endl;        
        cout << "'record' to record RawData in a DAT file" << endl;
        cout << "'play' to replay the DAT file" << endl;
        cout << "'pause' to pause the replay" << endl;
        cout << "'stop' to either stop record or play" << endl;
        cout << "'framerate' to set replay framerate during replay" << endl;
        cout << "'status' to get the recorder status" << endl;
        cout << "'export' to export DAT file in a CSV" << endl;
        cout << "'q' to leave this menu" << endl;

        cin >> command;
        if (command == "record") {
            // This will record the rawdata in the RawdataRecord.dat
            mySensor->GetDataRecorder()->Record();
            cout << "Recorder is recording" << endl;
        }
        if (command == "play") {
            // This will replay the rawdata stored in the RawdataRecord.dat
            // Data will be fetched by the mySensor->RetrieveRawData()
            // Replay has the priority on the sensor raw data sent through communication layer
            mySensor->GetDataRecorder()->Play();
            cout << "Recorder is playing" << endl;
        }
        else if (command == "pause") {
            // This will pause the replay (no effect on the recording)
            mySensor->GetDataRecorder()->Pause();
            cout << "Recorder is paused" << endl;
        }
        else if (command == "export") {
            // Once stopped, will export to a CSV file
            mySensor->GetDataRecorder()->ExportToCsv("export.csv", MILLIMETERS);
            cout << "Recorder is paused" << endl;
        }
        else if (command == "stop") {
            // This will stop the player if playing or the recorder if recording
            mySensor->GetDataRecorder()->Stop();
            cout << "Recorder has  been stopped" << endl;

        }
        else if (command == "status") {
            // Will show a status on the screen
            int status = mySensor->GetDataRecorder()->GetStatus();
            switch (status) {
            case BaseDataRecorder::STOPPED:
                cout << "Recorder is stopped" << endl;
                break;
            case BaseDataRecorder::PLAYING:
                cout << "Recorder is playing" << endl;
                break;
            case BaseDataRecorder::RECORDING:
                cout << "Recorder is recording" << endl;
                break;
            case BaseDataRecorder::PAUSED:
                cout << "Recorder is paused" << endl;
                break;
            }
        }
        else if (command == "framerate") {
            /// This will change the framerate
            cout << "Actual framerate is " << mySensor->GetDataRecorder()->GetFrameRate() << ", enter new framerate" << endl;
            cin >> command;
            mySensor->GetDataRecorder()->SetFrameRate(atoi(command.c_str()));
            cout << "Frame rate has been set to " << command << endl;
        }
    } while (command != "q");

    StopRawDataRead = true;
    rawDataThreadHandle.join();
    system("cls");
}

void SensorDemo::SettingsManagement()
{
    string command;
    system("cls");
    do {
        cout << endl;
        cout << "Please enter a command (/!\\ commands are case sensitive)" << endl;
        cout << "'version' to record RawData in a DAT file" << endl;
        cout << "'name' to manage the sensor's name" << endl;
        cout << "'anglerange' to manage the scan angle range" << endl;
        cout << "'scanfreq' to manage the scan frequency and angle resolution" << endl;
        cout << "'filter' to manage the data filter" << endl;
        cout << "'contat' to manage the window contamination threshold" << endl;
        cout << "'contaw' to get the window contamination status" << endl;        
        cout << "'packet' to manage the data packet type" << endl;
        cout << "'direction' to manage the data output direction" << endl;
        cout << "'protocol' to manage the output protocol" << endl;
        cout << "'leds' to manage the sensor's leds status" << endl;
        cout << "'lamps' to get the sensor's leds color" << endl;
        cout << "'reset' to reset the sensor to factory default" << endl;
        cout << "'q' to leave this menu" << endl;
        cin >> command;
        system("cls");
        if (command == "version") {
            DisplaySensorVersion();
        } 
        else if (command == "name") {
            try {
                string sensorname = mySensor->GetDeviceName();
                cout << "Current sensor's name is: " << sensorname << endl;
                cout << "Please enter the new name" << endl;
                cin >> sensorname;

                cout << "Fetching '" << sensorname << "' in the sensor..." << endl;
                mySensor->SetDeviceName(sensorname);
                cout << "Fetch succesful, new sensor name is " << mySensor->GetDeviceName() << endl;
            }
            catch (exception e) {
                cout << e.what() << endl;
            }
        }
        else if (command == "anglerange") {
            AngleRange range = mySensor->GetAngleRange();
            cout << "Angle range boundary 1 " << to_string(range.GetStart()) << ", boundary 2 " << to_string(range.GetStop()) << endl;
            int startAngle, stopAngle;
            cout << "Enter boundary 1 (between -47,6 and 226,6 bellow boundary 2)" << endl;
            cin >> startAngle;
            cout << "Enter boundary 2 (between -47,6 and 226,6 above boundary 1)" << endl;
            cin >> stopAngle;

            
            range.SetStart(startAngle);
            range.SetStop(stopAngle);

            if (mySensor->SetAngleRange(range).GetResult()) {
                range = mySensor->GetAngleRange();
                cout << "Angle range boundary 1 " << to_string(range.GetStart()) << ", boundary 2 " << to_string(range.GetStop()) << endl;
            }
            else {
                cout << "Failed";
            }
        }

        else if (command == "scanfreq") {
            SensorResolutionFrequencyEnum res = mySensor->GetAngularResolution();
            switch (res)
            {
            case 0:
                cout << "Current resolution is 0,2 @ 80hz" << endl;
                break;
            case 1:
                cout << "Current resolution is 0,1 @ 40hz" << endl;
                break;
            }
            cout << "Enter the new resolution (0 for 0,2@80hz, 1 for 0,1@40hz)" << endl;
            int resolution;
            cin >> resolution;

            switch (resolution) {
            case 0: 
                res = SensorResolutionFrequencyEnum::eigthy_hz;
                break;
            case 1: 
                res = SensorResolutionFrequencyEnum::fourthy_hz;
                break;
            }

            mySensor->SetAngularResolution(res);

            res = mySensor->GetAngularResolution();
            switch (res)
            {
            case 0:
                cout << "New resolution is 0,2 @ 80hz" << endl;
                break;
            case 1:
                cout << "New resolution is 0,1 @ 40hz" << endl;
                break;
            }
        }
        else if (command == "filter") {
            FILTER_STATUS filterStatus = mySensor->GetFilterStatus();
            cout << "Current filter status is: " << filterStatus << endl;
            cout << "Please enter the filter status (0: OFF, 1: ON)" << endl;
            cin >> filterStatus;
            cout << "Fetching '" << filterStatus << "' in the sensor..." << endl;
            mySensor->SetFilterStatus(filterStatus);
            cout << "Fetch succesful, new filter status is " << mySensor->GetFilterStatus() << endl;
        }
        else if (command == "contat") {
            int warning, error;
            cout << "enter warning level (between 0 and 100 and bellow ERROR contamination threshold)" << endl;
            cin >> warning;
            cout << "enter error level (between 0 and 100 and above WARNING contamination threshold)" << endl;
            cin >> error;

            ContaminationThreshold cont;
            cont.SetWarning(warning);
            cont.SetError(error);

            if (mySensor->SetContaminationThreshold(cont).GetResult()) {
                cont = mySensor->GetContaminationThreshold();
                cout << "Contamination threshold warning is " << to_string(cont.GetWarning()) << ", error is " << to_string(cont.GetError()) << endl;
            }
            else {
                cout << "Failed";
            }
        }
        else if (command == "contaw") {
            try {
                ContaminationStatus contStatus = mySensor->GetContaminationStatus();
                cout << "Contamination status left: " << contStatus.GetDataLeft() << ", middle: " << contStatus.GetDataMiddle() << ", and right: " << contStatus.GetDataRight() << endl;
            }
            catch (exception e) {
                cout << e.what() << endl;
            }
        }
        else if (command == "packet") {
            SCAN_PACKET_TYPE ptype = mySensor->GetDataPacketType();
            switch (ptype)
            {
            case 0:
                cout << "Current packet type is set to distance only" << endl;
                break;
            case 1:
                cout << "Current packet type is set to distance & amplitude" << endl;
                break;
            }
            cout << "Enter the packet type (0 for distance, 1 for distance & amplitude)" << endl;
            cin >> ptype;
            mySensor->SetDataPacketType(ptype);

            ptype = mySensor->GetDataPacketType();
            switch (ptype)
            {
            case 0:
                cout << "New packet type is set to distance only" << endl;
                break;
            case 1:
                cout << "New packet type is set to distance & amplitude" << endl;
                break;
            }
        }
        else if (command == "direction") {
            SCANNING_DIRECTION scandir = mySensor->GetScanningDirection();
            switch (scandir)
            {
            case 0:
                cout << "Current scanning direction is clockwise" << endl;
                break;
            case 1:
                cout << "Current scanning direction is counter clockwise" << endl;
                break;
            }
            cout << "Enter the new scanning directon (0 for CLOCKWISE, 1 for COUNTER CLOCKWISE)" << endl;
            cin >> scandir;
            mySensor->SetScanningDirection(scandir);

            scandir = mySensor->GetScanningDirection();
            switch (scandir)
            {
            case 0:
                cout << "New scanning direction is clockwise" << endl;
                break;
            case 1:
                cout << "New scanning direction is counter clockwise" << endl;
                break;
            }
        }
        else if (command == "protocol") {
            PROTOCOL protp = mySensor->GetRawDataExchangeMode();
            switch (protp)
            {
            case 0:
                cout << "Current  mode is UDP" << endl;
                break;
            case 1:
                cout << "Current mode is TCP" << endl;
                break;
            }
            cout << "Enter the new exchange mode (0 for UDP, 1 for TCP)" << endl;
            cin >> protp;
            mySensor->SetRawDataExchangeMode(protp);

            protp = mySensor->GetRawDataExchangeMode();
            switch (protp)
            {
            case 0:
                cout << "New  mode is UDP" << endl;
                break;
            case 1:
                cout << "New mode is TCP" << endl;
                break;
            }
        }
        else if (command == "leds") {
            SensorLeds leds;
            leds = mySensor->GetSensorLeds();
            cout << "Status led is " << to_string(leds.GetStatusLed()) << ", logo led is " << to_string(leds.GetLogoLed()) << endl;

            int status, logo;
            cout << "enter status led value" << endl;
            cin >> status;
            cout << "enter logo led value" << endl;
            cin >> logo;


            leds.SetStatusLed(status);
            leds.SetLogoLed(logo);

            if (mySensor->SetSensorLeds(leds).GetResult()) {
                leds = mySensor->GetSensorLeds();
                cout << "Status led is " << to_string(leds.GetStatusLed()) << ", logo led is " << to_string(leds.GetLogoLed()) << endl;
            }
            else {
                cout << "Failed";
            }
        }
        else if (command == "lamps") {
            SensorLamps lamps;
            lamps = mySensor->GetSensorLamps();
            DisplayLamp(lamps);
        }
        else if (command == "reset") {
            cout << "Reseting sensor to its default" << endl;
            mySensor->ResetDevice();
            cout << "Sensor has been reset to its default" << endl;
        }
    } while (command != "q");
    system("cls");
}

void SensorDemo::ChangeNetworkSettings()
{
    string newIp = "";
    cout << "Please, enter the new IP address" << endl;
    cin >> newIp;
    ip = newIp;
    NetworkInformation netInfo = mySensor->GetNetworkInformation();
    netInfo.SetIpAddress(newIp);
    try {
       mySensor->SetNetworkInformation(netInfo);
    }
    catch (CommunicationException e) {
        cout << "Disconnection failed, which is normal " << endl;
    }
    
#ifdef  ETHERNET
    cout << "Disconnecting the sensor" << endl;
    mySensor->GetCommunication()->Disconnect();
    ((EthernetSettings*)mySensor->GetCommunication()->GetSettings())->SetIp(newIp);
    ((EthernetSettings*)mySensor->GetCommunication()->GetSettings())->SetTcpPort(netInfo.GetIpPort());
    ((EthernetSettings*)mySensor->GetCommunication()->GetSettings())->SetUdpPort(netInfo.GetIpPort());
    cout << "Connecting the sensor" << endl;
    mySensor->GetCommunication()->Connect();
    cout << "New IP is " << ((EthernetSettings*)(mySensor->GetCommunication()->GetSettings()))->GetIp() << endl;
#endif     

    cout << "Connected, sensor IP is " << netInfo.GetIpAddress() << endl;
}

void SensorDemo::WriteDataToFile(RawData toWrite)
{
    fstream my_file;
    my_file.open("myrawdata.txt", std::fstream::in | std::fstream::out | std::fstream::app);

    for (int spots = 0; spots < toWrite._scanCount; spots++) {
        if (toWrite._intensities != NULL) {
            my_file << toWrite._indexes[spots] << ";" << toWrite._angles[spots] << ";" << toWrite._distances[spots] << ";" << toWrite._intensities[spots] << ";" << toWrite._timestamps[spots] << endl;
        }
        else {
            if (toWrite._indexes != NULL) {
                my_file << toWrite._indexes[spots] << ";" << toWrite._angles[spots] << ";" << toWrite._distances[spots] << ";" << toWrite._timestamps[spots] << endl;
            }
        }
    }

    my_file.flush();
    my_file.close();
}

void SensorDemo::DisplayLamp(SensorLamps lamps) {
    string ledToDisplay;
    switch (lamps.GetLed1()) {
    case 0:
        ledToDisplay = ledToDisplay + " led1 is black ";
        break;
    case 1:
        ledToDisplay = ledToDisplay + " led1 is red ";
        break;
    case 2:
        ledToDisplay = ledToDisplay + " led1 is green ";
        break;
    case 3:
        ledToDisplay = ledToDisplay + " led1 is orange ";
        break;
    }
    switch (lamps.GetLed2()) {
    case 0:
        ledToDisplay = ledToDisplay + ", led 2 is black ";
        break;
    case 1:
        ledToDisplay = ledToDisplay + ", led 2 is red ";
        break;
    case 2:
        ledToDisplay = ledToDisplay + ", led 2 is green ";
        break;
    case 3:
        ledToDisplay = ledToDisplay + ", led 2 is orange ";
        break;
    }
    switch (lamps.GetLed3()) {
    case 0:
        ledToDisplay = ledToDisplay + ", led 3 is black ";
        break;
    case 1:
        ledToDisplay = ledToDisplay + ", led 3 is red ";
        break;
    case 2:
        ledToDisplay = ledToDisplay + ", led 3 is green ";
        break;
    case 3:
        ledToDisplay = ledToDisplay + ", led 3 is orange ";
        break;
    }
    switch (lamps.GetLed4()) {
    case 0:
        ledToDisplay = ledToDisplay + ", led 4 is black ";
        break;
    case 1:
        ledToDisplay = ledToDisplay + ", led 4 is red ";
        break;
    case 2:
        ledToDisplay = ledToDisplay + ", led 4 is green ";
        break;
    case 3:
        ledToDisplay = ledToDisplay + ", led 4 is orange ";
        break;
    }
    cout << ledToDisplay << endl;
}