 // If false is returned, nothing gets written to the eeprom.
 //
 boolean eeprom_write_string(int addr, char* string) {
-	// actual number of bytes to be written
-	int numBytes;
-
-	// we'll need to write the string contents
-	// plus the string terminator byte (0x00)
-	numBytes = strlen(string) + 1;
-	for (int i = 0; i < numBytes; i++) {
-		EEPROM.write(addr + i, string[i]);
-	}
-	return true;
+  // actual number of bytes to be written
+  int numBytes;
+
+  // we'll need to write the string contents
+  // plus the string terminator byte (0x00)
+  numBytes = strlen(string) + 1;
+  for (int i = 0; i < numBytes; i++) {
+    EEPROM.write(addr + i, string[i]);
+  }
+  return true;
 }
 
 //


char* topic = (char*) malloc(1);

// byte server[] = { 172, 16, 0, 2 };
char server[] = "aceone.se";

Adafruit_CC3000 cc3000 = Adafruit_CC3000(ADAFRUIT_CC3000_CS, ADAFRUIT_CC3000_IRQ, ADAFRUIT_CC3000_VBAT, SPI_CLOCK_DIVIDER);


unsigned long timeStamp;

int initError = 1;



void setup() {
  pinMode(PRESSURE_READING_PIN, INPUT);
  //	pinMode(CONFIGURE_BUTTON_PIN, INPUT_PULLUP);
  pinMode(LED_INDICATOR_PIN, OUTPUT);
  pinMode(PRESSURE_IRQ_PIN, INPUT);

  Serial.begin(115200);
  Serial.print("Free RAM: "); 
  Serial.println(getFreeRam(), DEC);

  Serial.println();
  eeprom_read_string(0, placeName, PLACE_NAME_LENGTH);
  buildTopic();

  Serial.print(F("Conecting to WIFI ."));
  digitalWrite(LED_INDICATOR_PIN, HIGH);

  if (!cc3000.begin()) {
    Serial.println();
    Serial.println(F("Faild to begin"));
    errorMessage(2);            
  }

  Serial.print(F("."));

  if (!cc3000.deleteProfiles()) {
    Serial.println(F("Failed!"));
    errorMessage(3);            
  }

  Serial.println(F(" OK"));

  uint8_t macAddress[MAC_LENGTH];

  cc3000.getMacAddress(macAddress);

  bytesToHexString(macAddress, macHex, MAC_LENGTH);

  // read local air pressure and create offset.
  triggerValue = analogRead(A0) + THRESHOLD;
  delay(1000);
  Serial.println(F("Traffic Counter"));
  Serial.println(F("___________________________________________________"));
  Serial.println();
  Serial.print(F("Place: "));
  Serial.println(placeName);
  Serial.print(F("Local Air Pressure: "));
  Serial.println(triggerValue - THRESHOLD);
  Serial.println(F("___________________________________________________"));

  String tmpName = "TC-";
  tmpName.concat(macHex);
  mqttName[tmpName.length()];
  tmpName.toCharArray(mqttName, tmpName.length() + 1);
  Serial.print(F("Mqtt name: "));
  Serial.println(mqttName);

  while(!enableConnection()) {
    Serial.print(F("Failed to enable connection"));
    //while(1);
    shutdownConnection();
    delay(1000);
    Serial.print(F("Retrying to connect"));
  }


  digitalWrite(LED_INDICATOR_PIN, LOW);
	delay(1000);
  	Serial.println(F("Shut down!"));
  	shutdownConnection();
  	delay(1000);
  	Serial.println(F("Sleep now!"));
  	delay(100);
  	sleepNow();
  Serial.print("Free RAM: "); 
  Serial.println(getFreeRam(), DEC);
  initError = 0;
}

void loop() {
  client.loop();
  checkPressure();

  unsigned long ml = millis();

  if (connected() && ml > timeStamp + CONNECTION_ALIVE_TIME) {
    shutdownConnection();
    delay(100);
    sleepNow();
  }
  if (!bufferEmpty() && ml > timeStamp + PUBLISH_DELAY) {
    if (connected()) {
      publishBuffer();
    } 
    else {
      if (!enableConnection()) {
        shutdownConnection();
        timeStamp += PUBLISH_DELAY;
      }
    }
  }
}

