// Include libraries
#include <Arduino.h>
#include "SoftwareSerial.h"
SoftwareSerial mySerial(23, 24); //rx,tx
#include "math.h"
#include "Adafruit_NeoPixel.h"
#include "Adafruit_PWMServoDriver.h"

#define SCENECOUNT 9

// Define Motor parameters
#define CHANGETIME 2000
uint8_t PINMOTOR[2];

// Define Sensor parameters
#define ECHO 9
#define TRIG 8

// Define PWM parameters
#define SERVOCOUNT 4
#define DIST_Z 20
Adafruit_PWMServoDriver pwms[SERVOCOUNT] = {Adafruit_PWMServoDriver(0x40), Adafruit_PWMServoDriver(0x41), Adafruit_PWMServoDriver(0x42), Adafruit_PWMServoDriver(0x43)};

// Structure for position variables
struct position {byte x; byte y;};

// Structure for servos
struct servoStruct {
  int zones[3][2];
  // Automatic variables
  uint16_t length[3];
};

// Structure for scenes
struct scenestruct {
  long startTime;
  long endTime;
  byte protect;
  byte pulse;
  // Automatic variables
  unsigned long time;
  float coefX[SERVOCOUNT];
  float coefY[SERVOCOUNT];
};

/////////////////////////////////////////////
/////////////////////////////////////////////
/////////////////////////////////////////////
// Scene variables
byte id = 0;
bool book;

// Servo Directions
position start[SCENECOUNT][SERVOCOUNT] {
  {
    {10, 10},
    {0, 0},
    {0, 20},
    {20, 20}
  },
  {},
  {},
  {},
  {},
  {},
  {},
  {},
  {}
};

position end[SCENECOUNT][SERVOCOUNT] {
  {
    {0, 0},
    {10, 10},
    {20, 20},
    {0, 0}
  },
  {},
  {},
  {},
  {},
  {},
  {},
  {},
  {}
};

byte rad[SCENECOUNT][SERVOCOUNT] = {
  {50, 50, 0, 0},
  {4, 4, 4, 4},
  {},
  {},
  {},
  {},
  {},
  {},
  {}
};

byte colors[SCENECOUNT][SERVOCOUNT][3] {
  {
    {100, 0, 100},
    {100, 100, 0},
    {0, 100, 100},
    {100, 100, 100}
  },
  {},
  {},
  {},
  {},
  {},
  {},
  {},
  {}
};



// Servo structs
servoStruct servos[SERVOCOUNT] {
  {},
  {{{155, 550}, {200, 515}, {150, 400}}},
  {},
  {}
};

// Scene structs
scenestruct scenes[SCENECOUNT] {
  {0,   20, 0b10, 0b1111},
  {23,  31, 0b10},

  {34,  49, 0b10},
  {52,  57, 0b10},

  {60,  62, 0b10},
  {65,  69, 0b10},
  {72,  75, 0b10},

  {78,  101, 0b10},
  {104, 120, 0b10}
};

// Initializate scene structures
void initStructs() {
  // Init servos
  for (int servo=0; servo < SERVOCOUNT; servo++) {
    // INIT PWM
    pwms[servo].begin();
    pwms[servo].setPWMFreq(50);

    // INIT VARIABLES
    for (byte j; j < 3; j++) {
      servos[servo].length[j] = abs(servos[servo].zones[j][0] - servos[servo].zones[j][1]);
    }
    delay(100);
  }

  // Init scenes
  unsigned int time = millis();
  for(int scene=0; scene<SCENECOUNT; scene++) {
    if (scenes[scene].startTime < 1000) scenes[scene].startTime *= 1000;
    if (scenes[scene].endTime   < 1000) scenes[scene].endTime   *= 1000;
    scenes[scene].startTime += time;
    scenes[scene].endTime   += time;
    scenes[scene].time = scenes[scene].endTime - scenes[scene].startTime;

    for (int servo=0; servo < SERVOCOUNT; servo++) {
      scenes[scene].coefX[servo] = (float)abs(start[scene][servo].x - end[scene][servo].x) / scenes[scene].time;
      scenes[scene].coefY[servo] = (float)abs(start[scene][servo].y - end[scene][servo].y) / scenes[scene].time;
      Serial.println((float)abs(start[scene][servo].x - end[scene][servo].x) / scenes[scene].time);
    }
  }
}

// Function for display the storm
void storm(scenestruct scene) {
  // TODO: Storm function
}

// Function for circuit frame
void circuit(scenestruct scene) {
  // TODO: Circuit function
}

