// For printf used below
#include <stdio.h>
#include <queue>

// PJON library
#include <inttypes.h>
#include <stdlib.h>
#include <string.h>
// RPI serial interface
#include <wiringPi.h>
#include <wiringSerial.h>

#include "gateway.pb.h"
#include "gateway.grpc.pb.h"

#include <grpc/grpc.h>
#include <grpc++/channel.h>
#include <grpc++/client_context.h>
#include <grpc++/create_channel.h>
#include <grpc++/security/credentials.h>

#ifndef RPI
#define RPI true
#endif

#define PJON_INCLUDE_TS true // Include only ThroughSerial
#include "PJON/PJON.h"

using std::queue;

using grpc::Channel;
using grpc::ClientAsyncResponseReader;
using grpc::ClientContext;
using grpc::CompletionQueue;
using grpc::Status;

using gatekeeper::PJONGateway;
using gatekeeper::Empty;
using gatekeeper::Message;
using gatekeeper::SendStatus;

class PJONAgent final {
public:
  PJONAgent (PJON<ThroughSerial> *bus, std::unique_ptr<PJONGateway::Stub> *stub) {
    this->bus = bus;
    //this->bus->set_id(1);
    this->bus->strategy.set_enable_RS485_pin(1);
    this->addr = (":50051");
  }

  void run () {
    while (true) {
      //printf("Attempting to send a packet... \n");
      //this->bus->send(2, "B", 1);
      this->bus->send(2, "b", 1);

      printf("Attempting to roll bus... \n");
      this->bus->update();

      printf("Attempting to receive from bus... \n");
      this->bus->receive(1000);
      break;
    }
  }

private:
  std::string addr;
  PJON<ThroughSerial> *bus;
  std::unique_ptr<PJONGateway::Stub> stub_;
};


PJON<ThroughSerial> initBus() {
  printf("PJON instantiation... \n");
  PJON<ThroughSerial> bus(1);

  printf("Opening serial... \n");
  int s = serialOpen("/dev/ttyAMA0", 9600);

  if (s < 0) {
    printf("Serial open fail!");
  }

  if (wiringPiSetup() == -1) {
    printf("WiringPi setup fail");
  }

  printf("Setting serial... \n");
  bus.strategy.set_serial(s);

  printf("Opening bus... \n");
  bus.begin();

  return bus;
}


int main()
{
  PJON<ThroughSerial> bus = initBus();

  std::shared_ptr<Channel> channel = grpc::CreateChannel("localost:50051", grpc::InsecureChannelCredentials());
  std::unique_ptr<PJONGateway::Stub> stub = PJONGateway::NewStub(channel);
  PJONAgent svc(&bus, &stub);
  svc.run();

  /*printf("Attempting to send a packet... \n");
  bus.send(2, "B", 1);

  printf("Attempting to roll bus... \n");
  bus.update();

  printf("Attempting to receive from bus... \n");
  bus.receive();*/

  printf("Success! \n");
  return 0;
};