void checkPressure(){
  //1 - TUBE IS PRESSURIZED INITIALLY
  if (analogRead(A0) > triggerValue) {
    if (strikeNumber == 0 && isMeasuring == 0) { // FIRST HIT
      Serial.println();
      Serial.println(F("Car HERE. "));
      firstWheel = millis();
      isMeasuring = 1;
    }
    if (strikeNumber == 1 && isMeasuring == 1) { // SECOND HIT
      Serial.println(F("Car GONE."));
      secondWheel = millis();
      isMeasuring = 0;
    }
  }

  //2 - TUBE IS STILL PRESSURIZED
  while (analogRead(A0) > max && isMeasuring == 1) { //is being pressed, in all cases. to measure the max pressure.
    max = analogRead(A0);
  }

  //3 - TUBE IS RELEASED
  if (analogRead(A0) < triggerValue - 1 && countThis == 0) { //released by either wheel
    if (strikeNumber == 0 && isMeasuring == 1 && (millis() - firstWheel > WHEEL_DELAY)) {
      strikeNumber = 1;
    }
    if (strikeNumber == 1 && isMeasuring == 0 && (millis() - secondWheel > WHEEL_DELAY)) {
      countThis = 1;
    }
  }

  //4 - PRESSURE READING IS ACCEPTED AND RECORDED
  if ((analogRead(A0) < triggerValue - 1) && ((countThis == 1 && isMeasuring == 0) || ((millis() - firstWheel) > CAR_TIMEOUT) && isMeasuring == 1)) { //has been released for enough time.
    Serial.print(F("Pressure Reached = "));
    Serial.println(max);
    //Serial.print("time between wheels = ");
    wheelTime = ((secondWheel - firstWheel) / 3600000);
    //Serial.println(wheelTime);
    int time = ((millis() / 1000) / 60) + 1; // the number of seconds since first record.
    speed = (WHEEL_SPACEING / 1000) / wheelTime;
    if (speed > 0) {
      Serial.print(F("Estimated Speed (km/h) = "));
      Serial.println(speed);
    } 
    else {
      speed = 1.0;
    }
    publish(speed);
    //RESET ALL VALUES
    max = 0;
    strikeNumber = 0;
    countThis = 0;
    isMeasuring = 0;
  }
}

void publishBuffer() {
  while(!bufferEmpty()){
    float speed = bufferRead();
    publishToClient(speed)
  }
  timeStamp = millis();
}

void publish(float speed) {
  timeStamp = millis();
  if (bufferEmpty() && connected()) {
    publishToClient(speed)
  } else {
    bufferWrite(speed);
  }
}

void publishToClient(float speed) {
    char tmp[6];
    dtostrf(speed, 1, 2, tmp);
    client.publish(topic, tmp);
}


int bufferFull() {
  return speed_buffer_count == SPEED_BUFFER_SIZE;
}

int bufferEmpty() {
  return speed_buffer_count == 0;
}

void bufferWrite(float speed) {
  int end = (speed_buffer_start + speed_buffer_count) % SPEED_BUFFER_SIZE;
  speed_buffer[end] = speed;
  if (speed_buffer_count == SPEED_BUFFER_SIZE) {
    speed_buffer_start = (speed_buffer_start + 1) % SPEED_BUFFER_SIZE; /* full, overwrite */
    Serial.println(F("Buffer full ower writing"));
  } 
  else {
    ++speed_buffer_count;
  }
}

float bufferRead() {
  float speed = speed_buffer[speed_buffer_start];
  speed_buffer_start = (speed_buffer_start + 1) % SPEED_BUFFER_SIZE;
  --speed_buffer_count;
  return speed;
}