void moveServo(byte servo, float x, float y, int zones[3][2]) {
  float xAngle = x == 0 ? 90 : atan(x / DIST_Z) * RAD_TO_DEG + 90;
  float yAngle = y == 0 ? 90 : atan(y / DIST_Z) * RAD_TO_DEG + 90;

  Serial.print("Angles:");
  Serial.print("\t");
  Serial.print(x);
  Serial.print("\t");
  Serial.print(y);
  Serial.print("\t");
  Serial.print(xAngle);
  Serial.print("\t");
  Serial.print(yAngle);
  Serial.print("\t");
  Serial.print(map(xAngle, 0, 180, zones[0][0], zones[0][1]));
  Serial.print("\t");
  Serial.print(map(yAngle, 0, 180, zones[1][0], zones[1][1]));
  Serial.println("\t");

  pwms[servo].setPWM(0, 0, map(xAngle, 0, 180, zones[0][0], zones[0][1]));
  pwms[servo].setPWM(1, 0, map(yAngle, 0, 180, zones[1][0], zones[1][1]));
  pwms[servo].setPWM(2, 0, map(rad[id][servo], 0, 100, zones[2][0], zones[2][1]));
}

// Function for color dioids
void colorDiods(byte servo) {
  for (byte color=0; color < 3; color++) {
    for (byte pin=4; pin < 8; pin++) {
      pwms[servo].setPin(pin+color*4, map(colors[id][servo][color], 0, 100, 0, 4095)); // TODO: ^ | v;
    }
  }
}

// Function for move servos
void updatePWMS(scenestruct scene) {
  for (int servo=0; servo < SERVOCOUNT; servo++) {
    Serial.print("Servo:\t");
    Serial.print(servo);
    Serial.print("\t");
    float x = scene.coefX[servo] * (millis() - scene.startTime) + start[id][servo].x;
    float y = scene.coefY[servo] * (millis() - scene.startTime) + start[id][servo].y;
    moveServo(servo, x, y, servos[servo].zones);

    colorDiods(servo);
  }
}

// Function for change scene
volatile uint8_t motorState[2];   // 0 - off, 1 - start, 2 - go, 3 - stop
uint8_t motorSpeed[2];
uint32_t motorLastTime[2];
void motorUpdate0() {motorState[0] = 3;}    // функция обработчик прерывания 0 (второй пин)
void motorUpdate1() {motorState[1] = 3;}    // функция обработчик прерывания 1 (третий пин)


void changeScene() {
  for (int i = 0; i < 2; i++) {
    if (motorState[i] == 0 || motorState[i] == 2) ;               // ничего не делать, если выкл, если едет, читаем датчики ( работает прерывание )
    else if (millis() - motorLastTime[i] > CHANGETIME / 255) {    // периодически изменяем скорость
      motorState[i] == 1 ? motorSpeed[i]++ : motorSpeed[i]--;
      motorLastTime[i] = millis();
      analogWrite(PINMOTOR[i], motorSpeed[i]);
      if (motorSpeed[i] == 255) {                             	  // активация прерывания
        if (i==0) attachInterrupt(i, motorUpdate0, FALLING);
        else  attachInterrupt(i, motorUpdate1, FALLING);
        motorState[i] = 2;
      }
      else if (motorSpeed[i] == 0) {
        detachInterrupt(i);                                   		// отключение прерывания
        motorState[i] = 0;
      }
    }
  }
}

// Function for check distance from UltraSonic
bool checkDist() {
  digitalWrite(TRIG,1);
  delayMicroseconds(10);
  digitalWrite(TRIG,0);

  int distance = pulseIn(ECHO, 1, 5000) / 58;
  // Serial.println(distance);
  return distance > 0;
}
//=======================================================
//====================== MAIN CODE ======================
//=======================================================
void setup() {
  delay(3000);
  Serial.begin(9600);
  Serial.println("Let");
  mySerial.begin(38400);

  pinMode(ECHO, INPUT);
  pinMode(TRIG, OUTPUT);


  byte state;
  do {
    if (millis() % 100 == 0) {
      state = mySerial.available() + checkDist()*2;
      book = state == 2 ? false : true;
      Serial.println(state);
    }
  } while (!state);
  Serial.println(book ? "Book is ok" : "Book is broken");
  initStructs();
}

unsigned long time = millis();
void loop() {
  Serial.print("Update:\t");
  Serial.print(id);
  Serial.println("\t");

  if bitRead(scenes[id].protect, 0) storm(scenes[id]);
  if bitRead(scenes[id].protect, 1) updatePWMS(scenes[id]);
  changeScene();

  if (millis() - time > 1.2*CHANGETIME && book ? Serial.available():checkDist()) {
    id++;
    motorState[0] = 1; motorState[1] = 1;
    time = millis();
  }
  Serial.println("  ");
}