#include <WiFiNINA.h>
#include "arduino_secrets.h"
#include "AccelStepper.h"

// Define motor controller connections and motor interface type (TMC2208)
#define dirPinA 2
#define stepPinA 3
#define enPinA 4

#define dirPinB 5
#define stepPinB 6
#define enPinB 7

#define TMC2208 1

// Define endstops
#define endstopA 8 
#define endstopB 9

//Open and Closed Positions
int closedPosition = 52000; //Closed position, in number of steps from home.  Needs to be measured manually
int openPosition = 0; //Open position (home)

//Define Movement Speeds (note higher speeds will be louder)
int blindSpeed = 2000;
int blindAcceleration = 2000;

//Define assigned IP address
IPAddress ip(192, 168, 1, 202);

// Create control variables
long homePosA=-1;  // Used to home motor A
long homePosB=-1;  // Used to home motor B
int completedHome = 0; //checks if a home was completed before allowing open and close actions
int target = openPosition; //target variable passed from HomeBridge
int state = 2; //0-moving, 2-idling

// Create motor instances in AccelStepper
AccelStepper stepperA = AccelStepper(TMC2208, stepPinA, dirPinA);
AccelStepper stepperB = AccelStepper(TMC2208, stepPinB, dirPinB);

// Create Server
char ssid[] = SECRET_SSID;             //  your network SSID (name) between the " "
char pass[] = SECRET_PASS;      // your network password between the " "
int keyIndex = 0;                 // your network key Index number (needed only for WEP)
int status = WL_IDLE_STATUS;      //connection status
WiFiServer server(80);            //server socket
WiFiClient client = server.available();

void setup() {
  
  //Begin serial connection
  Serial.begin(9600);  //Begin serial connection

  // Setup endstop inputs
  pinMode(endstopA, INPUT_PULLUP);
  pinMode(endstopB, INPUT_PULLUP);
  
  //Setup motor driver enable pins
  pinMode(enPinA, OUTPUT);
  pinMode(enPinB, OUTPUT);

  //Set movement speeds
  stepperA.setMaxSpeed(blindSpeed);
  stepperA.setAcceleration(blindAcceleration);
  stepperB.setMaxSpeed(blindSpeed);
  stepperB.setAcceleration(blindAcceleration);

  //enable both stepper drivers
  enableMotors();

  //Home blinds on start up
  homeBlinds();

  //enable wifi and start server
  WiFi.config(ip);
  enable_WiFi();
  connect_WiFi();
  server.begin();
  printWifiStatus();

}

void loop() {
  if (stepperA.currentPosition() == target && target < (0.1*closedPosition)){       //turn off motors if they're less than 10% open to save power
    disableMotors();
  }
  else{
    enableMotors();
  }
  
  moveOneStep();                                                                    //step both motors if a step is due

  client = server.available();

  if (client) {
    runServer();
  }
}

void enable_WiFi() {
  // check for the WiFi module:
  if (WiFi.status() == WL_NO_MODULE) {
    Serial.println("Communication with WiFi module failed!");
    // don't continue
    while (true);
  }

  String fv = WiFi.firmwareVersion();
  if (fv < "1.0.0") {
    Serial.println("Please upgrade the firmware");
  }
}

void moveOneStep() {
  stepperA.run();
  stepperB.run();
}

void connect_WiFi() {
  // attempt to connect to Wifi network:
  while (status != WL_CONNECTED) {
    Serial.print("Attempting to connect to SSID: ");
    Serial.println(ssid);
    // Connect to WPA/WPA2 network. Change this line if using open or WEP network:
    status = WiFi.begin(ssid, pass);
    // wait 10 seconds for connection:
    delay(10000);
  }
}

void printWifiStatus() {
  // print the SSID of the network you're attached to:
  Serial.print("SSID: ");
  Serial.println(WiFi.SSID());

  // print your board's IP address:
  IPAddress ip = WiFi.localIP();
  Serial.print("IP Address: ");
  Serial.println(ip);

  // print the received signal strength:
  long rssi = WiFi.RSSI();
  Serial.print("signal strength (RSSI):");
  Serial.print(rssi);
  Serial.println(" dBm");

  Serial.print("IP Address: http://");
  Serial.println(ip);
}

