#include <Arduino.h>

/* assumed platformio config
platform = nordicnrf52
board = delta_dfbm_nq620
framework = arduino
monitor_speed = 115200
upload_protocol = jlink
*/

/* MyBattery Relay pins */
#define LED_OUT             22  
#define REL1_SET            11  // pulse to set latchin relay
#define REL1_RES            12  // pulse to reset latching relay
#define REL2_SET            13
#define REL2_RES            14
#define NTC_PWR1            8   // powers R / NTC current measurtement divider
#define NTC_ADC1            2   // pin to measure NTC resistance
#define NTC_PWR2            7
#define NTC_ADC2            3
#define EXT_EN1             25  // enable power on EN1
#define EXT_EN2             26  // enable power on EN2
#define EXT_INT             27  // RAW GPIO (potentially external interrupt)
#define EXTP1               28  // current limited / predivided GPIO / ADC 
#define EXTP2               29  // current limited / predivided GPIO / ADC 
#define EXTP3               30  // RAW GPIO
#define EXTP4               31  // RAW GPIO
#define VBAT_ADC            4   // external supply sense with 2M / 499K divider
#define VBAT_EN             9   // connect external supply sense
#define RX_PAD              18  // RX PCB PAD
#define TX_PAD              19  // TX PCB PAD

#define MY_NODE_ID 10           // CHANGE ME (OR COMMENT)
#define MY_RADIO_NRF5_ESB
//#define  MY_PASSIVE_NODE        // in passive mode we don't depend on parent at all, much safer for battery and operation

//#define MY_DEBUG
//#define NODE_DEBUG

// Timing and power consumption control
#define LOOP_TIME   5000                              // loop time (how often measure temperature and manage pereferials)
#define LOOP_NOREG  30000                             // loop time when node not registered, lets node deep sleep
                                                      // LOOP_NOREG - MY_SLEEP_TRANSPORT_RECONNECT_TIMEOUT_MS which litle save battery
                                                      // if for any reason node can't find parent
#define ACTIVE_LOOP 60                                // every ACTIVE_LOOP loop iteration sleep with active RF
#define MY_SMART_SLEEP_WAIT_DURATION_MS         500
#define MY_TRANSPORT_WAIT_READY_MS              5000  // don't wait more that 5 second before star doing something useful
#define MY_SLEEP_TRANSPORT_RECONNECT_TIMEOUT_MS 500   // 500ms is enough to connecte if parrent become visible

#define MY_DISABLED_SERIAL                            // important to save 6uA of battery
#ifdef MY_DEBUG
#undef MY_DISABLED_SERIAL
#endif

#include <MySensors.h>
#include "nrf.h"

// Function defentitions
void updateRelayState();
int batteryLevelByMv(int16_t);
int16_t nrf52ReadADC(uint8_t input, uint8_t resolution, uint8_t conversiotime, uint8_t refsel, uint8_t gain );

// State related variables
#define TEMP_SWITCH_TH 4    // hestersis in degreers to turn off and on boiler
#define TEMP_REPORT_TH 0.5  // minimal change to be reported to server
float setpointTemperature;
float reportedTemperature;
float measuredTemperature;
int   relayState=-1;
int   reportedBattery=-1;

// Temperature averaging buffer
#define TFA_SIZE    5     // size of floating average buffer
int tfa_idx=0,            // current index in circular buffer
    tfa_size=0;           // current size of buffer
float tfa_buf[TFA_SIZE];

int loop_counter=0;

// Message sent helpers
MyMessage msgHVACFlowState(1,V_HVAC_FLOW_STATE); bool sentHVACFlowState = true;
MyMessage msgHVACSetPoint(1,V_HVAC_SETPOINT_HEAT); bool sentHVACSetPoint = true;
MyMessage msgHVACTemperature(1,V_TEMP); bool sentHVACTemperature = true;
bool sentPresentation = true;
bool sentBatteryLevel = true;

void setup() {
#ifdef NODE_DEBUG  
  Serial.setPins(30,31);
  Serial.begin(115200);
  Serial.println("Started");
#endif
  pinMode(LED_OUT,OUTPUT);
  digitalWrite(LED_OUT,LOW);  
  pinMode(REL2_SET,OUTPUT);
  digitalWrite(REL2_SET,LOW);  
  pinMode(REL2_RES,OUTPUT);
  digitalWrite(REL2_RES,LOW);  
  pinMode(NTC_PWR2,OUTPUT);
  digitalWrite(NTC_PWR2,LOW); 

  // loading setting with checking signature
  if (loadState(0)==0xA1)
    setpointTemperature = loadState(1)+loadState(2)/100;  
  else {
    saveState(0,0xA1);
    setpointTemperature = 35;
    saveState(1,(int)setpointTemperature);
    saveState(2,((int)setpointTemperature*100)%100);
  }
  msgHVACSetPoint.set(setpointTemperature,1);
  
  analogReadResolution(14);
  analogReference(AR_VDD4);    
}

