#include "WiFi.h"
#include "AsyncUDP.h"


#include <Pixy2I2C.h>
Pixy2I2C pixy;


const char * ssid = "Module-xxx-00";  /// Module Credentials for Wifi Connection
const char * password = "zirokits";

const double pixy_x = 315 / 2;
const double pixy_y = 207 / 2;
int cntr = 0;

bool WifiConnected;
AsyncUDP udp;

void setup()
{


  Serial.begin(19200);

  while (WifiConnected == false && cntr < 10) {
    WiFi.mode(WIFI_STA);
    WiFi.begin(ssid, password);
    if (WiFi.waitForConnectResult() != WL_CONNECTED) {
      WifiConnected = false;
      Serial.println("WiFi Failed");
    }
    cntr++;
  }
  if (udp.listen(2325)) {
    Serial.print("UDP Listening on IP: ");
    WifiConnected = true;
    Serial.println(WiFi.localIP());
    udp.onPacket([](AsyncUDPPacket packet) {
      Serial.print("UDP Packet Type: ");
      Serial.print(packet.isBroadcast() ? "Broadcast" : packet.isMulticast() ? "Multicast" : "Unicast");
      Serial.print(", From: ");
      Serial.print(packet.remoteIP());
      Serial.print(":");
      Serial.print(packet.remotePort());
      Serial.print(", To: ");
      Serial.print(packet.localIP());
      Serial.print(":");
      Serial.print(packet.localPort());
      Serial.print(", Length: ");
      Serial.print(packet.length());
      Serial.print(", Data: ");
      Serial.write(packet.data(), packet.length());
      Serial.println();
      //reply to the client
      packet.printf("Got %u bytes of data", packet.length());
    });
  }


  pixy.init();
}

void loop()
{
  if (WifiConnected) {

    PixyLoop();
  }
}

void PixyLoop() {

  int i;

  pixy.ccc.getBlocks();

  // If there are detect blocks, print them!
  if (pixy.ccc.numBlocks)
  {
    Serial.print("Detected ");
    Serial.println(pixy.ccc.numBlocks);
    int max_w = 0;
    int ball;
    for (int i = 0; i < pixy.ccc.numBlocks; i++) {
      if (pixy.ccc.blocks[i].m_width > max_w) {
        max_w = pixy.ccc.blocks[i].m_width;
        ball = i;
      }
    }
    if (pixy.ccc.blocks[ball].m_height < 70) {
      Serial.print("  block ");
      pixy.ccc.blocks[ball].print();
      Block block1 = pixy.ccc.blocks[ball];
      int x = block1.m_x;
      int y = block1.m_y;
      int turn_x = pixy_x - x;

      if (x > pixy_x + 40) {
        udp.broadcast("{""pType"":7,""m_T"":[45,45,0,0,0,0,0],""m_M"":[2,2,0,0,0,0,0]}");
      } else if (x < pixy_x - 40) {
        udp.broadcast("{""pType"":7,""m_T"":[45,45,0,0,0,0,0],""m_M"":[1,1,0,0,0,0,0]}");
      } else {
        udp.broadcast("{""pType"":7,""m_T"":[55,55,0,0,0,0,0],""m_M"":[1,2,0,0,0,0,0]}");
      }
    } else {
      udp.broadcast("{""pType"":7,""m_T"":[0,0,0,0,0,0,0],""m_M"":[0,0,0,0,0,0,0]}");
    }

  } else {
    udp.broadcast("{""pType"":7,""m_T"":[0,0,0,0,0,0,0],""m_M"":[0,0,0,0,0,0,0]}");
  }
}