#include <WiFi.h>
#include "Adafruit_MQTT.h"
#include "Adafruit_MQTT_Client.h"
#include <Adafruit_NeoPixel.h>

#define FLEX_SENSOR1 32
#define FLEX_SENSOR2 34
#define FLEX_SENSOR3 35
#define FLEX_SENSOR4 36
#define FLEX_SENSOR5 39
#define AIO_SERVER            "io.adafruit.com"
#define AIO_SERVERPORT     1883
#define AIO_USERNAME  "KfirHB12"
#define AIO_KEY       "aio_KIqI85WFBmhc6r9SS0EvY66Hsprz"
#define AIO_FEED_FINGER1 "/feeds/fingers.finger1"
#define AIO_FEED_FINGER2 "/feeds/fingers.finger2"
#define AIO_FEED_FINGER3 "/feeds/fingers.finger3"
#define AIO_FEED_FINGER4 "/feeds/fingers.finger4"
#define AIO_FEED_FINGER5 "/feeds/fingers.finger5"
#define NO_FINGERS 5
#define WLAN_SSID "Wokwi-GUEST"
#define WLAN_PASS ""
//KfirHB12/feeds/fingers.finger1
#define LED1 19
#define LED2 18
#define LED3 5
#define LED4 17
#define LED5 16
#define NUMLEDS 16
//כל 340
Adafruit_NeoPixel pixelsArray[NO_FINGERS] = {
    Adafruit_NeoPixel(NUMLEDS, LED1, NEO_GRB + NEO_KHZ800),
    Adafruit_NeoPixel(NUMLEDS, LED2, NEO_GRB + NEO_KHZ800),
    Adafruit_NeoPixel(NUMLEDS, LED3, NEO_GRB + NEO_KHZ800),
    Adafruit_NeoPixel(NUMLEDS, LED4, NEO_GRB + NEO_KHZ800),
    Adafruit_NeoPixel(NUMLEDS, LED5, NEO_GRB + NEO_KHZ800)
};
static const uint32_t RED = pixelsArray[1].Color(0xFF, 0x00, 0x00);
static const uint32_t BLUE = pixelsArray[1].Color(0x00, 0x00, 0xFF);
static const uint32_t GREEN = pixelsArray[1].Color(0x00, 0xFF, 0x00);
static const uint32_t YELLOW = pixelsArray[1].Color(0xFF, 0xFF, 0x00);
static const uint32_t ORANGE = pixelsArray[1].Color(0xFF, 0x99, 0x33);
//Adafruit_NeoPixel pixles[NO_FINGERS] = {

//}

WiFiClient client;
Adafruit_MQTT_Client mqtt(&client, AIO_SERVER, AIO_SERVERPORT, AIO_USERNAME, AIO_KEY); //mqtt client
Adafruit_MQTT_Publish sendData[NO_FINGERS] = {
  Adafruit_MQTT_Publish(&mqtt, AIO_USERNAME AIO_FEED_FINGER1),
  Adafruit_MQTT_Publish(&mqtt, AIO_USERNAME AIO_FEED_FINGER2),
  Adafruit_MQTT_Publish(&mqtt, AIO_USERNAME AIO_FEED_FINGER3),
  Adafruit_MQTT_Publish(&mqtt, AIO_USERNAME AIO_FEED_FINGER4),
  Adafruit_MQTT_Publish(&mqtt, AIO_USERNAME AIO_FEED_FINGER5)
};
struct Flex{
  int Val;
  bool changed;
};
Flex flexValues[NO_FINGERS];

