#include "WiFi.h"
#include "AsyncUDP.h"
#include "wifi_config.h"
AsyncUDP udp;

int bindex = 0;

#include <ESP32Servo.h>
struct ServoPin {
  Servo servo;
  int pin;
};

ServoPin servos[6] = {
  {Servo(), 14}, {Servo(), 5},
  {Servo(), 12}, {Servo(), 4},
  {Servo(), 13}, {Servo(), 2} // 19 and 21 are untested
};

// braille alphabet
// example:
// *1 = 180 degrees
// a (0 index)
// 10
// 00
// 00
// braille alphabet array
struct BrailleChar {
  int servoStates[6];
};
BrailleChar brailleAlphabet[26] = {
  {{1, 0,
    0, 0,
    0, 0
  }}, // a
  {{1, 0,
    1, 0,
    0, 0
  }}, // b
  {{1, 1,
    0, 0,
    0, 0
  }}, // c
  {{1, 1,
    0, 1,
    0, 0
  }}, // d
  {{1, 0,
    0, 1,
    0, 0
  }}, // e
  {{1, 1,
    1, 0,
    0, 0
  }}, // f
  {{1, 1,
    1, 1,
    0, 0
  }}, // g
  {{1, 0,
    1, 1,
    0, 0
  }}, // h
  {{0, 1,
    1, 0,
    0, 0
  }}, // i
  {{0, 1,
    1, 1,
    0, 0
  }}, // j
  {{1, 0,
    0, 0,
    1, 0
  }}, // k
  {{1, 0,
    1, 0,
    1, 0
  }}, // l
  {{1, 1,
    0, 0,
    1, 0
  }}, // m
  {{1, 1,
    0, 1,
    1, 0
  }}, // n
  {{1, 0,
    0, 1,
    1, 0
  }}, // o
  {{1, 1,
    1, 0,
    1, 0
  }}, // p
  {{1, 1,
    1, 1,
    1, 0
  }}, // q
  {{1, 0,
    1, 1,
    1, 0
  }}, // r
  {{0, 1,
    1, 0,
    1, 0
  }}, // s
  {{0, 1,
    1, 1,
    1, 0
  }}, // t
  {{1, 0,
    0, 0,
    1, 1
  }}, // u
  {{1, 0,
    1, 0,
    1, 1
  }}, // v
  {{0, 1,
    1, 1,
    0, 1
  }}, // w
  {{1, 1,
    0, 0,
    1, 1
  }}, // x
  {{1, 1,
    0, 1,
    1, 1
  }}, // y
  {{1, 0,
    0, 1,
    1, 1
  }} // z
};

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

  WiFi.disconnect(true);
  WiFi.mode(WIFI_STA);
  WiFi.begin(ssid, pass);
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
		// and print status
		Serial.println(WiFi.status());
  }
  if (udp.listen(1234)) {
    Serial.print("UDP Listening on IP: ");
    Serial.println(WiFi.localIP());
    udp.onPacket([](AsyncUDPPacket packet) {
      Serial.print(", Length: ");
      Serial.print(packet.length()); //dlzka packetu
      Serial.print(", Data: ");
      // Serial.write(packet.data(), packet.length());
      // that is printing a character instead of a number
      // so do this instead
      // Serial.print(packet.data()[0]);
      // and set the bindex to that value
      bindex = packet.data()[0];
      Serial.print(bindex);
      Serial.println();
      uint32_t receivedData;
      memcpy(&receivedData, packet.data(), sizeof(receivedData));
      packet.printf("Got %u bytes of data", packet.length());
    });
  }

  // Allow allocation of all timers
	ESP32PWM::allocateTimer(0);
	ESP32PWM::allocateTimer(1);
	ESP32PWM::allocateTimer(2);
	ESP32PWM::allocateTimer(3);
	// myservo.setPeriodHertz(50);    // standard 50 hz servo
	// myservo.attach(servoPin, 1000, 2000); // attaches the servo on pin x to the servo object
	// using default min/max of 1000us and 2000us
	// different servos may require different min/max settings 
	// for an accurate 0 to 180 sweep
  for (int i = 0; i < 6; i++) {
    servos[i].servo.setPeriodHertz(50);
    servos[i].servo.attach(servos[i].pin, 1000, 2000);
  }
}

void loop()
{
  // delay(1000);
  // udp.broadcast("Anyone here?");

  // for (pos = 0; pos <= 180; pos += 1) { // goes from 0 degrees to 180 degrees
	// 	// in steps of 1 degree
	// 	myservo.write(pos);    // tell servo to go to position in variable 'pos'
	// 	delay(15);             // waits 15ms for the servo to reach the position
	// }
	// for (pos = 180; pos >= 0; pos -= 1) { // goes from 180 degrees to 0 degrees
	// 	myservo.write(pos);    // tell servo to go to position in variable 'pos'
	// 	delay(15);             // waits 15ms for the servo to reach the position
	// }

  // myservo.write(bindex);
  for (int i = 0; i < 6; i++) {
    servos[i].servo.write(brailleAlphabet[bindex].servoStates[i] * 180);
  }
}