//
//  servo
//
#include <Servo.h>

Servo Tilt;
Servo Pan;


const int kTilt = 14; // 10 - 130degree(up-down)
const int kPan  = 15; // 40 - 160degree(right-left)

const int kMinTilt =  10;
const int kMaxTilt = 130;
const int kMinPan =   40;
const int kMaxPan =  160;

const int kInitTilt =  10;
const int kInitPan  = 100;


float currentTilt, currentPan;  //  current position
int   targetTilt,  targetPan;   //  command from iPhone

ETSTimer Timer;

void ServoController(void *temp) {
  if (targetTilt < kMinTilt) targetTilt = kMinTilt;
  if (targetTilt > kMaxTilt) targetTilt = kMaxTilt;
  if (targetPan < kMinPan) targetPan = kMinPan;
  if (targetPan > kMaxPan) targetPan = kMaxPan;

  float vTilt = ((float)targetTilt - currentTilt) / 10.0;
  float vPan  = ((float)targetPan  - currentPan)  / 10.0;

  currentTilt += vTilt;
  currentPan  += vPan;

  Serial.print(currentTilt);
  Serial.print(",");
  Serial.println(currentPan);

  Tilt.write(currentTilt);
  Pan.write(currentPan);
}


//
//  WiFi
//
#define USE_US_TIMER 1
extern "C" {
#include "user_interface.h"
#include "osapi.h"
}
#include <ESP8266WiFi.h>

const char* ssid     = "*************";
const char* password = "*************";

WiFiServer Server(51515);
WiFiClient Client;

const int kMaxBuffer = 80;


//
//  IR Sensor
//
#include <Wire.h>
#include <Adafruit_MLX90614.h>
#include <Time.h>

Adafruit_MLX90614 mlx = Adafruit_MLX90614();

int prevSecond = second();

const int LED = 13;


//
//  Setup
//
void setup()
{
  Serial.begin(115200);
  //
  // Wifi
  Serial.println("start to connect");
  WiFi.begin(ssid, password);
  while (WiFi.status() != WL_CONNECTED) {
    Serial.print('.');
    delay(500);
  }
  Serial.println("wifi connected");

  Server.begin();
  Server.setNoDelay(true);
  Serial.print("receive ready. IP addr:");
  Serial.print(WiFi.localIP());
  Serial.println(", port:51515");

  //
  //  Temperature sensor
  mlx.begin();
  prevSecond = second();

  //
  //  Servo
  Tilt.attach(kTilt);
  Pan.attach(kPan);

  currentTilt = kInitTilt;
  currentPan  = kInitPan;

  targetTilt =  kInitTilt;
  targetPan  =  kInitPan;

  Tilt.write(currentTilt);
  Pan.write(currentPan);

  os_timer_setfn(&Timer, ServoController, NULL);
  os_timer_arm(&Timer, 10, true);

  //
  //  other
  pinMode(LED, OUTPUT);
}


//
//  loop
//
void loop() {
  //
  // check connection
  if (Server.hasClient()) {
    if (!Client || !Client.connected()) {
      if (Client) Client.stop();
      Client = Server.available();
    }
  } else {
    //no free/disconnected spot so reject
    WiFiClient client = Server.available();
    client.stop();
  }

  //
  //  receive data from iPhone
  if (Client && Client.connected()) {
    if (Client.available()) {
      //get data from the telnet client and push it to the UART
      int c;
      int p = 0;
      int p2 = 0;
      char line[80];
      do {
        c = Client.read();
        if (c == ',') {
          p2 = p + 1;
          c = 0;
        }
        line[p++] = c;
      } while (p < kMaxBuffer - 1 && c != '\n') ;
      line[p] = 0;
      targetTilt = atoi(line);
      targetPan  = atoi(line + p2);
    }
  }

  //
  //  send sensor data
  if (prevSecond != second()) {
    digitalWrite(LED, HIGH);

    float ambient = mlx.readAmbientTempC();
    float object  = mlx.readObjectTempC();

    if (Client && Client.connected()) {
      char line[20];
      int a, o;
      a = ambient * 100;
      o = object  * 100;
      sprintf(line, "%d,%d\n", a, o);
      Client.write((const char *)line);

    }

    digitalWrite(LED, LOW);
    prevSecond = second();
  }
}