int ledstrip[NO_FINGERS];
void setup() {
  	serial_init();
    connectToWifi();
  	flex_init(); 
    init_leds();

    delay(3000);
}
void loop()
{
  	//test_all();
    main_logic();
}
void main_logic()
{
  MQTTconnect();
  update_flex_array();
  for(int i = 0; i<NO_FINGERS; i++){
    if(flexValues[i].changed){
      update_iot(i);
      write_led(i,flexValues[i].Val);
      flexValues[i].changed = false;
    }
  }
  if (!mqtt.ping())  {
      mqtt.disconnect();
  }
}
/// TEST
void test_all()
{
  	//flex_test();
    test_leds ();
    delay (1000);
  //test_MQTT();
}
/// SERIAL
void serial_init()
{
  	Serial.begin(9600);
}
/// FLEX_SENSOR
void flex_init()
{
  pinMode(FLEX_SENSOR1, INPUT);
  pinMode(FLEX_SENSOR2, INPUT);
  pinMode(FLEX_SENSOR3, INPUT);
  pinMode(FLEX_SENSOR4, INPUT);
  pinMode(FLEX_SENSOR5, INPUT);
  for(int i = 0; i<NO_FINGERS; i++){
    flexValues[i].Val = 0;
    flexValues[i].changed = true;
  }
}
void flex_test()
{
  for (int i = 0; i<NO_FINGERS; i++)
  {
    Serial.print("Flex sensor no. ");
    Serial.print(i);
    Serial.print(" ");
  	Serial.println(read_flex(i));
  }
}
            
int read_flex(int num)
{
  
  switch (num){ 
    case 0:
    	return analogRead(FLEX_SENSOR1);
      	break;
	case 1:
    	return analogRead(FLEX_SENSOR2);
      	break;
    case 2:
    	return analogRead(FLEX_SENSOR3);
      	break;
    case 3:
    	return analogRead(FLEX_SENSOR4);
      	break;
    case 4:
    	return analogRead(FLEX_SENSOR5);
      	break;
  }
}

void update_flex_array(){
  int flex;
  for(int k=0; k<NO_FINGERS; k++ ){
    flex = read_flex(k);
    if( flexValues[k].Val !=flex){
      flexValues[k].Val = flex;
      flexValues[k].changed = true;
    }
  }
}

//leds
void init_leds(){
  for (int i=0; i<NO_FINGERS; i++)
  {
    pixelsArray[i].begin();
  }
}
void test_leds (){
  for (int i=0; i<NO_FINGERS; i++ )
  {
    for (int j=0; j<NUMLEDS; j++)
    {
      pixelsArray[i].clear();
      pixelsArray[i].setPixelColor(j, RED);
      pixelsArray[i].show();
    }
  }
  
}
void write_led(int num, int max_val)
{
  pixelsArray[num].clear();
  int val = map(max_val, 0, 4095, 0, NUMLEDS);
  for (int j=0; j<=val; j++)
    {
      Serial.println("write led val=");
      Serial.println(val);
      pixelsArray[num].setPixelColor(j, RED);
      pixelsArray[num].show();
    }
}
void test_write_led()
{
  write_led(0,1000);
  write_led(1,4000);
}

/// WIFI
void connectToWifi()
{
  Serial.print("Connecting to ");
  Serial.println(WLAN_SSID);
  WiFi.begin(WLAN_SSID, WLAN_PASS);
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }
  Serial.println();
  Serial.println("WiFi connected");
  Serial.println("IP address: "); 
  Serial.println(WiFi.localIP());
}
/// MQTT
void MQTTconnect() {
  if (mqtt.connected()) {
    return;
  }
  
  Serial.println("Connecting to MQTT server");
  uint8_t retries = 3;
  int8_t  ret;
  while ((ret = mqtt.connect()) != 0) 
  {
       Serial.println(mqtt.connectErrorString(ret));
       Serial.println("Retrying MQTT connection in 5 seconds");
       mqtt.disconnect();
       delay(5000); 
       retries--;
       if (retries == 0) {
         Serial.println("Connection failed");
         while (1);
       }
  }
  Serial.println("Connected to MQTT server");
}
void test_MQTT() {
  update_iot(1);
}
void update_iot(int finger)
{
 if (sendData[finger].publish(static_cast<uint32_t>(flexValues[finger].Val)))
    Serial.println("Finger " + String(finger+1) + " " + String(flexValues[finger].Val));
}