// vim: filetype=cpp
#include <SPI.h>
#include <Ethernet.h>

byte mac[] = {  0xAD, 0xEE, 0x00, 0xEF, 0xFE, 0xED };
const char *serverHost = "stomp.xapek.org";
const int serverPort = 61613;
const char *serverLogin = "arduino1";
const char *serverPasscode = "arduino1";
const char *serverQueue = "/queue/queue-put";

const int ledPin = 13;      // select the pin for the LED

struct StompHeader {
  const char *name;
  const char *value;
};

class StompClient; // Forward

struct StompFrame {
  const char *command;
  struct StompHeader *headers = 0;
  int headersPos = -1;
  const char *body = 0;
  void (*bodyFunc)(class StompClient&) = NULL;
  void addHeader(const char *name, const char *value) {
    headersPos++;
    headers = (StompHeader*)realloc(headers, sizeof(StompHeader) * (headersPos+1));
    memset(headers + headersPos, 0, sizeof(StompHeader));
    (headers+(sizeof(StompHeader)*headersPos))->name = name;
    (headers+(sizeof(StompHeader)*headersPos))->value = value;
  }
  void setBody(const char *b) {
    body = strdup(b);
  }
  StompFrame(const char *command) : command(strdup(command)) {};
  ~StompFrame() {
    if (headers != NULL) 
      delete headers;
    delete command;
    if (body)
      delete body;
  }
};

class StompClient : public EthernetClient {
  const char *serverHost;
  int serverPort;
  const char *serverLogin = NULL;
  const char *serverPasscode = NULL;
public:
  StompClient(const char *serverHost, int serverPort, const char *serverLogin, const char *serverPasscode) :
      serverHost(serverHost), serverPort(serverPort), serverLogin(serverLogin), serverPasscode(serverPasscode) {};
  int connect() {
    int result = EthernetClient::connect(serverHost, serverPort);
    if (result) {
      StompFrame frame("CONNECT");
      frame.addHeader("host", serverHost);
      if (serverLogin) frame.addHeader("login", serverLogin);
      if (serverPasscode) frame.addHeader("passcode", serverPasscode);
      
      send(frame);
      
      StompFrame *answer = readFrame();
      if (strcmp(answer->command, "CONNECTED") != 0) {
        Serial.print("Got no CONNECTED Frame, read: ");
        Serial.println(answer->command);
        stop();
        delete answer;
        return false;
      } else {
        delete answer;
        return true;
      }
    } else {
      return result;
    }
  };
  void send(StompFrame &frame) {
    EthernetClient::print(frame.command);
    EthernetClient::print("\n");
    for (int i = 0; i<=frame.headersPos; i++) {
      StompHeader *header = frame.headers+sizeof(StompHeader)*i;
      EthernetClient::print(header->name);
      EthernetClient::print(":");
      EthernetClient::print(header->value);
      EthernetClient::print("\n");
    }
    EthernetClient::print("\n");
    if (frame.bodyFunc) {
      frame.bodyFunc(*this);
    } if (frame.body) {
      EthernetClient::print(frame.body);
    }
    EthernetClient::write((byte)0);
  };

  void sendKeyValue(const char *key, const char *value) {
      EthernetClient::print(key);
      EthernetClient::print("=");
      EthernetClient::print(value);
      EthernetClient::print("\n");
  }
  StompFrame *readFrame() {
    char buf[255];
    int i = readBytesUntil('\n', buf, 254);
    buf[i] = '\0';
    StompFrame *frame = new StompFrame(buf);
    
    while (readBytesUntil('\n', buf, 255)) {
      //read header
      //ignore 
    }
    readBytesUntil('\0', buf, 255);
    frame->setBody(buf);
    
    flush();
    return frame;
  }
};





const long ADC_REF_MV = 3300;
const long ADC_MAX_STEP = 1024;
const long OV_FACTOR = 4; // Op. Verstärker Faktor = 4; 10mV = 1dB

void sendPinValue(class StompClient& c) {
  long sensorValue,
      sensorValueMV, 
      sensorValueDBA,
      pinnr;
  char buf[12];
  char *sensorKey = "arduino.aX", 
       *sensorKeyMV = "arduino.aX.mv",
       *sensorKeyDBA = "arduino.aX.dba";
  for (int i = A0; i <= A5; i++) {
    pinnr = i - A0;
    
    sensorValue = analogRead(i);
    sensorKey[9] = '0' + pinnr;
    snprintf(buf, 8, "%u", sensorValue);
    c.sendKeyValue(sensorKey, buf);
    
    sensorValueMV = sensorValue * (ADC_REF_MV*100/ADC_MAX_STEP);
    sensorKeyMV[9] = '0' + pinnr;
    snprintf(buf, 8, "%ld.%03ld", sensorValueMV/100, sensorValue%100);
    c.sendKeyValue(sensorKeyMV, buf);
    
    sensorValueDBA = sensorValueMV / OV_FACTOR; 
    sensorKeyDBA[9] = '0' + pinnr;
    snprintf(buf, 12, "%ld.%04ld", sensorValueDBA/1000, sensorValueDBA%1000);
    c.sendKeyValue(sensorKeyDBA, buf);
  }
}


void setup() {
  Serial.begin(9600);

  // declare the ledPin as an OUTPUT:
  pinMode(ledPin, OUTPUT);  
  
  // start the Ethernet and UDP: Use DHCP for IP-Configuration
  Ethernet.begin(mac);
  
  analogReference(EXTERNAL);
}



StompClient client(serverHost, serverPort, serverLogin, serverPasscode);

void loop() {
  unsigned long dt = millis();
  
  digitalWrite(ledPin, HIGH);
  if (client.connected()) {
    Serial.println("Send packet");
    StompFrame frame("SEND");
    frame.addHeader("destination", serverQueue);
    frame.bodyFunc = &sendPinValue;
    client.send(frame);
  } else {
    Serial.println("connecting");
    client.stop();
    if (client.connect()) {
      Serial.println("connected");
    } else {
      Serial.println("connection failed");
    }
  }
  digitalWrite(ledPin, LOW);
  
  dt = (millis() - dt);

  if (dt < 500) {
    Serial.println(String("sleep: ") + String((500-dt)));
    delay(500 - dt);
  }
}