void presentation()  {
  sentPresentation = true;
  sentHVACFlowState = true;
  sentHVACSetPoint = true;
  sentHVACTemperature = true;
}

#define THERMISTORNOMINAL   10000   // resistance at 25 degrees C
#define RESISTORNOMINAL     10000   // resistor nominal (aka R1)     
#define TEMPERATURENOMINAL  25      // temp. for nominal resistance (almost always 25 C)   
#define BCOEFFICIENT        3950    // The beta coefficient of the thermistor (usually 3000-4000)

void before() {
  NRF_POWER->DCDCEN = 1;  
}

void loop() {
  // enable termometer and read state
  digitalWrite(NTC_PWR2,HIGH); 
  sleep(1,false);
  int16_t i= nrf52ReadADC(SAADC_CH_PSELP_PSELP_AnalogInput1, 
    SAADC_RESOLUTION_VAL_14bit, 
    SAADC_CH_CONFIG_TACQ_10us,
    SAADC_CH_CONFIG_REFSEL_VDD1_4,
    SAADC_CH_CONFIG_GAIN_Gain1_4);
  digitalWrite(NTC_PWR2,LOW); 

  float T;
  T = (RESISTORNOMINAL / (16383.0 / (float)i - 1.0)) / THERMISTORNOMINAL; // (R/Ro)
  T = log(T);                               // ln(R/Ro)
  T /= BCOEFFICIENT;                        // 1/B * ln(R/Ro)
  T += 1.0 / (TEMPERATURENOMINAL + 273.15); // + (1/To)
  T = 1.0 / T;                              // Invert
  T -= 273.15;                              // convert to C

#ifdef NODE_DEBUG
  Serial.print("Temperature: "); 
  Serial.print(T);
  Serial.println(" C");
#endif

  // update floating average buffer
  tfa_buf[tfa_idx++]=T;
  if (tfa_size<TFA_SIZE) tfa_size++;  // grow actual size untill allowed
  if (tfa_idx>=TFA_SIZE) tfa_idx=0;   // rest index to keep circular updates

  // calculate an average
  for (i=1,T=tfa_buf[0]; i<tfa_size; i++) T+=tfa_buf[i];
  T=((float)round((T/tfa_size)*10))/10;
  measuredTemperature = T;
  updateRelayState();

  // send updates with relativel significant change
  if (abs(T-reportedTemperature)>TEMP_REPORT_TH) {
    msgHVACTemperature.set(T,1);
    reportedTemperature=T;
    sentHVACTemperature = true;
  }

  // manage loop cycle and shortly blink every active loop
  if (!loop_counter++) {
    // lets sleep long enough to let battery to restore from anything
    sleep(1000,false);
    int batteryLevel = batteryLevelByMv(nrf52ReadADC(SAADC_CH_PSELP_PSELP_VDD, SAADC_RESOLUTION_VAL_14bit, SAADC_CH_CONFIG_TACQ_10us,SAADC_CH_CONFIG_REFSEL_Internal,SAADC_CH_CONFIG_GAIN_Gain1_6)*3600/16383);
    if (batteryLevel!=reportedBattery) {
      sentBatteryLevel = true;
      reportedBattery = batteryLevel;
    }
    digitalWrite(LED_OUT, HIGH);
    sleep(1,false);
    digitalWrite(LED_OUT, LOW);
  }
  if (loop_counter>ACTIVE_LOOP) loop_counter=0;

  sleep(isTransportReady()?LOOP_TIME:LOOP_NOREG,loop_counter==0?true:false);  

  // As a sleeping node we are receiving commands in last moment before going deep sleep
  // therefore sending from receive might not work all the time and therefore
  // we postponing it for better time (primary loop). In addition this ensures
  // sending of eveyrhitng on first loop
  if (sentPresentation) {
    sentPresentation = false;
    sendSketchInfo("Heater Thermostat", "1.1");
    present(1, S_HEATER);
  }
  if (sentHVACFlowState)  sentHVACFlowState = !send(msgHVACFlowState);
  if (sentHVACSetPoint) sentHVACSetPoint = !send(msgHVACSetPoint);
  if (sentHVACTemperature) sentHVACTemperature = !send(msgHVACTemperature);
  if (sentBatteryLevel) sentBatteryLevel = !sendBatteryLevel(reportedBattery);
}

