//  Dome Control ver2-0
//  11/8/19  testing reverese pulse excess
// 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

bool A_M = false;             // auto/manual status from panel switch  true = auto
bool slip_abort = false;
byte slip_recovery_status = 1;
int slip_jump = 10;    
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 (reverse) 1 clockwise (forward)
byte target_direction = 0;   // calculated target direction
byte old_target_direction = 0;
byte slip_direction;
unsigned int target_distance;  // 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 old_position = 280;
int new_target_position;
int upcount = 0;
int downcount = 0;
int target_position = 0;
int abort_target_position;
char target_buffer[5];
int target_digits = 3;
char target_position_digit = 0;
unsigned long millisec;
unsigned long target_wait_timeout = 10000;
unsigned long pulse_width;
int slip_detect_status = 3;   //count of 0 speed and no-zero delta_position
int max_slip_detect_status = 3;     
float delta_time = 1.000;
int delta_position = 0;
float dome_speed = 0;
int motor_pulse = 1200;
int min_count_pulse = 15000;    //microseconds, count pulse noise filter
int ii = 0;
long loop_count = 0;

// 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 target_distance_limit[] = {1,4,30};
const byte motor_speed_level[] = {0,70,128,255,70,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,FALLING);
  Serial.begin(9600);
  Serial1.begin(2400);
  Serial.println("Dome Control ver02-0");
  //Serial.println("Time\tPOS\tTAR\tDelta\tDir\tIndex\tOld\tMotor\tLevel\tUp\tDown\tA/M");
  //Serial.println("Time\tPOS\tTAR\tDelta\tDir\tIndex\tOld\tslip#\tdpos\tdtime\tspeed\tA/M");
  motor_mode = 1;
  digitalWrite(MOTOR_POWER_COMMAND,HIGH);
  new_position = 280;
  target_wait_timeout = 10000;

}

void loop() {

  millisec = millis();

  A_M = digitalRead(AUTO_MANUAL_STATUS);

  switch (A_M)  {

    case false:                 //  just read and send status in manualmode 
                               // interrupts are still running 
      current_position = new_position;
      if (get_target_position(target_wait_timeout)) {     //wait here for input
        Serial1.println(current_position);              //send current position to PC
      }

      slip_abort = false;           // clear and abort status
      slip_detect_status = 5;       // and reset slip count status

      
      single_line_trace();
      
      break;
      
    case true:                 //  normal auto dome operation including slip recovery

      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();    // calculate pointer to transition table output
        if (!slip_abort) slip_detect();    // check for slipping friction wheel

        switch (motor_mode) {    // get next motor mode from state transition table
          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);
        }
  
        single_line_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);
        single_line_trace();
      }
      break;

  }
}

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


 void count_up() {

    long up_loop_count;

    up_loop_count=0;

    do {                  //make sure interrupt not caused by noise spike     
      up_loop_count++;
      if (up_loop_count>200000) break;      
    } while (digitalRead(COUNT_UP));
    
    if (up_loop_count > 100)  {
      new_position++;
      upcount++;
      new_position = fmod(new_position,360);
    }
 }

 void count_down() {     

    loop_count=0;

    do {                   //make sure interrupt not caused by noise spike
      loop_count++;
      if (loop_count>200000) break;
    } while (digitalRead(COUNT_DOWN));

    if (loop_count > 100)  { 
      new_position--;
      downcount++;
      new_position = fmod(new_position,360);
      if (new_position < 0) new_position = new_position + 360;
    }
 
 }

 void north_index() {

   long index_loop_count;

   index_loop_count = 0;
   
   do {                     //make sure interrupt not caused by noise spike
    index_loop_count++;
    if (index_loop_count > 200000) break;
   } while (!digitalRead(NORTH_INDEX));
      
   if (index_loop_count > 100) 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_digits=n;
      if (n == 3) {
        new_target_position = atoi (target_buffer);
        if (!slip_abort) target_position = new_target_position;       // don't update taget_position if slip abort
        OK = 1;
        break;
      }
    }

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