void sleepNow()         // here we put the arduino to sleep
{
  // Turn off the ADC while asleep.
  power_adc_disable();
  power_all_disable();

  /* Now is the time to set the sleep mode. In the Atmega8 datasheet
   	 * http://www.atmel.com/dyn/resources/prod_documents/doc2486.pdf on page 35
   	 * there is a list of sleep modes which explains which clocks and 
   	 * wake up sources are available in which sleep mode.
   	 *
   	 * In the avr/sleep.h file, the call names of these sleep modes are to be found:
   	 *
   	 * The 5 different modes are:
   	 *     SLEEP_MODE_IDLE         -the least power savings 
   	 *     SLEEP_MODE_ADC
   	 *     SLEEP_MODE_PWR_SAVE
   	 *     SLEEP_MODE_STANDBY
   	 *     SLEEP_MODE_PWR_DOWN     -the most power savings
   	 *
   	 * For now, we want as much power savings as possible, so we 
   	 * choose the according 
   	 * sleep mode: SLEEP_MODE_PWR_DOWN
   	 * 
   	 */
  set_sleep_mode (SLEEP_MODE_PWR_DOWN);   // sleep mode is set here

  sleep_enable();          // enables the sleep bit in the mcucr register
  // so sleep is possible. just a safety pin 

  /* Now it is time to enable an interrupt. We do it here so an 
   	 * accidentally pushed interrupt button doesn't interrupt 
   	 * our running program. if you want to be able to run 
   	 * interrupt code besides the sleep function, place it in 
   	 * setup() for example.
   	 * 
   	 * In the function call attachInterrupt(A, B, C)
   	 * A   can be either 0 or 1 for interrupts on pin 2 or 3.   
   	 * 
   	 * B   Name of a function you want to execute at interrupt for A.
   	 *
   	 * C   Trigger mode of the interrupt pin. can be:
   	 *             LOW        a low level triggers
   	 *             CHANGE     a change in level triggers
   	 *             RISING     a rising edge of a level triggers
   	 *             FALLING    a falling edge of a level triggers
   	 *
   	 * In all but the IDLE sleep modes only LOW can be used.
   	 */

  attachInterrupt(PRESSURE_IRQ, wakeUpNow, FALLING); // use interrupt 0 (pin 2) and run function
  // wakeUpNow when pin 2 gets LOW 

  sleep_mode();            // here the device is actually put to sleep!!
  // THE PROGRAM CONTINUES FROM HERE AFTER WAKING UP

  sleep_disable();         // first thing after waking from sleep:
  // disable sleep...
  detachInterrupt(PRESSURE_IRQ);      // disables interrupt 0 on pin 2 so the 
  // wakeUpNow code will not be executed 
  // during normal running time.
  power_all_enable();
  Serial.print(F("Back from sleep"));

}

void startTimeIntrrupt(){
  // initialize timer1 
  noInterrupts();           // disable all interrupts
  TCCR1A = 0;
  TCCR1B = 0;
  TCNT1  = 0;

  OCR1A =  INTERRUPT_FREQENCY;
  TCCR1B |= (1 << WGM12);   // CTC mode
  TCCR1B |= (1 << CS12);    // 256 prescaler 
  TIMSK1 |= (1 << OCIE1A);  // enable timer compare interrupt
  interrupts();             // enable all interrupts
}

void stopTimeIntrrupt(){
  noInterrupts();           // disable all interrupts
  TIMSK1 &= ~(1 << OCIE1A);  // disable timer compare interrupt
  interrupts();             // enable all interrupts
}

ISR(TIMER1_COMPA_vect)          // timer compare interrupt service routine
{
  checkPressure();
}

