/*
   Zadefinovanie funkcií,ktorými trieda MOTOR disponuje
*/

#include "motory.h"

/*
   "MOTOR" - Konštruktor danej triedy,
   Definuje piny dosky Arduino, ku ktorým sú pripojené
   infračervené sentory, slúžiace na vyhľadanie počiatočnej
   polohy motora Gamma, a motora C-plane,
   Taktiež definuje počiatočnú hodnotu logických premenných,
   určujúcich stav zariadenia po založení objektu v hlavnej
   časti programu, počiatočný stav: UKONČENÉ MERANIE?: NIE,
                                    NAJDENÁ REERENČNÁ POLOHA MOTORA GAMMA?: NIE,
                                    NAJDENÁ REERENČNÁ POLOHA MOTORA C-PLANE?: NIE,
                                    POHOL SA MOTOR GAMMA?: NIE,
                                    POHOL SA MOTOR C-PLANE?: NIE
*/

MOTOR::MOTOR() {
  GAMMA_INTERRUPTOR = 4;
  C_PLANE_INTERRUPTOR = 3;
  pinMode(C_PLANE_INTERRUPTOR, INPUT);
  pinMode(GAMMA_INTERRUPTOR, INPUT);
  endMeasurementFlag = false;
  ref_Position_C_Motor = false;
  ref_Position_Gamma_Motor = false;
  cMove = false;
  gammaMove = false;
}

/*
   "Cplane_MotorInit" - funkcia zodpovedná, za inicializáciu motora C-plane,
   definuje sa tu, ráchlosť otáčok motora, smerovanie,
   počiatočný počet krokov, dôležitý hlave pre určenie
   krokovania daného motora, a prostredníctvom parametra
   danej funkcie, sa nastavý merací krok konkrétneho motora.
*/

void MOTOR::Cplane_MotorInit(int multiplicationOfRegularStep) {
  Cplane_Motor = new Stepper(GAMMA_RANGE_LIMIT, MOTOR_C_PULL, MOTOR_C_DIR);
  Cplane_Motor -> setSpeed(SPEED_CPLANE);
  cPlane_Direction = 1;
  stepCplane = multiplicationOfRegularStep;
  cPlaneStepValue = 0;
}

/*
   "Gamma_MotorInit" - funkcia zodpovedná, za inicializáciu motora Gamma,
   definuje sa tu, ráchlosť otáčok motora, smerovanie,
   počiatočný počet krokov, dôležitý hlave pre určenie
   krokovania daného motora, a prostredníctvom parametra
   danej funkcie, sa nastavý merací krok konkrétneho motora.
*/

void MOTOR::Gamma_MotorInit(int multiplicationOfRegularStep) {
  Gamma_Motor = new Stepper(GAMMA_RANGE_LIMIT, MOTOR_GAMMA_PULL, MOTOR_GAMMA_DIR);
  Gamma_Motor -> setSpeed(SPEED_GAMMA);
  gamma_Direction = 1;
  stepGamma = multiplicationOfRegularStep;
  gammaStepValue = 0;
}

/*
 * "find_refPosition_Gamma_Motor" - slúži na odkrokovanie motora, 
 * s cieľom nájsť počiatočnú referenčnú pozíciu motora Gamma.  
 * V prípade, že motor vykoná celú jednu otáčku, a referenčnú
 * polohu nenájde, vráti hodnotu log. 0 - false, čím v hlavnom
 * programe zabráni v spustení meracieho procesu ... 
 */

boolean MOTOR::find_refPosition_Gamma_Motor() {
  Stepper *stepper = Gamma_Motor;
  int stepCounter = 0;
  int interruptor_Value = digitalRead(GAMMA_INTERRUPTOR);
  if (interruptor_Value == 0) {
    while (interruptor_Value == 0) {
      stepper->step(-1);
      stepCounter++;
      delay(DELAY_VALUE / 100);
      interruptor_Value = digitalRead(GAMMA_INTERRUPTOR);
      if (interruptor_Value > 0) {
        ref_Position_Gamma_Motor = true;
        break;
      }
      if (stepCounter == GAMMA_RANGE_LIMIT) {
        ref_Position_Gamma_Motor = false;
        break;
      }
    }
  } else {
    ref_Position_Gamma_Motor = true;
  }
  return ref_Position_Gamma_Motor;
}

