#include "motor_controls.h"
#include  <MD_MAX72xx.h>
#include <SPI.h>
#include "OLED_controls.h"
#include "drill_controls.h"
#include "js_state.h"
#include <U8g2lib.h>
#include <Servo.h>
#include "matrix_controls.h"
#include "field_controls.h"
#include <stdlib.h>

MD_MAX72XX mx = MD_MAX72XX(MD_MAX72XX::FC16_HW, 24, 25, 26, 1);


U8G2_SH1107_128X128_F_HW_I2C u8g2(U8G2_R0);

double* current_data;

double* jerk_track;

double excav_data[8][6] = {0};  //array element consists of {depth,soft,semi-soft,hard,max_jerk}

int c1=0; int c2=0; int c3 = 0;   //ith index

int jerk_iter = -1;

Servo myServo;
int set = 1;
bool cond = true;
bool moved_x = false;
bool moved_y = false;

void SELECT_ISR(){
  
   selected = true;
   prim_cursor = set;
   if(prim_cursor==1){
    cond = false;     //for the JS to control drill for 1
   }
   else{
    cond = true;   //for the JS to control the main options 1,2 and 3
   }
}

void BACK_ISR(){
  period  = 0;
  freq1 = 0;
  freq2 = 0;
   selected = false;
   just_got_hit = false;
   cond = true;
   mx.clear();
}


void setup(){
  u8g2.begin();
  u8g2.setContrast(255);
  pinMode(28,OUTPUT);  //standdard LED
  pinMode(29,OUTPUT);  // buzzer
  pinMode(7,OUTPUT);  //RED of RGB
  pinMode(5,OUTPUT);   //BLUE of RGB
  pinMode(13,INPUT);
  pinMode(STEP_PIN, OUTPUT);
  pinMode(DIR_PIN, OUTPUT);
  pinMode(JS_VER, INPUT);
  pinMode(JS_HOR, INPUT);
  pinMode(SELECT_PIN, INPUT_PULLUP);
  pinMode(BACK_PIN, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(SELECT_PIN), SELECT_ISR, FALLING);
  attachInterrupt(digitalPinToInterrupt(BACK_PIN), BACK_ISR, FALLING);
  myServo.attach(SERVO_PIN);
  mx.control(MD_MAX72XX::INTENSITY, 5);
  mx.begin();

}

void loop(){
  int val_x = analogRead(JS_HOR);
  int val_y = analogRead(JS_VER);
  if(val_y < 300 && !moved_y){
        moved_y  = true;
        JS_state = DOWN;
        
    }
  else if(val_y > 700 && !moved_y){
        moved_y = true;
        JS_state = UP;
        
    }
  else if (val_y >= 300 && val_y <= 700){
        JS_state = IDLE;
        moved_y = false;
        
    }
  
  if(cond==false){
          if(JS_state == DOWN){
                  sec_cursor1 = 1;
                  
                }
          else if(JS_state == UP){
                  sec_cursor1 = 2;
                  
                  }
          else  {
                  if(val_x >= 300 && val_x <= 700){
                      
                  }
                  sec_cursor1 = 3;
                  
                }
          
          if  (val_x < 300 && !moved_x){    //need to check x only if drill mode is on, i.e, cond==false
               moved_x  = true;
               sec_cursor2 = 2;               //moves right
        
                    }
          else if  (val_x > 700 && !moved_x){
                moved_x = true;                      //moves left
                sec_cursor2 = 1;    
                  }
          else if(val_x >= 300 && val_x <= 700){

                moved_x = false;
                sec_cursor2 = 3;                   //stat
          }

  }
  
  else{
    if(JS_state == DOWN){
              if(set < 3){
               set++;
                }
               JS_state = IDLE;
            }
    else if(JS_state == UP){
              if(set > 1){
                set--;
                }
                JS_state = IDLE;
             } 
  }
  OLED_control();

}