/**
 * proxy - Sample application to encapsulate HW/SW interfacing with embedded systems.
 * Copyright (C) 2012 - 2015 Christian Berger
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * as published by the Free Software Foundation; either version 2
 * of the License, or (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
 */

#include <ctype.h>
#include <cstring>
#include <cmath>

#include "core/base/KeyValueConfiguration.h"
#include "core/data/Container.h"
#include "core/data/TimeStamp.h"
#include "OpenCVCamera.h"
#include "core/data/control/VehicleControl.h"
#include "core/data/environment/VehicleData.h"
#include "GeneratedHeaders_Data.h"

#include "Proxy.h"

 // IMPORT FOR SERIAL
#include <string>
#include <iostream>
#include <cstdio>
#include <unistd.h>
#include "serial/serial.h"

// IMPORT FOR NANO
#include <stdio.h>
#include <pb_encode.h>
#include <pb_decode.h>
#include "message.pb.h"

 // USING FOR SERIAL
 using std::string;
 using std::exception;
 using std::cout;
 using std::cerr;
 using std::endl;
 using std::vector;

 namespace msv {

    using namespace std;
    using namespace core::base;
    using namespace core::data;
    using namespace core::data::control;
    using namespace core::data::environment;
    using namespace tools::recorder;

    Proxy::Proxy(const int32_t &argc, char **argv) :
    ConferenceClientModule(argc, argv, "proxy"),
    m_recorder(NULL),
    m_camera(NULL)
    {}

    Proxy::~Proxy() {
    }

    void Proxy::setUp() {
        // This method will be call automatically _before_ running body().
        if (getFrequency() < 20) {
            cerr << endl << endl << "Proxy: WARNING! Running proxy with a LOW frequency (consequence: data updates are too seldom and will influence your algorithms in a negative manner!) --> suggestions: --freq=20 or higher! Current frequency: " << getFrequency() << " Hz." << endl << endl << endl;
        }


        // Get configuration data.
        KeyValueConfiguration kv = getKeyValueConfiguration();

        // Create built-in recorder.
        const bool useRecorder = kv.getValue<uint32_t>("proxy.useRecorder") == 1;
        if (useRecorder) {
            // URL for storing containers.
            stringstream recordingURL;
            recordingURL << "file://" << "proxy_" << TimeStamp().getYYYYMMDD_HHMMSS() << ".rec";
            // Size of memory segments.
            const uint32_t MEMORY_SEGMENT_SIZE = getKeyValueConfiguration().getValue<uint32_t>("global.buffer.memorySegmentSize");
            // Number of memory segments.
            const uint32_t NUMBER_OF_SEGMENTS = getKeyValueConfiguration().getValue<uint32_t>("global.buffer.numberOfMemorySegments");
            // Run recorder in asynchronous mode to allow real-time recording in background.
            const bool THREADING = true;

            m_recorder = new Recorder(recordingURL.str(), MEMORY_SEGMENT_SIZE, NUMBER_OF_SEGMENTS, THREADING);
        }

        // Create the camera grabber.
        const string NAME = getKeyValueConfiguration().getValue<string>("proxy.camera.name");
        string TYPE = getKeyValueConfiguration().getValue<string>("proxy.camera.type");
        std::transform(TYPE.begin(), TYPE.end(), TYPE.begin(), ::tolower);
        const uint32_t ID = getKeyValueConfiguration().getValue<uint32_t>("proxy.camera.id");
        const uint32_t WIDTH = getKeyValueConfiguration().getValue<uint32_t>("proxy.camera.width");
        const uint32_t HEIGHT = getKeyValueConfiguration().getValue<uint32_t>("proxy.camera.height");
        const uint32_t BPP = getKeyValueConfiguration().getValue<uint32_t>("proxy.camera.bpp");

        if (TYPE.compare("opencv") == 0) {
            m_camera = new OpenCVCamera(NAME, ID, WIDTH, HEIGHT, BPP);
        }

        if (m_camera == NULL) {
            cerr << "No valid camera type defined." << endl;
        }
    }

    void Proxy::tearDown() {
        // This method will be call automatically _after_ return from body().
        OPENDAVINCI_CORE_DELETE_POINTER(m_recorder);
        OPENDAVINCI_CORE_DELETE_POINTER(m_camera);
    }

    void Proxy::distribute(Container c) {
        // Store data to recorder.
        if (m_recorder != NULL) {
            // Time stamp data before storing.
            c.setReceivedTimeStamp(TimeStamp());
            m_recorder->store(c);
        }

        // Share data.
        getConference().send(c);
    }

    double usCentre;
    double usRight;
    double irRight_one;
    double irRight_two;
    double irRear;
    int wheelTicks;
    string port("/dev/ttyACM0");
    unsigned long baud = 38400; //19200; //9600;
    serial::Serial my_serial(port, baud, serial::Timeout::simpleTimeout(100)); //simpleTimeout(1000)

    // This method will do the main data processing job.
    ModuleState::MODULE_EXITCODE Proxy::body() {
        uint32_t captureCounter = 0;
        int numOfErrors = 0;
        string mark = "=";
        while (getModuleState() == ModuleState::RUNNING) {
            // Capture frame.
            if (m_camera != NULL) {
                core::data::image::SharedImage si = m_camera->capture();

                Container c(Container::SHARED_IMAGE, si);
                //distribute(c);
                captureCounter++;
            }

            // TODO: Here, you need to implement the data links to the embedded system
            // to read data from IR/US.
            // DECODE
            //read serial when available for sensor data sent fto Arduino
            if(my_serial.available()){
            ArduinoToOdroid messageIn;
            uint8_t bufferIn[64];
            string result;
            //string result = my_serial.read(27);
            my_serial.readline(result, 28, mark);
            //cerr << "length: " << result << endl;
            for(int i = 0; i < 27; i++){
                bufferIn[i] = result[i];
            }
            pb_istream_t streamIn = pb_istream_from_buffer(bufferIn, 27);
            bool read_status = pb_decode(&streamIn, ArduinoToOdroid_fields, &messageIn);
            //cerr << "Test " << endl;
            if(!read_status){
                numOfErrors++;
                cerr << "Error number: " << numOfErrors << endl;
            }
            else{
                cerr << "Ultrasound centre: " << messageIn.usCentre << endl;
                cerr << "Ultrasound right: " << messageIn.usRight << endl;
                cerr << "Infrared right 1: " << messageIn.irRight_one << endl;
                cerr << "Infrared right 2: " << messageIn.irRight_two << endl;
                cerr << "Infrared rear: " << messageIn.irRear << endl;
                cerr << "Wheel ticks: " << messageIn.wheelTicks << endl;
                cerr << endl << endl;
            }
            }

            // Encode
            OdroidToArduino messageOut;
            messageOut.spd = 70;
            messageOut.angle = 24;
            uint8_t bufferOut[64];
            pb_ostream_t streamOut = pb_ostream_from_buffer((uint8_t*)bufferOut, sizeof(bufferOut));
            bool sendStatus = pb_encode(&streamOut, OdroidToArduino_fields, &messageOut);
            int length = streamOut.bytes_written;
            if (sendStatus) {
                for (int i = 0; i < length; i++) {
                    my_serial.write(&bufferOut[i], 1);
                }
                my_serial.write(mark);
                //cerr << "length of =:"<<mark.length() << endl;
            }  else {
                //cerr << "failed" << endl;
            }

        }

        cout << "Proxy: Captured " << captureCounter << " frames." << endl;

        return ModuleState::OKAY;
    }
} // msv