/*
 * "find_refPosition_C_Motor" - slúži na odkrokovanie motora, 
 * s cieľom nájsť počiatočnú referenčnú pozíciu motora C-plane.  
 * V prípade, že motor vykoná celú jednu otáčku, a referenčnú
 * polohu nenájde, vráti hodnotu log. 0 - false, čím v hlavnom
 * programe zabráni v spustení meracieho procesu ... 
 */
 
boolean MOTOR::find_refPosition_C_Motor() {
  Stepper *stepper = Cplane_Motor;
  int stepCounter = 0;
  int interruptor_Value = digitalRead(C_PLANE_INTERRUPTOR);
  if (interruptor_Value == 0) {
    while (interruptor_Value == 0) {
      stepper->step(-1);
      stepCounter++;
      interruptor_Value = digitalRead(C_PLANE_INTERRUPTOR);
      delay(DELAY_VALUE / 100);
      if (interruptor_Value > 0) {
        ref_Position_C_Motor = true;
        break;
      }else if(stepCounter == GAMMA_RANGE_LIMIT){
        ref_Position_C_Motor = false;
        break;
      }
    }
  } else {
    ref_Position_C_Motor = true;
  }
  return ref_Position_C_Motor;
}

/*
 * "moveGammaMotor" - funkcia určujúca proces krokovania
 * motora Gamma ...
 */

void MOTOR::moveGammaMotor() {
  gammaStepValue += gamma_Direction * stepGamma;
  if (gammaStepValue > GAMMA_RANGE_LIMIT) {
    gamma_Direction *= -1;
    gammaStepValue += gamma_Direction * stepGamma;
    moveCplaneMotor();
  }
  else if (gammaStepValue < 0) {
    gamma_Direction *= -1;
    gammaStepValue += gamma_Direction * stepGamma;
    moveCplaneMotor();
  }
  else {
    gammaMove = true;
    delay(DELAY_VALUE);
    //Gamma_Motor -> step(gamma_Direction * stepGamma);
    slowStepGamma(gamma_Direction, stepGamma);
    delay(DELAY_VALUE);
  }
}

/*
 * "moveCplaneMotor" - funkcia určujúca proces krokovania
 * motora C-plane ...
 */
 
void MOTOR::moveCplaneMotor() {
  if (cPlaneStepValue == 0 && gamma_Direction == -1) {
    cPlaneStepValue += cPlane_Direction * stepCplane;
    cMove = true;
    delay(DELAY_VALUE);
    Cplane_Motor -> step(cPlane_Direction * stepCplane);
    //slowStepCplane(cPlane_Direction, stepCplane);
    delay(DELAY_VALUE);
    if (cPlaneStepValue >= CPLANE_RANGE_LIMIT) {
      endMeasurementFlag = true;
    }
    delay(DELAY_VALUE);
    //moveGammaMotor();
  } else if (cPlaneStepValue >= CPLANE_RANGE_LIMIT) {
    endMeasurementFlag = true;
  } else if (cPlaneStepValue < CPLANE_RANGE_LIMIT) {
    cMove = true;
    delay(DELAY_VALUE);
    Cplane_Motor -> step(cPlane_Direction * stepCplane);
    //slowStepCplane(cPlane_Direction, stepCplane);
    cPlaneStepValue += cPlane_Direction * stepCplane;
    if (cPlaneStepValue >= CPLANE_RANGE_LIMIT) {
      endMeasurementFlag = true;

    }
    delay(DELAY_VALUE);
  }
}

/*
   "slowStepGamma" - funkcia, ktorá prostrednictvom zaciklenia, spomaľuje pohyb
   motora Gamma.
*/
void MOTOR::slowStepGamma(int dir, int steps) {
  Stepper *stepper = Gamma_Motor;
  for (int i = 0; i < steps; i++) {
    stepper->step(dir * 1);
    delay(8);
  }
}

