#include <EEPROM.h>
#include <Servo.h>
#include <SoftwareSerial.h>

SoftwareSerial mySerial(11, 5); //Product board

int motorPin = 12;    // The pin output PWM to control motor
int motorPin2 = 0;   // For the new board with 2 motors, if motorPin2 < 0, then ignore it 

int hornPin = 10;
int hornOffTime = 1000; // Cannot use horn too long, must less than 1s
int hornStartTime = 0;

int power = 50;       // Control pow value, from 0 to 100;
long lastRecvTime = 0; // If cannot recv control info in 0.2s, stop the moto
int shutDownTimeRange = 200;
int powerScale = 100;  //if set the power scale by mobile
int testingValue = 0;

int lightMode = 2;   // 1: on; 2:off; 3: auto
int lightCtrlPin = 10;
int envLightSensorPin = 1;
boolean lightStatus = false;  // shutdown ligth by default

Servo powerServo;
Servo powerServo2;

// The Led pin for debug
int ledPin = 13;
int light = 1;
int counter = 0;

// Beep pin
int beepPin = 6;
boolean beepStatus = false;

// Connect to speaker
int tonePin = 6;

// battery pin
int batteryPin = 0;
float batteryScale = 11;
float batteryAlert = 3.5;

void setup()
{
  pinMode(ledPin,OUTPUT);
  pinMode(beepPin,OUTPUT);
  pinMode(lightCtrlPin,OUTPUT);
  pinMode(hornPin,OUTPUT);
    
  powerScale = EEPROM.read(0);
  if (powerScale <= 0 || powerScale > 100) powerScale = 100;
  lightMode = EEPROM.read(1);
  if (lightMode <= 0 || lightMode > 3) lightMode = 2; // default light off
  
  Serial.begin(9600); 
  mySerial.begin(9600); 
  powerServo.attach(motorPin);
  if (motorPin2 > 0) powerServo2.attach(motorPin2);
}

void checkBattery()
{
  if (counter == 0 || counter == 200) {
    int readValue = analogRead(batteryPin);
    if (readValue > 5 && batteryScale * readValue / 1024 * 5 < batteryAlert * 6) {
        digitalWrite(beepPin, beepStatus);
        beepStatus = !beepStatus;
    } else {
        digitalWrite(beepPin, 0);
    }
  }
}

void updateCounter() {
  counter++;
  if (counter >= 400) counter = 0;
}

void switchLight()
{
  if (counter == 0) {
    digitalWrite(ledPin, light);
    light = 1 - light;
    //Serial.println(testingValue);
  }
}

void sendPowerInfo()
{
  if (counter == 0) {
      mySerial.write((byte)(analogRead(batteryPin) / 4));
  }
}

void loop()
{  
  long t0 = micros();

  int readPower = -1;
  while(mySerial.available()) {
    readPower = mySerial.read();
    lastRecvTime = micros();
    digitalWrite(ledPin, light);
    light = 1 - light;
    //counter = 0;
    if (readPower > 100) {
      break;
    }
  }
  if (readPower > -1 && readPower <= 100) {
    // Normal status
    power = readPower;
  }
  if (readPower == 255 || (lastRecvTime > 0 && (t0-lastRecvTime)/1000 > shutDownTimeRange)) {
    power = 50; // 松手默认滑行
  }
  if (readPower > 100 && readPower <= 200) {
    powerScale = readPower - 100;
    EEPROM.write(0, (byte) powerScale);  
  }
  if (readPower > 200 && readPower <= 203) {
    lightMode = readPower - 200;
    EEPROM.write(1, (byte) lightMode);  
  }
  if (readPower == 204) {
    digitalWrite(hornPin, 1);  // start horn
    hornStartTime = t0;
    tone(tonePin, 4000);
  }
  //if (readPower == 205 || (hornStartTime > 0 && (t0 - hornStartTime) / 1000 > hornOffTime)) {
  if (readPower == 205) {
    digitalWrite(hornPin, 0);  // stop horn
    hornStartTime = 0;
    noTone(tonePin);
  }
  // light control
  if (lightMode == 1) {
    // light on
    digitalWrite(lightCtrlPin, 1);
    lightStatus = true;
  } else if (lightMode == 2){
    // light off
    digitalWrite(lightCtrlPin, 0);
    lightStatus = false;
  } else {
    //auto 
    int envLight = analogRead(envLightSensorPin);
    testingValue = envLight;
    if (!lightStatus && envLight < 800) {
      lightStatus = true;
    }
    if (lightStatus && envLight > 900) {
      lightStatus = false;
    }
    digitalWrite(lightCtrlPin, lightStatus);
  }
  updateCounter();
  switchLight();
  checkBattery();
  
  double T = 2500;
  // if has power scale
  double scaledPower = power;
  if (power > 50) {
    scaledPower = 50 + (power - 50) * powerScale / 100;
  }
  double len = 900 + scaledPower * 12; //(0~100对应总周期0.9ms～2.1ms)  
  powerServo.writeMicroseconds(len);
  if (motorPin2 > 0) powerServo2.writeMicroseconds(len);

  int leftMs = (int) (t0 + T - micros());
  if (leftMs > 1500 && (t0-lastRecvTime)/1000 < shutDownTimeRange) {
    sendPowerInfo();
    leftMs = (int) (t0 + T - micros());
  }
  if (leftMs < 1) leftMs = 1;
  delayMicroseconds(leftMs);
}