return OK;

}

//******* update target distance and target direction


void checkin() {

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

  current_position = new_position;                // first update current posion using new position from interrupt
  old_target_direction = target_direction;        // save target direction as old
  f = target_position - current_position;         // calculate shortest distance and directionb
  if (f < 0) f = f+360;
  r = 360 - f;
  if (f < r) {
    target_distance = f;
    target_direction = 1;
  }
  else {
    target_distance = r;
    target_direction = 0;
  }

  return;
}

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

void input_index() {		//generates pointer into the state transition table

  if (!slip_abort) {
    if (target_distance == 0){ii=0; return;}
    if (target_distance >= target_distance_limit[0] && target_distance <= target_distance_limit[1]){ ii = 3*target_direction + 1; return;}
    if (target_distance > target_distance_limit[1] && target_distance < target_distance_limit[2]){ ii = 3*target_direction + 2; return;}
    if (target_distance >= target_distance_limit[1]) ii=3*target_direction + 3;
  }
  else {			// this block runs in 'slip abort/recovery' mode
    if (target_distance == 0){		
      ii=0;
      if (slip_recovery_status == 1) {
        target_position = target_position + 2*slip_jump*(2*slip_direction-1);  //set new target back to original direction 10 past
        if (target_position < 0) target_position = target_position + 360;
        slip_recovery_status = 2;
      }
      else {
        slip_abort = false;            // restore to normal operation, hope slipping drive wheel fixed
      }
      return;
    }
    if (target_distance >= target_distance_limit[0] && target_distance <= target_distance_limit[1]){ ii = 3*target_direction + 1; return;}
    if (target_distance > target_distance_limit[1]){ ii = 3*target_direction + 3; return;}
  }
  return;
}

//****************************************************

void slip_detect()			//detects no movement and not at target

{

  delta_position = current_position - old_position;     // dome position change since last target received
  old_position = current_position;                      // reset old position after use
  delta_time = (millis() - millisec)/1000;              // elapsed time since last target received
  dome_speed = delta_position/delta_time;               //(should be) unsigned dome speed
  if (dome_speed == 0 && target_distance != 0) {        // if no movement and still not at target, slip alarm
    if (!slip_detect_status--) {                        // count slip alarms, if limit reached
      slip_abort = true;                                // signal alarm, initialise slip recovery
      ii=7;                                             // stop motor 
      new_target_position = target_position;            //save real target
      slip_direction = target_direction;
      target_position = current_position - slip_jump*(2*target_direction-1);  //set new target back about 10 degs.
      if (target_position < 0) target_position = target_position + 360;
    }
  }
  else {slip_detect_status = max_slip_detect_status;}

  return;
}

//*******************monitor variables in the loop
//****************single line printing

void single_line_trace() {

  Serial.print(millisec);Serial.print("\t");
  Serial.print(current_position);Serial.print("\t");
  Serial.print(target_position);Serial.print("\t");
  Serial.print(target_distance);Serial.print("\t");
  Serial.print(target_direction);Serial.print("\t");
  Serial.print(ii);Serial.print("\t");
 //
  Serial.print(old_mode);Serial.print("\t");
  Serial.print(motor_mode);Serial.print("\t");
  Serial.print(motor_drive_speed);Serial.print("\t");
  Serial.print(upcount);Serial.print("\t");
  Serial.print(downcount);Serial.print("\t");
  //
  //Serial.print(old_position);Serial.print("\t");
  //Serial.print(slip_detect_status);Serial.print("\t");
  //Serial.print(delta_position);Serial.print("\t");
  //Serial.print(delta_time);Serial.print("\t");
  //Serial.print(dome_speed);Serial.print("\t");

  Serial.println(A_M);
  return;
}

//************************************************************