/*
   "slowStepCplane" - funkcia,ktora prostrednictvom zaciklenia, spomaľuje
   pohyb motora C-plane.
*/

void MOTOR::slowStepCplane(int dir, int steps) {
  Stepper *stepper = Cplane_Motor;
  for (int i = 0; i < steps; i++) {
    stepper->step(dir * 1);
    delay(10);
  }
}

/*
   "moveForCorrection" - Funkcia, ktorá v prípade ukončeného merania,
   posunie vrchný motor o stanovený počet krokov, aby nedošlo k chybnej
   indikácii referenčnej polohy, čo by mohlo viesť, k chybnému
   nastaveniu smeru motora, a poškodeniu kabeláže príslušného
   motora.
*/

void MOTOR::moveForCorrection(int stepM) {
  //int stepMotor = stepM;
  Stepper *stepper = Gamma_Motor;
  stepper->step(stepM);
}

/*
   "setGammaMove" - funkcia, ktorá nastavuje indikátor pohybu
   motora Gamma, pričom jej zásadnosť spočíva v indikácii kroku
   motora, .... umožňuje umiestnenie separačných znakov
   v dátovom súbore, preposielanom do sotvéru používateľského
   rozhrania.
*/

void MOTOR::setGammaMove(boolean flag) {
  gammaMove = flag;
}

/*
   "setCplaneMove" - funkcia, ktorá nastavuje indikátor pohybu
   motora C-plane, pričom jej zásadnosť spočíva v indikácii kroku
   motora, .... umožňuje umiestnenie separačných znakov
   v dátovom súbore, preposielanom do sotvéru používateľského
   rozhrania.
*/

void MOTOR::setCplaneMove(boolean flag) {
  cMove = flag;
}

/*
   "cMotor_Move" - funkcia, ktorá vracia hodnotu indikátora pohybu
   motora C-plane, pričom jej zásadnosť spočíva v indikácii kroku
   motora, .... umožňuje umiestnenie separačných znakov
   v dátovom súbore, preposielanom do sotvéru používateľského
   rozhrania.
*/

boolean MOTOR::cMotor_Move() {
  return cMove;
}

/*
   "gammaMotor_Move" - funkcia, ktorá vracia hodnotu indikátora pohybu
   motora C-plane, pričom jej zásadnosť spočíva v indikácii kroku
   motora, .... umožňuje umiestnenie separačných znakov
   v dátovom súbore, preposielanom do sotvéru používateľského
   rozhrania.
*/

boolean MOTOR::gammaMotor_Move() {
  return gammaMove;
}

/*
   "getEndOf_MeassurementProcess" - funkcia, indikujúca ukončenie meracieho
   procesu, ... umožňuje zariadenie v prípade správneho chodu, prechod
   do neaktívneho stavu.
*/

boolean MOTOR::getEndOf_MeassurementProcess() {
  return endMeasurementFlag;
}

/*
 * "get_stepCplane" - funkcia vracajuca pocet krokov motora C-plane
 */

int MOTOR::get_stepCplane() {
  return stepCplane;
}

/*
 *  "set_ref_Position_C_Motor" - funkcia, ktorá prostredníctvom svojho
 *  parametra, umožňuje zmenu parametra nájdenej referenčnej polohy 
 *  motora  C-plane, pričom prepisuje predtým nastavenú hodnotu z 
 *  konštruktora tejto triedy
 */
 
void MOTOR::set_ref_Position_C_Motor(boolean flag) {
  ref_Position_C_Motor = flag;
}

/*
 *  "set_ref_Position_Gamma_Motor" - funkcia, ktorá prostredníctvom svojho
 *  parametra, umožňuje zmenu parametra nájdenej referenčnej polohy 
 *  motora  Gamma, pričom prepisuje predtým nastavenú hodnotu z 
 *  konštruktora tejto triedy
 */
 
void MOTOR::set_ref_Position_Gamma_Motor(boolean flag) {
  ref_Position_Gamma_Motor = flag;
}
