#include <Arduino_MKRIoTCarrier.h>
#include <WiFiNINA.h>
#include <ArduinoMqttClient.h>

#include "secrets.h"

MKRIoTCarrier carrier;
WiFiSSLClient wifi_client;
MqttClient mqtt_client(wifi_client);

// Motor pins
int motor_pin1 = 8;  // Blue   - 28BYJ48 pin 1
int motor_pin2 = 9;  // Pink   - 28BYJ48 pin 2
int motor_pin3 = 10; // Yellow - 28BYJ48 pin 3
int motor_pin4 = 11; // Orange - 28BYJ48 pin 4

// Motor variables
int motor_speed = 1200;
int motor_count = 0;
int motor_count_max = 256;
int motor_lookup[8] = {B01000, B01100, B00100, B00110, B00010, B00011, B00001, B01001};

void setup(void)
{
	Serial.begin(9600);

	Serial.println("Started");

	carrier.begin();
	carrier.display.init(240, 240);

	pinMode(TFT_BACKLIGHT, OUTPUT);
	digitalWrite(TFT_BACKLIGHT, HIGH);

	carrier.display.setCursor(20, 100);
	carrier.display.setTextSize(3);
	carrier.display.print("Connecting..");

	Serial.println("Connecting to WiFi...");

	if (wifi_enterprise) WiFi.beginEnterprise(wifi_ssid, wifi_username, wifi_password);
	else WiFi.begin(wifi_ssid, wifi_password);

	while (WiFi.status() != WL_CONNECTED) delay(1000);

	Serial.println("Connected to WiFi, connecting to MQTT...");

	mqtt_client.setUsernamePassword(mqtt_username, mqtt_password);
	if (!mqtt_client.connect(mqtt_server, 8883)) {
		Serial.print("MQTT connection failed with error code: ");
		Serial.println(mqtt_client.connectError());
		return;
	}

	Serial.println("Connected to MQTT");

	mqtt_client.subscribe("dispense");

	carrier.display.fillScreen(0x000);

  //pinouts til motor
  pinMode(motor_pin1, OUTPUT);
  pinMode(motor_pin2, OUTPUT);
  pinMode(motor_pin3, OUTPUT);
  pinMode(motor_pin4, OUTPUT);
}

void loop(void)
{
  // Rotate motor if needed
  if (motor_count) {
    motor_clockwise();
    motor_count--;
  }

  // Receive MQTT messages
	int message_size = mqtt_client.parseMessage();
	if (message_size) {
		Serial.print("Got message with topic: ");
		Serial.println(mqtt_client.messageTopic());

		if (mqtt_client.messageTopic() == "dispense") {
			Serial.println("Dispensing...");

			// Start rotating motor
      motor_count = motor_count_max;
		}
	}
}

void motor_clockwise()
{
  for (int i = 7; i >= 0; i--) {
    motor_set_output(i);
    delayMicroseconds(motor_speed);
  }
}
void motor_anticlockwise()
{
  for (int i = 0; i < 8; i++) {
    motor_set_output(i);
    delayMicroseconds(motor_speed);
  }
}

void motor_set_output(int out)
{
  digitalWrite(motor_pin1, bitRead(motor_lookup[out], 0));
  digitalWrite(motor_pin2, bitRead(motor_lookup[out], 1));
  digitalWrite(motor_pin3, bitRead(motor_lookup[out], 2));
  digitalWrite(motor_pin4, bitRead(motor_lookup[out], 3));
}