#include <ESP8266WiFi.h>
extern "C" {
#include "user_interface.h"
}

// motors vars
#define m1o1 5
#define m1o2 4
#define m1o3 0
#define m1o4 2
#define m2o1 16
#define m2o2 14
#define m2o3 12
#define m2o4 13
// wifi vars
#define ssid "robots_flock"
#define password "qwerfdsa"
#define serverIP "192.168.10.200"

os_timer_t myTimer;
volatile int motorState[] = {0,0};
volatile bool rotating[] = {true,true};
volatile bool rot_direction[] = {false,true};
volatile int motorPinsState[] = {0,0,0,0};



int getStepFull(int motor = 0) {
  // full step mode
  switch(motorState[motor]) {
    case 0:
      motorPinsState[0] = HIGH;
      motorPinsState[1] = HIGH;
      motorPinsState[2] = LOW;
      motorPinsState[3] = LOW;
      break;
    case 1:
      motorPinsState[0] = LOW;
      motorPinsState[1] = HIGH;
      motorPinsState[2] = HIGH;
      motorPinsState[3] = LOW;
      break;
    case 2:
      motorPinsState[0] = LOW;
      motorPinsState[1] = LOW;
      motorPinsState[2] = HIGH;
      motorPinsState[3] = HIGH;
      break;
    case 3:
      motorPinsState[0] = HIGH;
      motorPinsState[1] = LOW;
      motorPinsState[2] = LOW;
      motorPinsState[3] = HIGH;
      break;
  }
  motorState[motor] += rot_direction[motor] ? 1 : -1;
  if (motorState[motor] == 4) motorState[motor] = 0;
  if (motorState[motor] == -1) motorState[motor] = 3;
}

void getStepHalf(int motor = 0) {
  // half step mode (2x slower)
  switch(motorState[motor]) {
    case 0:
      motorPinsState[0] = HIGH;
      motorPinsState[1] = LOW;
      motorPinsState[2] = LOW;
      motorPinsState[3] = LOW;
      break;
    case 1:
      motorPinsState[0] = HIGH;
      motorPinsState[1] = HIGH;
      motorPinsState[2] = LOW;
      motorPinsState[3] = LOW;
      break;
    case 2:
      motorPinsState[0] = LOW;
      motorPinsState[1] = HIGH;
      motorPinsState[2] = LOW;
      motorPinsState[3] = LOW;
      break;
    case 3:
      motorPinsState[0] = LOW;
      motorPinsState[1] = HIGH;
      motorPinsState[2] = HIGH;
      motorPinsState[3] = LOW;
      break;
    case 4:
      motorPinsState[0] = LOW;
      motorPinsState[1] = LOW;
      motorPinsState[2] = HIGH;
      motorPinsState[3] = LOW;
      break;
    case 5:
      motorPinsState[0] = LOW;
      motorPinsState[1] = LOW;
      motorPinsState[2] = HIGH;
      motorPinsState[3] = HIGH;
      break;
    case 6:
      motorPinsState[0] = LOW;
      motorPinsState[1] = LOW;
      motorPinsState[2] = LOW;
      motorPinsState[3] = HIGH;
      break;
    case 7:
      motorPinsState[0] = HIGH;
      motorPinsState[1] = LOW;
      motorPinsState[2] = LOW;
      motorPinsState[3] = HIGH;
      break;
  }

  motorState[motor] += rot_direction[motor] ? 1 : -1;
  if (motorState[motor] == 8) motorState[motor] = 0;
  if (motorState[motor] == -1) motorState[motor] = 7;
}

void timer(void *pArg) {
  // get steps for 1 motor
  if(rotating[0]) {
    getStepFull(0);
    digitalWrite(m1o1, motorPinsState[0]);
    digitalWrite(m1o2, motorPinsState[1]);
    digitalWrite(m1o3, motorPinsState[2]);
    digitalWrite(m1o4, motorPinsState[3]);
  }
  // get steps for 2 motor
  if(rotating[1]) {
    getStepFull(1);
    digitalWrite(m2o1, motorPinsState[0]);
    digitalWrite(m2o2, motorPinsState[1]);
    digitalWrite(m2o3, motorPinsState[2]);
    digitalWrite(m2o4, motorPinsState[3]);
  }
}

void connectWiFi() {
  WiFi.begin(ssid, password);

  Serial.printf("Connecting to %s", ssid);
  while (WiFi.status() != WL_CONNECTED)
  {
    delay(500);
    Serial.print(".");
  }

  Serial.print("\nConnected, IP address: ");
  Serial.println(WiFi.localIP());

}


void setup() {
  pinMode(m1o1, OUTPUT); //to m1o1
  pinMode(m1o2, OUTPUT); //to m1o2
  pinMode(m1o3, OUTPUT); //to m1o3
  pinMode(m1o4, OUTPUT); //to m1o4

  pinMode(m2o1, OUTPUT);
  pinMode(m2o2, OUTPUT);
  pinMode(m2o3, OUTPUT);
  pinMode(m2o4, OUTPUT);

// https://www.switchdoc.com/2015/10/iot-esp8266-timer-tutorial-arduino-ide/
  os_timer_setfn(&myTimer, timer, NULL);
  os_timer_arm(&myTimer, 2, true); // 2ms, repeated
  // only ONE timer :(
  ///
// found out that the use of the os_timer set to a 2ms
// interrupt will cause the WiFi to fail.  We are investigating the cause,
// but right now it definitely kills the WiFi connection and will not reconnect.

  Serial.begin(115200);
  Serial.println();
  connectWiFi();
}

void loop() {
  WiFiClient client;

  Serial.printf("\nConnecting to %s:1337 ... ", serverIP);
  if (client.connect(serverIP, 1337))
  {
    Serial.println("connected");

    while (client.connected() || client.available())
    {
      if (client.available())
      {
        String line = client.readStringUntil('\n');
        rotating[0] = line[1] != 48; // != '0'
        rot_direction[0] = line[1] == 102; // == 'f'
        rotating[1] = line[3] != 48; // != '0'
        rot_direction[1] = line[3] == 102; // == 'f'
        Serial.printf("rot1:%d, rotdir1:%d\n", rotating[0], rot_direction[0]);
      }
    }
    client.stop();
    Serial.println("\nDisconnected");
  }
  else
  {
    Serial.println("connection failed!");
    client.stop();
  }
  delay(2000);
}