void receive(const MyMessage &message) {
  // ignore ack messages (though we should't get them)
  if (message.isAck()) {
#ifdef NODE_DEBUG    
     Serial.println("This is an ack from gateway");
#endif
    return;
  }

  // trigger sending of current values upon request
  if (message.getCommand() == C_REQ) {
#ifdef NODE_DEBUG    
     Serial.println("We've been requested for values");
#endif    
    sentHVACTemperature = message.type == V_TEMP;
    sentHVACFlowState = message.type == V_HVAC_FLOW_STATE;
    sentHVACSetPoint = message.type == V_HVAC_SETPOINT_HEAT;
  }

  if (message.getCommand() == C_SET && message.type == V_HVAC_SETPOINT_HEAT) {
#ifdef NODE_DEBUG    
     Serial.println("Getting setpoint");
#endif
    // Receive and save setpoint
    setpointTemperature = message.getFloat();
    saveState(1,(int)setpointTemperature);
    saveState(2,((int)setpointTemperature*100)%100);
    updateRelayState();
    msgHVACSetPoint.set(setpointTemperature,1);

    // do we always sent accepted state or only if echo requested?
    // energy wise it should be only if requested
    if (mGetRequestAck(message))
      sentHVACSetPoint = true;
   }   
}

// helper to set output
void updateRelayState() {
  if ((measuredTemperature+TEMP_SWITCH_TH/2)<setpointTemperature && relayState!=1) 
  {
    digitalWrite(REL2_SET, HIGH);
    delay(50);           
    digitalWrite(REL2_SET, LOW);
    msgHVACFlowState.set("HeatOn");
    sentHVACFlowState = true;
    relayState = 1;
  }

  if ((measuredTemperature-TEMP_SWITCH_TH/2)>setpointTemperature && relayState!=0) {
    digitalWrite(REL2_RES, HIGH);
    delay(50);           
    digitalWrite(REL2_RES, LOW);
    msgHVACFlowState.set("Off");
    sentHVACFlowState = true;
    relayState = 0;
  }
}

// 3V discharge curve approximation
int batteryLevelByMv(int16_t mvolts) {
  int battery_level=0;

  mvolts+=30;   // correction

  if (mvolts>= 3000)
    battery_level = 100;
  else if (mvolts> 2900)
    battery_level = 100 - ((3000 - mvolts) * 58) / 100;
  else if (mvolts> 2740)
    battery_level = 42 - ((2900 - mvolts) * 24) / 160;
  else if (mvolts> 2440)
    battery_level = 18 - ((2740 - mvolts) * 12) / 300;
  else if (mvolts> 2100)
    battery_level = 6 - ((2440 - mvolts) * 6) / 340;
  else
    battery_level = 0;
  return battery_level;
}

int16_t nrf52ReadADC(uint8_t input, uint8_t resolution, uint8_t conversiotime, uint8_t refsel, uint8_t gain ) {
	int32_t sample;
	NRF_SAADC->ENABLE = SAADC_ENABLE_ENABLE_Enabled << SAADC_ENABLE_ENABLE_Pos;
	NRF_SAADC->RESOLUTION = resolution << SAADC_RESOLUTION_VAL_Pos;
	NRF_SAADC->CH[0].PSELP = input << SAADC_CH_PSELP_PSELP_Pos;
	NRF_SAADC->CH[0].CONFIG = (SAADC_CH_CONFIG_BURST_Disabled << SAADC_CH_CONFIG_BURST_Pos) |
	                          (SAADC_CH_CONFIG_MODE_SE << SAADC_CH_CONFIG_MODE_Pos) |
	                          (conversiotime << SAADC_CH_CONFIG_TACQ_Pos) |
	                          (refsel << SAADC_CH_CONFIG_REFSEL_Pos) |
	                          (gain << SAADC_CH_CONFIG_GAIN_Pos) |
	                          (SAADC_CH_CONFIG_RESN_Bypass << SAADC_CH_CONFIG_RESN_Pos) |
	                          (SAADC_CH_CONFIG_RESP_Bypass << SAADC_CH_CONFIG_RESP_Pos);
	NRF_SAADC->OVERSAMPLE = SAADC_OVERSAMPLE_OVERSAMPLE_Bypass << SAADC_OVERSAMPLE_OVERSAMPLE_Pos;
	NRF_SAADC->SAMPLERATE = SAADC_SAMPLERATE_MODE_Task << SAADC_SAMPLERATE_MODE_Pos;
	NRF_SAADC->RESULT.MAXCNT = 1;
	NRF_SAADC->RESULT.PTR = (uint32_t)&sample;

	NRF_SAADC->EVENTS_STARTED = 0;
	NRF_SAADC->TASKS_START = 1;
	while (!NRF_SAADC->EVENTS_STARTED);
	NRF_SAADC->EVENTS_STARTED = 0;

	NRF_SAADC->EVENTS_END = 0;
	NRF_SAADC->TASKS_SAMPLE = 1;
	while (!NRF_SAADC->EVENTS_END);
	NRF_SAADC->EVENTS_END = 0;

	NRF_SAADC->EVENTS_STOPPED = 0;
	NRF_SAADC->TASKS_STOP = 1;
	while (!NRF_SAADC->EVENTS_STOPPED);
	NRF_SAADC->EVENTS_STOPPED = 1;

	NRF_SAADC->ENABLE = (SAADC_ENABLE_ENABLE_Disabled << SAADC_ENABLE_ENABLE_Pos);

  return sample;
}