void homeBlinds() {
  // Home both motors if neither are home
  while (digitalRead(endstopA) == 1 && digitalRead(endstopB) == 1) {
    stepperA.moveTo(homePosA);
    stepperB.moveTo(homePosB);
    homePosA--;
    homePosB--;
    stepperA.run();
    stepperB.run();
  }

  // Home stepper B only if stepper B isn't home
  while (digitalRead(endstopA) != 1 && digitalRead(endstopB) == 1) {
    stepperB.moveTo(homePosB);
    homePosB--;
    stepperB.run();
  }

  // Home stepper A only if stepper A isn't home
  while (digitalRead(endstopA) == 1 && digitalRead(endstopB) != 1) {
    stepperA.moveTo(homePosA);
    homePosA--;
    stepperA.run();
  }

  //reset positions temporarily
  stepperA.setCurrentPosition(0);
  stepperB.setCurrentPosition(0);

  //back off endstops
  stepperA.moveTo(500);
  stepperB.moveTo(500);
  while (stepperA.currentPosition() != 500){
    stepperA.run();
    stepperB.run();
  } 

  //Set open position for both motors (0)
  stepperA.setCurrentPosition(openPosition);
  stepperB.setCurrentPosition(openPosition);  
  target = openPosition;
}

void setTarget(int scaledTarget) {
  target = map(scaledTarget, 100, 0, 0, closedPosition);  //map the scaled target from homebridge to 0 - closedPosition scale for motors
  stepperA.moveTo(target);                                //set new target for motors A and B
  stepperB.moveTo(target);
}

int getPosition() {
  return (map(stepperA.currentPosition(), 0, closedPosition, 100, 0));  //map the current position (in steps) to 0-100 scale for homebridge
}

int getState(){
  if (stepperA.currentPosition() < target) {
    return 0;                                     //if moving up, report opening
  } 
  if (stepperA.currentPosition() > target) {
    return 1;                                     //if moving down, report closing
  }
  else {
    return 2;                                     //otherwise report idle
  }
}


int enableMotors() {
  //pulls both motor driver enable pins low (enables current to motors)
  digitalWrite(enPinA, LOW);
  digitalWrite(enPinB, LOW);
}

int disableMotors() {
  //pulls both motor driver enable pins high (disables current to motors)
  digitalWrite(enPinA, HIGH); 
  digitalWrite(enPinB, HIGH);
}

void runServer() {
  if (client) {                             // if you get a client,
    String currentLine = "";                // make a String to hold incoming data from the client
    while (client.connected()) {            // loop while the client's connected
      if (client.available()) {             // if there's bytes to read from the client,
        char c = client.read();             // read a byte, then
        if (c == '\n') {                    // if the byte is a newline character
          if (currentLine.length() == 0) {
            //The HTTP response ends with another blank line:
            client.println();
            // break out of the while loop:
            break;
          }
          else {      // if you got a newline, then clear currentLine:
            currentLine = "";
          }
        }
        else if (c != '\r') {    // if you got anything else but a carriage return character,
          currentLine += c;      // add it to the end of the currentLine
        }

        if (currentLine.endsWith("/current_position")) {
          client.println("HTTP/1.1 200 OK");            //respond to request
          client.println("Content-type: text/plain");   //prepare response
          client.println();
          client.println(getPosition());                //give current motor position
        }
        if (currentLine.indexOf("set/") > 0) {
          String parameter = "";
          c = client.read();
          while (c != '\n') {
            Serial.write(c);
            parameter += c;
            c = client.read();
          }
          int scaledTarget = parameter.toInt();
          client.println("HTTP/1.1 200 OK");            //respond to request
          client.println("Content-type: text/plain");   //prepare repsonse
          client.println();
          client.println(parameter);
          setTarget(scaledTarget);                      //set new target position (0-100 scale)
          currentLine = "";                             // We overrode the newline check, so we have to reset ourselves
        }
        if (currentLine.endsWith("/current_state")) {
          client.println("HTTP/1.1 200 OK");            //respond to request
          client.println("Content-type: text/plain");   //prepare response
          client.println();
          client.println(getState());                   //report state (opening/closing/idle)
        }
      }
    }
    // close the connection:
    client.stop();
  }
}