#include "Timer1.h"
#include "Display.h"
#include "Motor.h"


uint32_t sqrtBinary(uint32_t x) {
  if (x == 0 || x == 1)
    return x;

  uint32_t start = 0, end = x, ans;
  while (start <= end) {
    uint32_t mid = start + (end - start) / 2;

    // Checking if mid*mid is less than or equal to x
    if (mid <= x / mid) {
      start = mid + 1;
      ans = mid;
    }
    else
      end = mid - 1;
  }
  return ans;
}

Timer1 t1;
Display d(8, 9, 4, 5, 6, 7); //RS , E , D4 , D5 , D6 , D7
Motor m(&t1, 5, 800, 4);

const uint16_t steps_per_mm = m.get_steps_per_mm();
const uint16_t a            = ACCELERATION;
const uint32_t a_           = ACCELERATION * 1e3;
const uint16_t dS_          = 1e3 / steps_per_mm;
const uint32_t ads2         = 2 * a_ * dS_;

void setup() {
  Serial.begin(9600);
  Serial.println();

  pinMode(10, OUTPUT);
  pinMode(11, OUTPUT);

  pinMode(A1, INPUT);
  pinMode(A2, INPUT);

  Serial.println("-----------------------");

  m.dirLeft();
  m.const_move(20);
  //m.millisDelay(3000);
  //t1.stopPWM();

  
  Serial.println("inputCoilLength");
  uint16_t L = d.inputCoilLength(50, 600, 50, 2);

  Serial.println("printCoilSettings");
  d.printCoilSettings(L, L+2.2);

  Serial.println("inputSpeed");
  d.inputSpeed(0.1, 20, 10, 0.1);

  Serial.println("settingsConfirm");
  d.settingsConfirm();
  
  Serial.println("saveConfirm");
  d.saveConfirm();
  
}

void loop() {}

// Прерывание по BOTTOM
ISR(TIMER1_OVF_vect) {
  cli(); ICR1  = t1.getICR(); sei();
  cli(); OCR1B = t1.getOCR(); sei();

  switch (m.MOVE_TYPE) {
    case m.ACCEL:
      if (a == 0) return;
      
      // Формула:
      // dt = ( sqrt( 2 * a * dS + v0 ^ 2 ) - v ) / a

      uint32_t v_                 = m.speed * 1e3;
      uint32_t v__                = v_ * 1e3;
      uint32_t v_sq               = v_ * v_;

                                                                      //Serial.print("v         ");   Serial.println(m.speed);
      uint32_t sqrt_term  = ads2 + v_sq;                              //Serial.print("sqrt_term ");   Serial.println(sqrt_term);
      uint32_t sqrt_num_  = 1e3 * sqrtBinary( sqrt_term );            //Serial.print("sqrt_num' ");   Serial.println(sqrt_num_);
                                                                      //Serial.print("v''       ");   Serial.println(v__);
      uint32_t dt__       = ( sqrt_num_ - v__ ) / a;                  //Serial.print("dt''      ");   Serial.print(dt__);          Serial.println(" мкс");
      uint32_t new_speed_ = ( 1e6 * dS_ ) / dt__;                     //Serial.print("new_speed ");   Serial.print(new_speed_);    Serial.println(" мкм/с");

      uint32_t speed = 0;
      if (new_speed_ >= min( MAX_SPEED, m.target_speed ) * 1e3 ) {
        m.MOVE_TYPE = m.CONST;

        if (new_speed_ >= MAX_SPEED * 1e3) {
          speed = MAX_SPEED * 1e3;
          m.speed = MAX_SPEED;
        } else {
          speed = m.target_speed * 1e3;
          m.speed = m.target_speed;
        }
      } else {
        speed = new_speed_;
        m.speed = new_speed_ / 1e3f;
      }

      uint32_t freq = (speed * steps_per_mm ) / 1e3f;                 //Serial.print("freq      ");  Serial.println(freq);
      uint16_t TOP  = F_CPU / (2 * freq) - 1;                         //Serial.print("TOP       ");  Serial.println(TOP);
                                                                      //Serial.println();
      
      t1.setICR(TOP);
    break;

    /*
    case m.DECCEL:
      //
    break;
    */
  }
}

// Прерывание по TOP
ISR(TIMER1_CAPT_vect) {
  m.incSTEPS();

  if ((m.getDIR() == LEFT && m.IS_LEND()) || (m.getDIR() == RIGHT && m.IS_REND())) {
    m.MOVE_TYPE = m.STOP;
    m.speed = 0;
  }

  if (m.MOVE_TYPE == m.STOP) t1.stopPWM();
}