#include "wiring_private.h"
#include "pins_arduino.h"

int poti[6];  // last cumulated poti values
int beyondHome[] = {319, 260, 626, 317, 383, 155}; // joint poti values beyond index pulse (boundary)
int index[6]; // stores the current EncI pulses (true if we are at index pulse)
boolean indexBefore[] = {false, false, false, false, false, false}; // to distinguish between raising and failing index edge
int i;
int potiIndex = 0;  // current poti that is in A/D conversion
boolean indexBackup[] = {false, false, false, false, false, false};
boolean isBeyond[6];

/* variables to check if homing can be initialized to start over again */
//int lastPoti[6];  // poti values of the last comparison but first we will work with index only
boolean indexFiredRecently = false; // stores if there was a pulse since last comparison
unsigned long lastMilisecs = 0; // for counting when next check takes place
unsigned long distToNextCheck = 4000;  // dist in millis when the next check takes place (4sec till next check)

/* homing has 4 phases - represented in the variable homingPhase:
  1. Search for home switch (depending on initial position the procedure directly starts with 2)
  2. Back Off home switch
  3. Final detection of switch
  4. Go to home position. Basically the home procedure is already at the end here
*/
byte homingPhase[6] = {0, 0, 0, 0, 0, 0};

int potiCumCounter[6] = {0, 0, 0, 0, 0, 0}; // counts how many intermediate poti values are already read before we calculate the final value as the average
int potiMaxIntermValues[6] = {0, 0, 0, 0, 0, 0}; //max value from the intermediate reads to substract at the end
int potiMinIntermValues[6] = {1024, 1024, 1024, 1024, 1024, 1024}; //min value from the intermediate reads to substract at the end
int potiCumIntermValues[6] = {0, 0, 0, 0, 0, 0}; //cumulated value from the intermediate reads to build average at the end
int potiValue;
int cumulationNumber = 10; // cumulate 10 values of the Poti

int debugHome = -1;


void setup()
{
  //Serial.begin(115200);
  //Serial.println("start: ");
  pinMode(8, OUTPUT);
  digitalWrite(8, 0);
  pinMode(9, OUTPUT);
  digitalWrite(9, 0);
  pinMode(10, OUTPUT);
  digitalWrite(10, 0);
  pinMode(11, OUTPUT);
  digitalWrite(11, 0);
  pinMode(12, OUTPUT);
  digitalWrite(12, 0);
  pinMode(2, INPUT);
  digitalWrite(2, HIGH);
  pinMode(3, INPUT);
  digitalWrite(3, HIGH);
  pinMode(4, INPUT);
  digitalWrite(4, HIGH);
  pinMode(5, INPUT);
  digitalWrite(5, HIGH);
  pinMode(6, INPUT);
  digitalWrite(6, HIGH);
  pinMode(7, INPUT);
  digitalWrite(7, HIGH);

  delay(1000); // wait till all voltage levels are stable – e.g. the voltages at the potis

  initializeHoming();
}

void loop()
{
  unsigned long millisec = 0;
  boolean allHomed = true;

  if (AR_Ready())
  {
    potiValue = AR_Value();
    potiCumIntermValues[potiIndex] = potiCumIntermValues[potiIndex] + potiValue;
    potiCumCounter[potiIndex]++;
    if (potiValue > potiMaxIntermValues[potiIndex]) {
      potiMaxIntermValues[potiIndex] = potiValue;  // store the biggest value
    }
    if (potiValue < potiMinIntermValues[potiIndex]) {
      potiMinIntermValues[potiIndex] = potiValue;  // store the smalles value
    }
    if (potiCumCounter[potiIndex] > cumulationNumber) {
      poti[potiIndex] =  (potiCumIntermValues[potiIndex] - potiMaxIntermValues[potiIndex] - potiMinIntermValues[potiIndex]) / (cumulationNumber - 1); // poti value is the average out of 10 measurements - hightest and lowest value deleted
      potiCumCounter[potiIndex] = 0; // next time start with cumulation again.
      potiMaxIntermValues[potiIndex] = 0;
      potiMinIntermValues[potiIndex] = 1024;
      potiCumIntermValues[potiIndex] = 0;

      isBeyond[potiIndex] = isBeyondHomeOffset(poti[potiIndex], beyondHome[potiIndex], isBeyond[potiIndex]);
      if (isBeyond[potiIndex] && homingPhase[potiIndex] == 1) {
        homingPhase[potiIndex] = 2; // entering phase 2 of the homing.
        setOutput(potiIndex, true);
        //debugInfo("isBeyondIndex ", potiIndex, poti[potiIndex], isBeyond[potiIndex], homingPhase[potiIndex], debugHome);
      } else if (!isBeyond[potiIndex] && homingPhase[potiIndex] == 0) {
        homingPhase[potiIndex] = 1; // entering normal home phase 1.
        setOutput(potiIndex, false);
        //debugInfo("isNowInNormalHomingPosition ", potiIndex, poti[potiIndex], isBeyond[potiIndex], homingPhase[potiIndex], debugHome);
      }

    }
    potiIndex ++;
    if (potiIndex > 5) {
      potiIndex = 0; // start from poti 0 again
    }
    AR_Start(potiIndex + 1); // start the reading of the next poti value
  }

  readIndex(index);
  for (int i = 0; i < 6; i++)
  {
    if (index[i] == true && indexBackup[i] == false) { // raising edge of an index signal
      indexFiredRecently = true;  // store that there are joints moving to prevent homeing from start over again
      digitalWrite(12, true); // indicate that home sequence is not initialzed so start homing is not possible.
      indexBackup[i] = true; // store the raising edge so it is not processed again
      if (homingPhase[i] == 3) {  // entering 4: in final detection: reached again the index pulse so we finally reached HOME_OFFSET
        homingPhase[i] = 4;
        setOutput(i, true);
        if (i > 2) {
          delay(10); // wait for 1 ms to switch off the output signal so the next axis (JT 5+6) can be homed. 
          setOutput(11, false);
        }
        //debugInfo("finalDetectionReached ", i, poti[i], isBeyond[i], homingPhase[i], debugHome);
      } else if (homingPhase[i] == 2 && isBeyond[i] == false ) { // first index pulse after poti is out of the isBeyond-area: we are at the end of back off phase
        setOutput(i, false);
        homingPhase[i] = 3;
        //debugInfo("is in Final Detection ", i, poti[i], isBeyond[i], homingPhase[i], debugHome);
      }
    } else if (index[i] == false && indexBackup[i] == true) { // store the failing edge of index
      indexBackup[i] = false;
      indexFiredRecently = true;  // store that there are joints moving to prevent homeing from start over again
      digitalWrite(12, true); // indicate that home sequence is not initialzed so start homing is not possible.
    }
/*    if (homingPhase[i] < 4 && i < 6) {
      allHomed = false;
    }*/
  }
/*  if (allHomed) { // homing for all JTs completed. Reset values to allow the next homeing
    delay(3000);  // before we reset the home switches
    initializeHoming();
  }*/ 

  // check if homing can be initialized again
  millisec = millis();
  if ((lastMilisecs + distToNextCheck) < millisec) {
    lastMilisecs = millisec;
    if (!indexFiredRecently) {  // there was no activity on index pulses since last
      digitalWrite(12, false); // indicate that homing will be initialized and can be homed.
      initializeHoming();
    }
    indexFiredRecently = false;
  }
}

