#define VERSION 	"1.2"

#include <mbed.h>
#include <ctype.h>
#include <stdio.h>
#include <string.h>
#include <millis.h>
#include "IO_Ports.h"
#include "MQTT_Com.h"
#include "WIZnetInterface.h"
#include "DS18B20Sensor.h"
#include "TEnergyMonitor/TEnergyMonitor.h"
//#include "MAX6675.h"

#define IP 					"10.0.2.251"
#define NETMASK				"255.255.255.0"
#define GATEWAY 			"10.0.2.1"
#define RECEIVE_TOPIC 		"emonhub/etcontroller/send"
#define SEND_TOPIC 			"emonhub/etcontroller/receive"

// General Functions Prototypes
void sendMessage(char*topic, char* msg);
void sendPoll(void);
void readCurrent(void);
void onMessage(MQTT::MessageData &msgMQTT);
void initEth(void);

/////////////// ETHERNET ///////////////////////////
extern int connack_rc;
uint8_t MAC[6]={0x58,0x72,0x2D,0xBF,0xB3,0x88};
SPI spi(MOSI, MISO, SCLK);
MQTTEthernet ipstack(MAC, IP, NETMASK, GATEWAY, &spi, CS, RST);
MQTT::Client<MQTTEthernet, Countdown, MQTT_MAX_PACKET_SIZE> client(ipstack);
/////////////// WIFI ///////////////////////////

/////////////// SYSTEM /////////////////////////
Serial pc(SERIAL_TX, SERIAL_RX);
DigitalOut myLed(LED1);
Ticker pollTimer, currentTimer;
BusOut relayOut(DO0,DO1,DO2,DO3,DO4,DO5,DO6,DO7);
char type,command;
int relayStatus = 0, relayCommand = 0, lastPower = 0;

/////////////// SYSTEM /////////////////////////

/////////////// GLOBAL VARS /////////////////////
char inMessage[MQTT_MAX_PAYLOAD_SIZE],outMessage[MQTT_MAX_PAYLOAD_SIZE],buf1[8],buf2[8],buf3[8];
bool newMessage=false, poll=false, rcurrent=true, DEBUG=true;
unsigned int i=0;
unsigned int pLevels[]={3500,0,2000,2500,3000};
/////////////// GLOBAL VARS /////////////////////

/////////////// SENSORS /////////////
DS18B20Sensor TempSens(D8);
TEnergyMonitor ct1(&pc);
/////////////// SENSORS /////////////

//////////////////////////////////////////////////////////////////
/////////////////////////   MAIN    //////////////////////////////
//////////////////////////////////////////////////////////////////

int main() {
	millisStart();
	pc.printf("%c[2J \tE&T Soft. Heatpump Power Controller and Heating System Monitoring V%s \n",27,VERSION);
	myLed=1;
	wait(2);
	initEth(); // Enables Network Connection to MQTT Server
    pc.printf("Subscribed to: %s, %d \r\n", RECEIVE_TOPIC, client.subscribe(RECEIVE_TOPIC, MQTT::QOS1, onMessage));
    pollTimer.attach(&sendPoll, 5);
    currentTimer.attach(&readCurrent, 5);
    TempSens.search();
    relayOut=255;
    ct1.current(A0,40);
//////////////////////////////////////////////////////////////////
///////////////////////// MAIN LOOP //////////////////////////////
//////////////////////////////////////////////////////////////////
    while (true){
    	client.yield(10);
        myLed=!myLed;
        if (newMessage) {
        	newMessage=false;
        	relayStatus = ~relayOut.read();
        	int rv = sscanf(inMessage,"%c:%d",&type,&command);
        	//pc.printf("sscanf:%d - %c,%d\n", rv,type,command);
        	if (rv <= 1 || rv == EOF)
        		sprintf(outMessage,"NAK");
        	else {
        		switch (type) {
        		case 'P':
        			if(command >= 1 && command <= 4) {
        				relayCommand = (relayStatus & 0x0070) | (1 << (command-1));
        			}
        			else if (command == 0) {
        				relayCommand = (relayStatus & 0x0070);
        			}
        			//lastPower=pLevels[command];
        			sprintf(outMessage,"RL_O:%02X",relayCommand);
        			relayOut=~relayCommand;
        			break;

        		case 'R':
        			if(command>=0 && command<=7){
        				relayCommand = (relayStatus & 0x000F) | (command << 4);
        				sprintf(outMessage,"RL_O:%02X",relayCommand);
        				relayOut=~relayCommand;
        			}
        			break;

        		case 'S':
        			sprintf(outMessage,"RL_O:%02X",relayCommand);
        			break;

        		case 'T':
        			TempSens.startReading(true);
        			TempSens.getReading(buf1,0);
        			TempSens.getReading(buf2,1);
        			TempSens.getReading(buf3,2);
        			sprintf(outMessage,"WT_T:%s;SL_T:%s;HT_T:%s",buf2,buf3,buf1);
        			break;

        		case 'H':
        			sprintf(outMessage,"HP_W:%d",lastPower);
        			break;

        		default:
        			break;
        		}
        	}
        	sendMessage(SEND_TOPIC,outMessage);
        }
        if (poll){
        	poll=false;
        	sprintf(outMessage,"POLL [%d]",i++);
        	sendMessage(SEND_TOPIC, outMessage);
        	client.yield(10);
        }
        if (rcurrent){
        	rcurrent=false;
        	lastPower=(int)((ct1.calcIrms(2)-0.08)*235);
        	if(DEBUG) pc.printf("current:%1.3f, power:%d\n",ct1.calcIrms(2)-0.08,lastPower);
        }
        wait(1);
    }
}

//////////////////////////////////////////////////////////////////
////////////////////// GENERAL FUNCTS ////////////////////////////
//////////////////////////////////////////////////////////////////


//////////////////////////////////////////////////////////////////
////////////////////// ETHERNET FUNCTIONS ////////////////////////
//////////////////////////////////////////////////////////////////
void onMessage(MQTT::MessageData &msgMQTT) {
	strncpy(inMessage,(char*)msgMQTT.message.payload,(uint)msgMQTT.message.payloadlen);
	inMessage[(uint)msgMQTT.message.payloadlen]='\0';
    newMessage=true;
    if (DEBUG) pc.printf("Received: %s\n",inMessage);
}

void sendMessage(char*topic,char* msg){
	if (DEBUG) pc.printf("Sent: %s\n", msg);
	publish(&client, &ipstack, topic, msg);
}

void sendPoll(void){
	poll=true;
}

void readCurrent(void){
	rcurrent=true;
}

void initEth (void){
    pc.printf("\nConnecting to ETH\n");
    attemptConnect(&client, &ipstack);
   if (connack_rc == MQTT_NOT_AUTHORIZED || connack_rc == MQTT_BAD_USERNAME_OR_PASSWORD){
      while (true){
            wait(1.0); // Permanent failures - don't retry
            pc.printf("\nMQTT_NOT_AUTHORIZED or BAD_USERNAME_OR_PASSWORD\n");
      }
   }   
}