//  Dome Control ver01

// Include libraries

#include "Arduino.h"
#include "math.h"

// Pin definitions

#define SERIAL_RECEIVE 0
#define SERIAL_TRANSMIT 1
#define SERIAL1_RECEIVE 19
#define SERIAL1_TRANSMIT 18
#define COUNT_UP 2
#define COUNT_DOWN 3
#define NORTH_INDEX 21
#define MOTOR_POWER_COMMAND 6
#define DOME_SPEED_COMMAND 4
#define DOME_DIRECTION_COMMAND 7
#define AUTO_MANUAL_STATUS 5

//variable declarations

byte motor_mode = 1;  // system state variable 0-7
byte next_mode = 1;   // next state in state transition table
byte old_mode = 1;   // previous mode
byte motor_drive_speed = 0;  // PWM output to motor on pin4
byte motor_direction = 0;    // direction output to motor 0 anticlockwise 1 clockwise
byte target_direction = 0;   // calculated target direction
byte old_target_direction = 0;
unsigned int delta_position;  // this should never be more the 180


const byte interruptPin1 = COUNT_UP;
const byte interruptPin2 = COUNT_DOWN;
const byte interruptPin3 = NORTH_INDEX;
volatile int new_position = 280;
volatile int current_position = 280;
int target_position = 0;
char target_buffer[5];
char target_position_digit = 0;
unsigned long millisec;
unsigned long target_wait_timeout = 10000;
unsigned long start_time;
int dome_speed_level = 96;
int motor_pulse = 1000;
int min_count_pulse = 15000;    //microseconds, count pulse noise filter
int ii = 0;
int countpulse1 = 0;   //for testing
int countpulse2 = 0;   //for testing


// event vectors --  describe the state transition tables  //

const byte delta_0[] = {1,2,2,2,5,5,5,1};
const byte delta_1f[] = {1,2,3,4,1,1,1,1};
const byte delta_2f[] = {1,2,3,4,1,1,1,1};
const byte delta_3f[] = {2,2,3,4,1,1,1,1};
const byte delta_1r[] = {1,1,1,1,5,6,7,1};
const byte delta_2r[] = {1,1,1,1,5,6,7,1};
const byte delta_3r[] = {5,1,1,1,5,6,7,1};
const byte delta_position_limit[] = {1,10,50};
const byte motor_speed_level[] = {0,96,128,255,96,128,255};

void setup(){

  pinMode(interruptPin1, INPUT_PULLUP);
  pinMode(interruptPin2, INPUT_PULLUP);
  pinMode(interruptPin3, INPUT_PULLUP);
  pinMode(DOME_SPEED_COMMAND, OUTPUT);
  pinMode(DOME_DIRECTION_COMMAND, OUTPUT);
  pinMode(MOTOR_POWER_COMMAND, OUTPUT);
  pinMode(AUTO_MANUAL_STATUS, INPUT);
  
  attachInterrupt(digitalPinToInterrupt(COUNT_UP),count_up,RISING);
  attachInterrupt(digitalPinToInterrupt(COUNT_DOWN),count_down,RISING);
  attachInterrupt(digitalPinToInterrupt(NORTH_INDEX),north_index,RISING);
  Serial.begin(9600);
  Serial1.begin(2400);
  Serial.println("Dome Control Ver 1.1");
  motor_mode = 1;
  digitalWrite(MOTOR_POWER_COMMAND,HIGH);
}