void initializeHoming() {
  /* reset the whole procedure so homeing can start from the beginning
     initial check where the current poti values are */
  potiIndex = 0;
  for (int i = 0; i < 6; i++)
  {
    potiCumCounter[i] = 0; // next time start with cumulation again.
    potiMaxIntermValues[i] = 0;
    potiMinIntermValues[i] = 1024;
    potiCumIntermValues[i] = 0;
    poti[i] = readPoti(i);
    isBeyond[i] = isBeyondHomeOffset(poti[i], beyondHome[i], false);
    if (isBeyond[i]) {
      homingPhase[i] = 0; // is beyond the home offset so first back off before the normal procedure starts
      setOutput(i, true);
      if (i > 2) {
        digitalWrite(12, true); // indicate that JT4-6 or already beyond and can't be homed.
      }
    } else {
      homingPhase[i] = 1; // start with the normal homing procedure
      setOutput(i, false);
    }
    //debugInfo("first detection ", i, poti[i], isBeyond[i], homingPhase[i], debugHome);
  }
  AR_Start(1); // start the analog read on PIN A0. In the main loop the analog read is rotating from A0-A5

}
/*void debugInfo(String str, int idx, int poti, boolean isbey, byte homingPh, int onlyWhenIndex) {
  if (onlyWhenIndex < 0 || onlyWhenIndex == idx) {
    Serial.print(str);
    Serial.print(" idx: ");
    Serial.print(idx);
    Serial.print(" Poti: ");
    Serial.print(poti);
    Serial.print(" isBeyond: ");
    Serial.print(isbey);
    Serial.print(" homingPhase: ");
    Serial.print(homingPh);
    Serial.println();
  }
}
*/
int readPoti(int idx) {
  int cumulated = 0;
  int maxSteps = 30;
  for (int i = 0; i < maxSteps; i++) {
    cumulated += analogRead(idx + 1);
  }
  return cumulated / maxSteps;
}

boolean isBeyondHomeOffset(int poti, int boundary, boolean isActBeyond)
{
  int hysteresis = 3;
  if (poti < boundary)
  {
    return true;
  }
  else if ((poti - boundary < hysteresis) && isActBeyond)
  {
    return true;
  }
  else
  {
    return false;
  }
}

void readIndex(int indexValue[])
{
  for (int i = 0; i < 6; i++) {
    indexValue[i] = digitalRead(i + 2);
  }
}

void setOutput(int joint, boolean out)
{
  if (joint < 3) {
    digitalWrite(joint + 8, out);
  } else {
    digitalWrite(11, out);
  }
}


// uint8_t analog_reference = DEFAULT;

void AR_Start(uint8_t pin)

{

#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
  if (pin >= 54) pin -= 54; // allow for channel or pin numbers
#else
  if (pin >= 14) pin -= 14; // allow for channel or pin numbers
#endif

#if defined(ADCSRB) && defined(MUX5)
  // the MUX5 bit of ADCSRB selects whether we're reading from channels
  // 0 to 7 (MUX5 low) or 8 to 15 (MUX5 high).
  ADCSRB = (ADCSRB & ~(1 << MUX5)) | (((pin >> 3) & 0x01) << MUX5);
#endif
  // set the analog reference (high two bits of ADMUX) and select the
  // channel (low 4 bits).  this also sets ADLAR (left-adjust result)
  // to 0 (the default).
#if defined(ADMUX)
  ADMUX = (DEFAULT << 6) | (pin & 0x07);
#endif

  // without a delay, we seem to read from the wrong channel
  //delay(1);
  sbi(ADCSRA, ADSC);

}


boolean AR_Ready()
{

  // ADSC is cleared when the conversion finishes
  return bit_is_set(ADCSRA, ADSC) == 0;

}

int AR_Value()
{
  // we have to read ADCL first; doing so locks both ADCL
  // and ADCH until ADCH is read.  reading ADCL second would
  // cause the results of each conversion to be discarded,
  // as ADCL and ADCH would be locked when it completed.
  int low  = ADCL;
  int high = ADCH;
  // combine the two bytes
  return (high << 8) | low;

}