// Copyright (c) 2021 David G. Young
// Copyright (c) 2015 Damian Kołakowski. All rights reserved.
// cc servo.c -lmosquitto -l bcm2835 -o servo


#include <stdlib.h>
#include <errno.h>
#include <unistd.h>
#include <signal.h>
#include <sys/ioctl.h>
#include <sys/socket.h>
#include <time.h>

#include <stdio.h>
#include <bcm2835.h>
#include <string.h>
#include <ctype.h>

#include <mosquitto.h>


// MQTT broker settings
struct mosquitto *mosq;
char *Host = "127.0.0.1";
int Port = 1883;
const char *USERNAME = "gataki";
const char *PASSWORD = "gataki";

// servo settings
int pinA = 18;
int pinB = 23;
int prevangle=0;

int RSSI = 0;
int levelSignal = -60;

/* Callback function that will be called when a message is received. */
void on_message(struct mosquitto *mosq, void *userdata, const struct mosquitto_message *msg) {
    char *topic="gatakia/RSSI";
    printf("Received message on topic %s: %s\n", msg->topic, (char*)msg->payload);
    if ( strcmp(msg->topic, topic) == 0) {
		RSSI = atoi(msg->payload);
		//printf("RSSI: %d \n", RSSI);
	}
}

void on_connect(struct mosquitto *mosq, void *userdata, int rc)
{
    if (rc == 0) {
		printf("Connect ok \n");    
        mosquitto_publish(mosq, NULL, "gatakia/servo", 9, "listening", 0, false);
        rc = mosquitto_subscribe(mosq, NULL, "gatakia/RSSI", 0); 
        if (rc!=MOSQ_ERR_SUCCESS) {
			printf("Error on subscribe");
		}     

    } else {
        printf("Connect failed: %s\n", mosquitto_strerror(rc));
	// Attempt to reconnect
        mosquitto_reconnect(mosq);
    }
}


int init_mqtt() {
    int rc;

    mosquitto_lib_init();

    mosq = mosquitto_new("user65424", true, NULL);

	mosquitto_connect_callback_set(mosq, on_connect);
	mosquitto_message_callback_set(mosq, on_message);
	
    rc = mosquitto_connect(mosq, "127.0.0.1", 1883, 60);
    if (rc != MOSQ_ERR_SUCCESS) {
        printf("Error: Unable to connect to MQTT broker: %s\n", mosquitto_strerror(rc));
        mosquitto_destroy(mosq);
        mosquitto_lib_cleanup();
        return 1;
    }
    // Set the username and password for the connection
    mosquitto_username_pw_set(mosq, "gataki", "gataki");

}


// Αποστολη μνμ σε MQTT broker
void send_msg( char *s)
{
	//char str[16]; // Allocate enough space for the converted string
	
    //sprintf(str, "%s \n", s); // %d is the format specifier for integers

    printf("->>>> %s \n", s);
	int ret = mosquitto_publish(mosq, NULL, "gatakia/servo", strlen(s), s, 0, false);
	if (ret>0) printf("Send error %d \n",ret);
	
}

void moveServos(int angle, int delayTime) {
	int j;
	float hightime = 10.3*(float)angle + 546;
	int pulse = (int)abs(((angle - prevangle)*33)/180) ;
	for(j=0; j<pulse; j++)//exucting pulses
        {
	bcm2835_gpio_set(pinA);//pin18 high
    bcm2835_delayMicroseconds(hightime);
    bcm2835_gpio_clr(pinA);//pin18 low
    bcm2835_delayMicroseconds(20000 - hightime);//each pulse is 20ms
    
    bcm2835_gpio_set(pinB);//pin18 high
    bcm2835_delayMicroseconds(hightime);
    bcm2835_gpio_clr(pinB);//pin18 low
    bcm2835_delayMicroseconds(20000 - hightime);//each pulse is 20ms
    
    delay(delayTime);
	}
	prevangle = angle;
}

/*
void check_dist(int rssi) {
	//printf(rssi);
    if ( rssi < -60) {
	moveServos(80, 250); //align to 0 degree point
    } else 
    {
	moveServos(0, 250); //align to 0 degree point
    }
}
void check_dist(int rssi) {
	//printf(rssi);
    if ( rssi < levelSignal) {
	if (stateOpened == 0) {    
	send_msg("open");
	stateOpened = 1;
	}
	//moveServos(80, 250); //align to 0 degree point
    } else 
    {
	if (stateOpened == 1) {
	send_msg("close");
	stateOpened = 0;
	}
    }
}
*/

int main()
{
    	
    // προετοιμασία servo
    if(!bcm2835_init()) return 1;
    int angleA, prevangleA, pulse;

    bcm2835_gpio_fsel(18, BCM2835_GPIO_FSEL_OUTP); //set pin 18 as output
    bcm2835_gpio_fsel(23, BCM2835_GPIO_FSEL_OUTP); //set pin 23 as output
    
   
    // δοκιμαστική κίνηση Servo
    moveServos(40, 0);
    moveServos(0, 0);
    
    int rc = init_mqtt();
    mosquitto_loop_forever(mosq,-1,1);
    
}