void loop() {

do {      //just read and send status in manualmode 
  
    millisec = millis();
    if (get_target_position(target_wait_timeout)) {     //wait here for input
        Serial1.println(current_position);              //send current position to PC
    }
    
} while(!digitalRead(AUTO_MANUAL_STATUS));


current_position = new_position;

millisec = millis();

if (get_target_position(target_wait_timeout)) {     //wait here for input

  Serial1.println(current_position); //send current position to PC
  
  checkin();  // compute transition table input
  input_index();
  
  switch (motor_mode) {
    case 1:
      next_mode = delta_0[ii];
      break;
    case 2:
      next_mode = delta_1f[ii];
      break;
    case 3:
      next_mode = delta_2f[ii];
      break;
    case 4:
      next_mode = delta_3f[ii];
      break;
    case 5:
      next_mode = delta_1r[ii];
      break;
    case 6:
      next_mode = delta_2r[ii];
      break;
    case 7:
      next_mode = delta_3r[ii];
      break;
  }

  old_mode = motor_mode;
  motor_mode = next_mode;
  motor_drive_speed = motor_speed_level[motor_mode-1];
  if (old_target_direction != target_direction) {
    analogWrite(DOME_SPEED_COMMAND,0);   // Stop motor if changing direction
    delay(1000); //wait to stop 
    if (target_direction) digitalWrite(DOME_DIRECTION_COMMAND,HIGH);
    else digitalWrite(DOME_DIRECTION_COMMAND,LOW);
  }
  analogWrite(DOME_SPEED_COMMAND,motor_drive_speed);

  if (motor_drive_speed == motor_speed_level[1]) {    //pulse motor for small move
    delay(motor_pulse);
    analogWrite(DOME_SPEED_COMMAND,0);
  }
  
  trace();
    
 }
else {
  // if timeout waiting for target position from C14_cntrl computer 
  // stop motor 
  ii=7;
  next_mode = 1;
  motor_mode = next_mode;
  motor_drive_speed = 0;
  analogWrite(DOME_SPEED_COMMAND,motor_drive_speed);
  
  trace();
   
}



//*************   interrupt service routines ************* //
 
}

 void count_up() {
    for (int i=0; i<=2 ;i++) {
      delayMicroseconds(min_count_pulse);   //short pulse, noise filter
    }
    if (digitalRead(COUNT_UP)) {
      new_position++;
      new_position = fmod(new_position,360);
    }
 }

 void count_down() {
    for (int i=0; i<=2 ;i++) {
      delayMicroseconds(min_count_pulse);   //short pulse, noise filter
    }
    if (digitalRead(COUNT_DOWN)) {
      new_position--;
      new_position = fmod(new_position,360);
      if (new_position < 0) new_position = new_position + 360;
    }
 }

 void north_index() {
    for (int i=0; i<=2 ;i++) {
      delayMicroseconds(min_count_pulse);   //short pulse, noise filter
    }
    if (digitalRead(NORTH_INDEX)) new_position = 0;     
 }


//**********************************************************//
//****** wait here for next target position to arrive from C14-11-1 ***//

char get_target_position(long target_wait_timeout) {


long start_time;
long current_time;
int n;
char OK;

start_time= millis();
OK = 0;

do  {
    if (Serial1.available()) {
      size_t n = Serial1.readBytesUntil('\r',target_buffer,sizeof(target_buffer)-1);
      target_buffer[n] = '\0';
      target_position = atoi (target_buffer);
      current_position = new_position;
      OK = 1;
      break;
    }

} while ((millis()-start_time) < target_wait_timeout);

return OK;

}

//******* computed input, function of Target Position
//******* , Current Position and timeout error.

void checkin() {

  int f;  // unsigned forward (clockwise) distance
  int r;  // unsigned reverse (clockwise) distance

  old_target_direction = target_direction;
  f = target_position - current_position;
  if (f < 0) f = f+360;
  r = 360 - f;
  if (f < r) {
    delta_position = f;
    target_direction = 1;
  }
  else {
    delta_position = r;
    target_direction = 0;
  }

  return;
}

//******************generate input index

void input_index()

{
  if (delta_position == 0){ii=0; return;}
  if (delta_position >= delta_position_limit[0] && delta_position <= delta_position_limit[1]){ ii = 3*target_direction + 1; return;}
  if (delta_position > delta_position_limit[1] && delta_position < delta_position_limit[2]){ ii = 3*target_direction + 2; return;}
  if (delta_position >= delta_position_limit[1]) ii=3*target_direction + 3;

  return;
}
//*******************monitor variables in the loop

void trace() {

  Serial.print("Time2 ");
  Serial.print(millisec);
  Serial.print(" Current Position ");
  Serial.print(current_position); 
  Serial.print(" Target Position  ");
  Serial.println(target_position);
  Serial.print("delta_position ");
  Serial.print(delta_position);
  Serial.print(" target direction ");
  Serial.println(target_direction);
  Serial.print(" input index ");
  Serial.print(ii);
  Serial.print(" old mode ");
  Serial.print(old_mode);
  Serial.print(" motor mode ");
  Serial.print(motor_mode);
  Serial.print(" Motor drive level ");
  Serial.println(motor_drive_speed);
  //Serial.print(" count pulse levels ");
  //Serial.print(countpulse1);
  //Serial.println(countpulse2);
  
  return; 
}