#include <SPI.h>
#include <EthernetENC.h>
#include <PubSubClient.h>


int motorSpeedPin =   6;
int motorUpPin =      2;
int motorDownPin =    3;
int motorLeftPin =    4;
int motorRightPin =   5;
int posTilt =         A0;
int posPan =          A1;
int motorSpeedStat =   0;

int speedStatus = 0;
int motorSpeed = 100;
int gotoTilt = 70;


int val;
int valOld;
char buf[12];
char buf2[12];

// Update these with values suitable for your network.
byte mac[]    = {  0xDE, 0xED, 0xBA, 0xFE, 0xFE, 0xED };
IPAddress ip(10, 13, 37, 165); 
byte server[] = {10, 13, 37, 101};

void callback(char* topic, byte* payload, unsigned int length) {

  Serial.print("Message arrived [");
  Serial.print(topic);
  Serial.print("] ");
  for (int i = 0; i < length; i++) {
    Serial.print((char)payload[i]);
  }
  Serial.println("");



  
}

EthernetClient ethClient;
PubSubClient client(server, 1883, callback, ethClient);

void setup()
{

  pinMode(motorSpeedPin, OUTPUT);
  pinMode(motorUpPin, OUTPUT);
  pinMode(motorDownPin, OUTPUT);
  pinMode(motorLeftPin, OUTPUT);
  pinMode(motorRightPin, OUTPUT);

  pinMode(posTilt, INPUT);
  pinMode(posPan, INPUT);

  Serial.begin(115200);
  Ethernet.begin(mac, ip);
  client.setBufferSize(255);
 
}

void reconnect() {
  // Loop until we're reconnected
  while (!client.connected()) {
    Serial.print("Attempting MQTT connection...");
    // Attempt to connect
    if (client.connect("connectionName", "[[USER]]", "[[PASS]]")) {
      Serial.println("connected");
      client.publish("hamradio/rotator/status","READY");
      client.subscribe("hamradio/rotator/#");
        
  client.publish("hamradio/rotator/motor/speed", "100");
  client.publish("hamradio/rotator/pos/pan", "180");
  client.publish("hamradio/rotator/pos/tilt", "60");
  client.publish("hamradio/rotator/goto/pan", "180");
  client.publish("hamradio/rotator/goto/tilt", "60");
    } else {
      Serial.print("failed, rc=");
      Serial.print(client.state());
      Serial.println(" try again in 5 seconds");
      // Wait 5 seconds before retrying
      delay(5000);
    }
  }
}

void loop()
{
  if (!client.connected()) {
    reconnect();
  }
  client.loop();
val = analogRead(posTilt);
//Serial.println(val);
val = map(val, 0, 902, 0, 90);
if (((val - valOld) > 1) || ((valOld - val) > 1)) {  
  if(speedStatus == 0) {
    motorSpeed = map(abs((val - gotoTilt)), 5, 20, 30, 100);
    if (motorSpeed < 30) { motorSpeed = 30; }
    if (motorSpeed > 100) { motorSpeed = 100; }
    client.publish("hamradio/rotator/motor/speed", itoa(motorSpeed, buf2, 10));
  }
  valOld = val;
  client.publish("hamradio/rotator/pos/tilt", itoa(val, buf, 10));
  delay(15);
  Serial.println(analogRead(posTilt));
}
delay(15);

  
}