#include <stdio.h>
#include <string.h>
#include <math.h>
#include "OSP.h"

/** 
 * Declarations for stepper motor 1 (at the base)
 * All the pins below are connected to digital pins of Arduino Nano.
 */
 
#define dir1 11 
#define step1 12 
#define enable1 10 

#define relay1 2
#define relay2 4
#define relay3 5
#define relay4 6

/*
 * The output of step down module (4 Volts) is connected to analog pin A0. 
 * This is to check whether SMPS is ON or OFF. 
 * 
 */

int smps = A0;
int smps_on;

/**
 * Declarations for stepper motor 2 (at top holding LASER)
 */
#define dir2 8 
#define step2 9 
#define enable2 7 

/**
 * Digital pin to control the LASER 
 * This pin is connected to gate (G) of IRF540
 */
int laserPin = 3;

 
/**
 * Library for control the device. 
 */
OSP Axes = OSP();
String m_data, sac_data, salt_data; // to store LASER coorinates 

void setup(){
  Serial.begin(9600); 
  pinMode(relay1,OUTPUT);
  pinMode(relay2,OUTPUT);
  pinMode(relay3,OUTPUT);
  pinMode(relay4,OUTPUT);
  pinMode(A1,OUTPUT);
  digitalWrite(A1, LOW); 
  pinMode(smps,INPUT);
  digitalWrite(relay1, HIGH);
  digitalWrite(relay2, HIGH);
  digitalWrite(relay3, HIGH);
  digitalWrite(relay4, HIGH);
  pinMode(laserPin, OUTPUT);
  laserOff();
  Axes.setMotorsPins(dir1, step1, enable1, dir2, step2, enable2);
  Axes.delayValue = 30; // to adjust motor speed as Coarse and Fine 
 
  }
 

/**
 * Turn LASER on at given intensity 
 */
void laserOn(int intensity){
  analogWrite(laserPin, intensity);
  }

/**
 * Turn LASER Off
 */
void laserOff(){
  digitalWrite(laserPin, LOW);
}


void getAcAlt(){
  char bytes[40];
  int nbytes = 0;
  bool recv=false;
  while(!recv){
  while(nbytes < 17){
    if(Serial.available() > 0){
      bytes[nbytes] = Serial.read();
      nbytes++;
    }
    recv=true;
  }
  }

 
/**
 * As LASER coordinates, we receive string like movem_-4.5434_-2.5254.
 * This string is chunked to extract ac and alt values. 
 */

  String data= String(bytes);
  data.trim();
  int len=data.length();
  m_data=data;
  sac_data=data;
  salt_data=data;
  m_data.remove(1,len-1);
  sac_data.remove(0,2);
  len=sac_data.length();
  sac_data.remove(7,len-7);
  len=salt_data.length();
  salt_data=salt_data.substring(10,17);    
}


void loop(){

 
  float ac, alt;
  char comm[5];
  int bytes_recv = 0;
  bool mov_end;                                                     
  comm[4]='\0';
  while(bytes_recv < 4){
    //Waiting for a command from serial monitor 
    if (Serial.available() > 0)
      comm[bytes_recv++] = Serial.read();
        
      digitalWrite(A1, HIGH);
  if(analogRead(smps)>=300)
  {
    if(smps_on==0)
    //Serial.println("turning on motor relay");
    {
    delay(1000);
  digitalWrite(relay1, LOW);
  digitalWrite(relay2, LOW);
  digitalWrite(relay3, LOW);
  digitalWrite(relay4, LOW);
  smps_on=1;
    }
    }
    else
    {
      
 // Serial.println("turning off motor relay");
  digitalWrite(relay1, HIGH);
  digitalWrite(relay2, HIGH);
  digitalWrite(relay3, HIGH);
  digitalWrite(relay4, HIGH);
  smps_on=0;
    }
  }
  
    
  if(strcmp(comm, "move")==0){
    Serial.println("float");
    getAcAlt();
    Serial.println("done_move");
    Axes.goToRads(sac_data.toFloat(),salt_data.toFloat());
  }
  
  if(strcmp(comm, "movl")==0){
    mov_end=Axes.movx(false,step1,dir1); 
    if(mov_end)
      Serial.println("done_movl");
  } 
  
  else if(strcmp(comm, "movr")==0){
    mov_end=Axes.movx(true,step1,dir1); 
    if(mov_end)
      Serial.println("done_movr");
  } 
     
  else if(strcmp(comm, "movu")==0){
    mov_end=Axes.movy(false,step2,dir2); 
    if(mov_end)
      Serial.println("done_movu");
  }
  
  else if(strcmp(comm, "movd")==0){
    mov_end=Axes.movy(true,step2,dir2); 
    if(mov_end)
      Serial.println("done_movd");
  }
  
  else if(strcmp(comm, "init")==0){
//    delay(1000);
//    digitalWrite(relay1,LOW);
//    digitalWrite(relay2,LOW);
//    digitalWrite(relay3,LOW);
//    digitalWrite(relay4,LOW);
    Axes.init();
    Serial.println("done_init");
  }
  
  else if(strcmp(comm, "laon")==0){
    laserOn(50);
    Serial.println("done_laon");
  }
  
  else if(strcmp(comm, "loff")==0){
    laserOff();   
    Serial.println("done_loff");
  }
  else if(strcmp(comm, "post")==0){
    ac=Axes.getX();
    alt=Axes.getY();
    Serial.print("t_");
    Serial.print(ac,6);
    Serial.print("_");
    Serial.println(alt,6);
  }
  
  else if(strcmp(comm,"stop")==0){
   Axes.stopper=1;
  }
     
  else if(strcmp(comm,"coad")==0){
    Axes.delayValue = 3; 
    Serial.println("done_high_speed"); 
  }
  
  else if(strcmp(comm,"fiad")==0){
    Axes.delayValue = 30; 
    Serial.println("done_low_speed");
  }
  else if(strcmp(comm,"clos")==0){
    //digitalWrite(relay1, HIGH);
    //digitalWrite(relay2, HIGH);
    //digitalWrite(relay3, HIGH);
    //digitalWrite(relay4, HIGH);
      //digitalWrite(13, HIGH);
    laserOff(); 
    Serial.println("done_close");
    
  }
  else if(strcmp(comm,"lase")==0){
    Serial.println("done_lase");
    char bytes[40];
    int nbytes = 0;
    bool recv=false;
    while(!recv){
      while(nbytes < 4){
        if(Serial.available() > 0){
          bytes[nbytes] = Serial.read();
          nbytes++;
    }
      recv=true;
  }
  }
  String data= String(bytes);
  int intensity=data.toInt();
  laserOn(intensity);
  }
  
  else{ 
  }
}