
#include <stdlib.h>
#include <stdio.h>
#include <signal.h>
#include <unistd.h>
#include <assert.h>
#include <string>

#include "PuppetParserRQ.h"
#include "SerialLineHandler.h"

using namespace Puppet;

int end = 0;
void sighdl(int n) {
    end ++;
    if (end>2) {
        kill(getpid(),SIGKILL);
    }
}

class PuppetInterface {
    protected:
        SerialLineHandler serial;
        PuppetParserRQ pp;

        bool verbose;
        bool leave_zeros;

        static void handler(void * arg, const std::string & s) {
            PuppetInterface * pi = (PuppetInterface*)arg;
            pi->parseLine(s);
        }




        void parseLine(const std::string & s) {

            // ROS_INFO("Line %s",s.c_str());
            // printf(".");fflush(stdout);
            if (!pp.parseLine(s)) {
                return;
            }
            if (pp.measurementsAreReady()) {
                // Print the topology on stdout
                if (!pp.topologyHasChanged()) {
                    pp.printTopology(true,stdout);
                } else {
                    pp.printTopology(false,stdout);
                    // If the topology has changed, we'll get the value at the next
                    // run. Nothing to do for now.
                }
            }
        }

    public:
        PuppetInterface(unsigned int devId) : serial(devId,handler,this) {
            verbose=true;
            leave_zeros = true;
            serial.setMaxSize(256);
        }

        bool open(unsigned int speed=115200) {
            return serial.Open(speed);
        }

        void mainloop() {
            while (!end) {
                // nothing to do here, serial is creating its own thread
                sleep(1);
            }
            serial.Close();
        }
};


int main(int argc, char * argv[])
{
    unsigned int baudrate = 115200;
    unsigned int devId = 0;
    if (argc > 1) {
        sscanf(" %d",argv[1],&devId);
    }
    if (argc > 2) {
        sscanf(argv[2]," %d",&baudrate);
    }
    printf("Using serial port '%d' @ %d\n",devId,baudrate);

    PuppetInterface pi(devId);
    assert(pi.open(baudrate));
    
    signal(SIGINT,sighdl);
    pi.mainloop();

    return 0;
}

