#include <Wire.h>
//#include <Arduino.h>

#include "SHTSensor.h"
#include <ArduinoJson.h>

//Watchdog
#include <avr/sleep.h>
#include <avr/wdt.h>
#include <avr/io.h>


#define pinBLEPOW 2 //Digital PA6
#define pinVBAT 9   //Analogic PA2

int compteur=0;
int ADCres=1023;

//Watchdog preparation
//#define sbi(sfr,bit) (_SFR_BYTE(sfr) != _BV(bit))
// (SLPCTRL.CTRLA) = sleep register
//  SMODE = sleep mode
  //SLPCTRL.CTRLA &= (b10 << 1);
//set_sleep_mode(SLEEP_MODE_PWR_DOWN);
//sleep_enable();
//  SEN = Sleep enable
//  SLEEP = sleep start

volatile boolean t_wdt=1;


SHTSensor sht;
// To use a specific sensor instead of probing the bus use this command:
//SHTSensor sht();

void init_BLE(){
  Serial.println("$$$");//Command mode activation
  Serial.println("SS:C0");//Setting transparent UART mode activation
  Serial.println("R,1");//Reboot of the board
  delay(1000);//Let the board reboot

}

void init_sleep(){
  byte ii = WDT.STATUS;
  Serial.print(ii,BIN);
  ii = SLPCTRL.CTRLA;
  Serial.println(ii,BIN);
  SLPCTRL.CTRLA  |= (0b10 << 1);//Sleep mode is power down
  ii = SLPCTRL.CTRLA;
  Serial.println(ii,BIN);
  SLPCTRL.CTRLA |=  0b1; //Sleep enable
  ii = SLPCTRL.CTRLA;
  Serial.println(ii,BIN);
  //OUTPUT to INPUT
  //Switch off ADCs
}

void setup_watchdog(int val){
  //WDT.CTRLA
  //PERIOD BIT non zero activate watchdog
  //WINDOW=0x0 -> normal mode
}

void setup() {
  //Serial actiavtion
  Serial.begin(115200);
  //PIN mode to power BLE board
  pinMode(pinBLEPOW,OUTPUT);
  digitalWrite(pinBLEPOW,LOW);
  //PIN mode to measure Vbat voltage
  pinMode(pinVBAT,INPUT);
  ////Preparing sleep mode
  ////Sleep mode init
  ////setup_watchdog(7);
  //init_sleep();
  ////On wake up remove the sleep enable bit
  //SLPCTRL.CTRLA &=  ~1; //Sleep enable
  
  Wire.begin();
  //Changing internal reference for ADC
  //analogReference(INTERNAL4V34); // Deactivated until further reading on internal voltage reference.
  //The command above will most probably not work properly if the battery voltage drops below 4.3v

  delay(1000); // let serial console settle
  //Initialization of BLE board
  init_BLE();
  if (sht.init()) {
     // Serial.print("init(): success\n");
  } else {
     // Serial.print("init(): failed\n");
  }
  sht.setAccuracy(SHTSensor::SHT_ACCURACY_MEDIUM); // only supported by SHT3x

}

void loop() {

  if (sht.readSample()) {
    Serial.println((String)sht.getTemperature()+";"+(String)sht.getHumidity());
    // The lines below are temporarilly not sued. I need to learn more about voltage reference
    //Also it seems the SHT35 sensor does not read for vbattery voltage of 4.7V
    //Serial.print(";");
    //Serial.print("  Vbat:  ");
    //float vbat = analogRead(pinVBAT)/ADCres*INTERNAL4V34;
    //Serial.println(vbat, 2);
    //Serial.println(INTERNAL4V3,2);
    //Serial.print("\n");
  } else {
    Serial.print("-1;-1");
  }

  delay(10000);
}