// Enable the CC3000 and connect to the wifi network.
// Return true if enabled and connected, false otherwise.
boolean enableConnection() {
  Serial.println(F("Turning on CC3000."));

  if(!initError){
    startTimeIntrrupt();
  }
  if (!digitalRead(ADAFRUIT_CC3000_VBAT)) {
    unsigned long t = millis();
    // Turn on the CC3000.
    wlan_start(0);
    Serial.print(F("CC3000 Started in "));
    Serial.print(millis()-t);
    Serial.println(F(" millis"));
  }
  if (!cc3000.checkConnected()) {
    Serial.print(F("Connectint to AP"));
    unsigned long t = millis();
    // Connect to the AP.
    if (!cc3000.connectToAP(WLAN_SSID, WLAN_PASS, WLAN_SECURITY)) {
      // Couldn't connect for some reason.  Fail and move on so the hardware goes back to sleep and tries again later.
      Serial.println(F("Failed!"));
      if(initError){
        errorMessage(4);            
      }
      Serial.println();
      return false;
    }
    Serial.print(F(" in "));
    Serial.print(millis()-t);
    Serial.println(F(" millis"));
  }
  /*
  Serial.println(F("Set static IP"));
   uint32_t ipAddress = cc3000.IP2U32(192, 168, 1, 19);
   uint32_t netMask = cc3000.IP2U32(255, 255, 255, 0);
   uint32_t defaultGateway = cc3000.IP2U32(192, 168, 1, 1);
   uint32_t dns = cc3000.IP2U32(8, 8, 4, 4);
   if (!cc3000.setStaticIPAddress(ipAddress, netMask, defaultGateway, dns)) {
   Serial.println(F("Failed to set static IP!"));
   if(initError){
   errorMessage(5);            
   }
   while(1);
   }
   */
  /* Optional: Revert back from static IP addres to use DHCP.
   See note for setStaticIPAddress above, this only needs to be
   called once and will be remembered afterwards by the CC3000.
   */
  /*  
   if (!cc3000.setDHCP()) {
   Serial.println(F("Failed to set DHCP!"));
   while(1);
   }
   */


  Serial.print(F("Request DHCP"));
  unsigned long t = millis();
  int attempts = 0;
  while (!cc3000.checkDHCP()) {
    if (attempts > 50) {
      Serial.println(F(" DHCP didn't finish!"));
      //      if(initError){
      //         errorMessage(5);            
      //      }
      return false;
    }
    attempts += 1;
    delay(200);
  }
  Serial.print(" in ");
  Serial.print(millis()-t);
  Serial.println(F(" millis"));
  //	while (!cc3000.checkDHCP()) {
  //		delay(100);
  //	}
  Serial.println(F("Finished DHCP"));

  Serial.println(F("Connecting to MQTT server"));
  if (!client.connected()) {
    if (!client.connect(mqttName)) {
      Serial.println(F("MQTT Connetion error"));
      if(initError){
        errorMessage(6);            
      }
      return false;
    }
    client.publish("traffic/counter/start", mqttName);
    client.publish("traffic/counter/place", placeName);
    client.subscribe("traffic/counter/config");
    Serial.print(F("Conneted to: "));
    Serial.print(server);
    Serial.println();
  }

  if(!initError){
    startTimeIntrrupt();
  }

  // Return success, the CC3000 is enabled and connected to the network.
  timeStamp = millis();
  return true;
}

// Disconnect from wireless network and shut down the CC3000.
void shutdownConnection() {
  if (client.connected()) {
    client.disconnect();
    Serial.println(F("Disconnected MQTT"));
  }
  // Disconnect from the AP if connected.
  // This might not be strictly necessary, but I found
  // it was sometimes difficult to quickly reconnect to
  // my AP if I shut down the CC3000 without first
  // disconnecting from the network.
  if (cc3000.checkConnected()) {
    cc3000.disconnect();
  }

  // Wait for the CC3000 to finish disconnecting before
  // continuing.
  while (cc3000.checkConnected()) {
    delay(100);
  }
  Serial.println(F("Disconnected CC3000"));

  // Shut down the CC3000.
  wlan_stop();

  Serial.println(F("CC3000 shut down."));
}

boolean connected() {
  return digitalRead(ADAFRUIT_CC3000_VBAT) && cc3000.checkConnected() && client.connected();
}

void buildTopic() {
  free(topic);
  String tmpTopic = "traffic/counter/car/";
  tmpTopic.concat(placeName);
  topic = (char*) malloc(tmpTopic.length());
  tmpTopic.toCharArray(topic, tmpTopic.length() + 1);
}

void wakeUpNow()        // here the interrupt is handled after wakeup
{
}

void callback(char* topic, byte* payload, unsigned int length) {
  // handle message arrived
  if (length < MAC_HEX_LENGTH) {
    return;
  }

  for (int i = 0; i < MAC_HEX_LENGTH - 1; i++) {
    if (payload[i] != macHex[i]) {
      return;
    }
  }

  int len = length - MAC_HEX_LENGTH + 1;
  // Allocate the correct amount of memory for the payload copy
  char* p = (char*) malloc(len);
  int i;
  // Copy the payload to the new buffer
  for (i = 0; i < len; i++) {
    p[i] = payload[MAC_HEX_LENGTH + i];
  }
  p[i] = '\0';
  eeprom_write_string(0, p);
  memcpy(placeName, p, i + 1);
  // Free the memory
  free(p);
  buildTopic();
}

static char nibbleToChar(uint8_t nibble) {
  if (nibble >= 0 && nibble < 10) {
    return '0' + nibble;
  }
  return 'A' + nibble - 10;
}

/* Convert a buffer of binary values into a hex string representation */
void bytesToHexString(uint8_t *bytes, char *retval, int buflen) {
  int i;
  for (i = 0; i < buflen; i++) {
    retval[i * 2] = nibbleToChar(bytes[i] >> 4);
    retval[i * 2 + 1] = nibbleToChar(bytes[i] & 0x0f);
  }
  retval[i * 2] = '\0';
}

//
