//****************************************************************************
//
//     Description of Software:   
//
//     Mitsubishi 3000gt/Stealth Controller Program
//     for controlling stock OEM mitsubishi ECS Struts.
//     Mitsubish 3000gt model years 1991 - 1999.
//     Note:models years 1995-1999 are retrofit of this system+Original Struts.
//     Hardware Platform:   ATAMega 328 MicroController
//
//     Copyright (C) 2010,2011,2012,2013,2014,2015,2016
//     Marcus Diaz, RenegadeTechWorks, LLC
//
//****************************************************************************
// Licensing:
//              Licensed under GNU GPL Version 2
//
//    This program is free software; you can redistribute it and/or modify
//    it under the terms of the GNU General Public License as published by
//    the Free Software Foundation under Version 2 of the License
//
//    This program is distributed in the hope that it will be useful,
//    but WITHOUT ANY WARRANTY; without even the implied warranty of
//    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//    GNU General Public License for more details.
//
//    You should have received a copy of the GNU General Public License along
//    with this program; if not, write to the Free Software Foundation, Inc.,
//    51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
//****************************************************************************
// VERSION HISTORY
//
//--------------------------------------------------------------------
// SW Ver  :  v3.00       
// HWDesign:  ECM M03 PCB Rev
// Date:      03/08/2016            
// Comments: 
//            This code represents a branch and re-write for the Model 03 of
//            of theTechWorks ECS controller.
//            The new Gen3/ Model03(M03) Controller uses a modified MO1 design
//            New Features are
//                -- freed  up I/O ports to support I2C buss devices
//                -- includes add on daughther board support for an DLH303 6-axis Accellerometer
//                -- Code has been rewritten in more Ojbect Oriented style.
//                -- Auto Mode added to use accelerometer data to auto set struts
//                -- Manual Mode remains
//              
//
//--------------------------------------------------------------------
// SW Ver  :  v3.01       
// HWDesign:  ECM M03 PCB Rev
// Date:      07/05/2016            
// Comments: 
//            - Added to startup sequence to retrieve from eprom the
//              default startup mode
//            - Added Sport/Tour light blinking patterns to startup sequence that
//              shows what default startup  mode is
//             - Added Set Led Brightness Mode
//            - Added Save current mode as default startup mode
//            - Modes are:
//              1 push - set next stiffnes setting
//              2 pushes - toggle auto/manual modes
//              3 pushes - set led brightness
//              4 pushes - save curreent mode as default startup mode
//              5 pushes - read diagnostics mode
//              6 pushes - run diagnostics mode
//--------------------------------------------------------------------
// SW Ver  :  v3.01       
// HWDesign:  ECM M03 PCB Rev 3.1
// Date:      07/09/2016            
// Comments: 
//            - fixed logic in  sportTourLoopLights routine to correctly display
//              command mode & signal wire failures
//            - changed manual strut mode to do struts one at a time
//
///--------------------------------------------------------------------
// SW Ver  :  v3.01       
// HWDesign:  ECM M03 PCB Rev 3.1
// Date:      07/16/2016            
// Comments: 
//            - changed manual strut mode back to parrallel mode
//            - change H/W design to use 1.5 amp 9v VReg for old strut
///--------------------------------------------------------------------
// SW Ver  :  v3.2
// HWDesign:  ECM M03 PCB Rev 3.1
// Date:      07/25/2016            
// Comments: 
//            - added modes to set threshold values
//            - added modes to Calibrate accelerometer
//            - added code to use accelerometer calibration values
//            - added code in setStrutMode to turn motors back on for old struts
///--------------------------------------------------------------------
// SW Ver  :  v3.3
// HWDesign:  ECM M03 PCB Rev 3.1
// Date:      10/17/2016            
// Comments: 
//            - added logging Mode
//            - added firmware version & startup values to print at startup
//            - added seperate maxtries params for Auto vs Manual strut modes
//            - added LED = OFF when strut state cannot be determined during transition
//            - added LED = GREENYELLOW if strut goes into mode but cannot stay there
//            - reworked the logic & assignments in readStrutState() - to match reality
//            - changed sport tour lights for automode to show max target across the four struts
///--------------------------------------------------------------------
// SW Ver  :  v3.4
// HWDesign:  ECM M03 PCB Rev 3.1
// Date:      7/31/2020               
// Comments: 
//            - changed Accelerometer adjustment algorithm from scaling to absolute based on Calibration Values.
//
///--------------------------------------------------------------------
// SW Ver  :  v3.5
// HWDesign:  ECM M03 PCB Rev 3.1
// Date:      8/3/2020  
// Comments: 
//            - changed Accelerometer adjustment algorithm from scaling to absolute based on Calibration Values.
//              and added range scaling based on Neg+Pos calibration ranges recorded.
//
///***********************************************************************************
#include
#include
#include
#include

//
//********* Firmware Version  ****************
//
#define FIRMWARE_VERSION "Firmware Version M03 v3.5"


//
//********* LSM303DLHC Accelerometer/ Compass I2C Addresses  ****************
//
#define LSM303_ADDRESS_ACC            (0x32 >> 1)         // 0011001x
#define LSM303_ADDRESS_MAG            (0x3C >> 1)         // 0011110x

#define MANUAL         1
#define AUTO                        2
#define DIAG         3
#define READDIAG       4
#define SETSTARTUP     5
#define SETLED         6
#define SETTHRESHOLD   7
#define CALIBRATEACCEL 8

#define NOMINAL     0
#define CHANGING    1
#define FAILED                 2
#define OLDSTRUTS   3
#define PRENOMINAL  4

#define STRUT_FL 0
#define STRUT_FR 1
#define STRUT_RL 2
#define STRUT_RR 3

#define HARD                 1
#define MEDIUM          2
#define SOFT  3
#define UNKNOWN  7
#define NOCHANGE 10
#define STARTUP  11

#define D_RED                     1
#define D_GREEN              2
#define D_BLUE                  3
#define D_TURQUOIS     4
#define D_PURPLE       5
#define D_YELLOW       6
#define D_GREENYELLOW  7
#define D_STARTUP      8
#define D_HARD         9
#define D_MEDIUM       10
#define D_SOFT         11
#define D_WHITE        12
#define D_OFF          13
#define D_UNKNOWN      14 


#define RED100   0xFF0000 // Bright Red
#define GREEN100 0x00FF00 // Bright Green
#define BLUE100  0x0000FF // Bright Blue
#define RED001   0x010000 // Faint red
#define RED050   0x800000 // 1/2 red (0x80 = 128 out of 256)
#define RED025   0x300000 // 25% RED
#define GREEN050 0x008000 // half Green
#define GREEN025 0x003000 // 25% GREEN
#define BLUE050  0x000080 // half Blue
#define BLUE025  0x000030 // 25% BLUE
#define WHITE015 0x151515 // 15% White
#define WHITE010 0x101010 // 10% White
#define WHITE005 0x050505 // 05% White
#define WHITE001 0x020202 // 05% White
#define WHITE100 0xFFFFFF // 100% White

#define LEDOFF                  0x000000
#define RED            0xFF0000
#define ORANGE         0xFF5500
#define ORANGEYELLOW   0xFFA000
#define YELLOW         0xFFFF00
#define YELLOWGREEN    0xA0FF00
#define GREENYELLOW    0x50FF00
#define GREEN          0x00FF00
#define GREENCYAN      0x00FF4B
#define CYAN           0x00FFFF
#define CYANBLUE       0x00A0FF
#define BLUECYAN       0x005AFF 
#define BLUE           0x0000FF
#define BLUEVIOLET     0x4800FF
#define VIOLETBLUE     0x7500FF
#define MAGENTA        0xFF00FF
#define PINKDEEP       0xFF1493
#define PINKHOT        0xFF69B4
#define PINK           0xF3967A
 
#define VIOLET         0xEE82EE


#define ERROR 1
#define NOERROR 0

#define CLEAR 1
#define DONT_CLEAR 0

#define SIGNAL_WIRE_FAILURE   2
#define MOTOR_COMMAND_FAILURE 1
#define NO_FAILURES 0

#define AUTO_MAXTRIES        475          // maxtries = 475 = approximately  2.75 secs of Motor on
                                                                   // AUTO_MAXTRIES time equivalent should be less than STRUT_HOLD_INTERVAL
#define MANUAL_MAXTRIES      10000   // maxtries = 10000 = approximately  4 secs of Motor on
#define MAXFAILURES   100                    // maxiumum number of faliures allowed for a strut during a power on cycle


#define MAXLIGHTLOOPS 20

#define ROTARY 1
#define PUSHBUTTON 2
#define SAMEMODE 1
#define NEXTMODE 2

#define LED_LEVEL1 1       // minimum brightness level
#define LED_LEVEL2 2
#define LED_LEVEL3 3
#define LED_LEVEL4 4       // Maximum brightness level

// ******************************************************************
//
//                   EPROM ADDRESSES
//
// ******************************************************************
#define EPROM_LEDMODE           1   // address in EPROM for default start up LED Brightness Level
#define EPROM_CONTROLLER_MODE   3   // address in EPROM for default start up Controller Mode (Auto or Manual)
#define EPROM_STRUT_MODE        5   // address in EPROM for default start up Strut Stiffness Mode
#define EPROM_MEDIUM_THRESHOLD  7   // address in EPROM for default start up Strut Stiffness Mode
#define EPROM_HARD_THRESHOLD    9   // address in EPROM for default start up Strut Stiffness Mode
#define EPROM_ACC_X_NEG_1G      11  // address in EPROM for Accelerometer Negative X axis 1G Calibration Value
#define EPROM_ACC_Y_NEG_1G      13  // address in EPROM for Accelerometer Negative Y axis 1G Calibration Value 
#define EPROM_ACC_Z_NEG_1G      15  // address in EPROM for Accelerometer Negative Z axis 1G Calibration Value 
#define EPROM_ACC_X_POS_1G      17  // address in EPROM for Accelerometer Positive X axis 1G Calibration Value 
#define EPROM_ACC_Y_POS_1G      19  // address in EPROM for Accelerometer Positive Y axis 1G Calibration Value 
#define EPROM_ACC_Z_POS_1G      21  // address in EPROM for Accelerometer Positive Z axis 1G Calibration Value 



///////////////////////////////////////////////////////////////////////////////////////
//
//  Map Board I/O Pins to World
//
// /////////////////////////////////////////////////////////////////////////////////////
// Front Left Strut Pins
#define STRUT_FL_SWITCH1  3
#define STRUT_FL_SWITCH2  2
#define STRUT_FL_MOTOR    10

// Front Right Strut Pins
#define STRUT_FR_SWITCH1  5
#define STRUT_FR_SWITCH2  4
#define STRUT_FR_MOTOR    11

// Rear Left Strut Pins
#define STRUT_RL_SWITCH1  7
#define STRUT_RL_SWITCH2  6
#define STRUT_RL_MOTOR    14

// Rear Right Strut Pins
#define STRUT_RR_SWITCH1  9
#define STRUT_RR_SWITCH2  8
#define STRUT_RR_MOTOR    15

//
// LED Light Ports
//
#define DATA_PIN 16    // Data pin that led data will be written out over

//
// Switch State
//
#define NO_PUSH_TO_PROCESS   0
#define NEW_PUSH_OCCURED     1
#define CONTINUE_PROCESSING  2



#define ACC_DATA_POINTS      10   // maximum number of Acceleration DataPoints
#define STRUT_HOLD_INTERVAL  3000 // numbers of milliseconds to hold a strut higher state;

#define X_AXIS 0   // Acceleromter X Axis in Array
#define Y_AXIS 1   // Acceleromter Y Axis in Array
#define Z_AXIS 2   // Acceleromter Z Axis in Array

//
// Logging modes
//
#define LOG_TIME              1
#define LOG_STRUTS_NAME       2
#define LOG_ACCELEROMETER     3

//***************************************************************************************
//
//   Create and Intialize (some ) Global Variables
//
//***************************************************************************************

//
// Map Physical Accelerometer axis & sign to normalized variables
//

int ForwardAxis        = Y_AXIS;      // default to Y axis
int ForwardSign        = 1;      // default to Y+ +Acc = Acceleration

int LeftRightAxis      = Z_AXIS;      // default to Z axis
int LeftRightSign      = 1;      // default to Z+ +Acc = Left Turn

int UpDownAxis         = X_AXIS;      // default to X axis
int UpDownSign         = 1;      // default to X+ +Acc = Up
int accelDataPoints    = 10;     // default number of acceleration data points to average

float ForwardAccelerationArray[ACC_DATA_POINTS] ;
float LeftRightAccelerationArray[ACC_DATA_POINTS] ;
float UpDownAccelerationArray[ACC_DATA_POINTS] ;

float ForwardAcceleration;
float LeftRightAcceleration;
float UpDownAcceleration;

// Accelerometer Realtime Reading Calibration Adjustment values
float xadj,yadj,zadj;          
long XaxisAccCalibrationRange, YaxisAccCalibrationRange, ZaxisAccCalibrationRange;

//
// X Y Z Axis Calibration Variables
//
int XAxisNegative1gCalibration  = 0;
int YAxisNegative1gCalibration  = 0;
int ZAxisNegative1gCalibration  = 0;
int XAxisPositive1gCalibration  = 0;
int YAxisPositive1gCalibration  = 0;
int ZAxisPositive1gCalibration  = 0;

//
// variables to contain integer level value that corresponds to actual decimal value of threshold
// 1 = 0.1g   2= 0.2g    3= 0.3g    4= 0.4g     5 =0.5g    6= 0.6g   7=0.7g
int mediumThresholdLevel;
int hardThresholdLevel;
//
// Default Threshold variables for Auto Mode Strut transitions in G's of force
//
float mediumThreshold = 0.2;
float hardThreshold = 0.4;

//Mode Selector Switch Pin
#define SELECTOR A3        // A3=Analog pin A3=physical pin 26 on AT328)  is part of the PinChangeInt.h library defines
int selectorSwitchPin = 17;

//Switch Type Selector Option Pin
int switchTypeSelect = 1;

//LED Type Selector Option Pin
int LEDTypeSelect = 0;

// Sport Light Port
int SportLight = 12;

// Tour Light Port
int TourLight = 13;

//Print Mode  1=log/print 0=no logging
int logMode = 0;
int logSequence = 0;
 
long ledHardValue ;
long ledMediumValue;
long ledSoftValue;
long ledRedValue;

#define FL_LED  0    // Front Left Strut LED
#define FR_LED  1    // Front Right Strut LED
#define RL_LED  2    // Rear Left Strut LED
#define RR_LED  3    // Rear Right Strut LED

int lightLoopCounter = MAXLIGHTLOOPS;
int lightLoopCounterValue;

int accelerometerStrutTargets[4];

int previousMode;
//
//  Allocate Structure For LED Strip
//
#define NUM_LEDS 8
struct CRGB ledColors[NUM_LEDS];
long ledCurrentValue[4];


unsigned long lightCycleStartTime =0;
unsigned long signalWireFailureCycleStartTime;
unsigned long commandFailureCycleStartTime;

// **********************************
//  Selector Switch variables & Intterupt function
// **********************************

/*
// Notice that anything that gets modified inside an interrupt, that I wish to access
// outside the interrupt, is marked "volatile". That tells the compiler not to optimize
// them.
*/
volatile uint8_t pushCount=0;
volatile uint8_t pushEvent= NO_PUSH_TO_PROCESS;
volatile unsigned long pushTime=0;

int firstTime=1;

//
// Interupt functgion for Selector Switch
//
void selectorInterruptfunc() {
  pushCount = pushCount +1;
  pushTime=millis();
  pushEvent = NEW_PUSH_OCCURED;
}
 


// **********************************
//  Strut Object Class Definition
// **********************************
class Strut{
                public:
                                Strut(){


                                }

                                ~Strut(){}

                                //************************************
                int strutPosition() {
                                                return vstrutPosition;
                                }
                int setStrutPosition(int position) {
                                                vstrutPosition = position;
                                }
                                //************************************
                int desiredState() {
                                                return vdesiredState;
                                }
                int setDesiredState(int target) {
                                     vdesiredState = target;
                                }             
                                //************************************
                unsigned long lastChangeTime() {
                                                return vlastChangeTime;
                                }
                void setLastChangeTime() {
                                     vlastChangeTime= millis();
                                }             
                                //************************************
                int actualState(int display) {
                                                vstate = readStrutState(vstrutPosition,display);
                                                return vstate;
                                }
                                //***********************************
                int transitionStatus() {
                                                return vtransitionStatus;
                                }
                int setTransitionStatus(int status) {
                                                vtransitionStatus = status;
                                }
                                //************************************
                int switch1() {
                                                return vs1Pin;
                                }

                int setSwitch1(int pin) {
                                                vs1Pin = pin;
                                }
                                //************************************
                int switch2() {
                                                return vs2Pin;
                                }
                int setSwitch2(int pin) {
                                                vs2Pin = pin;
                                }
                                //************************************
                int motorPin() {
                                                return vmotorPin;
                                }
                int setMotor(int pin) {
                                                vmotorPin = pin;
                                }
                int motorOn() {
                               digitalWrite(vmotorPin, HIGH);
                                }
                int motorOff() {
                               digitalWrite(vmotorPin, LOW);
                                }
                                //************************************
                int tryCount() {
                                                return vtryCount;
                                }
                void clearTryCount() {
                                                vtryCount =0;
                                }
                void incrementTryCount() {
                                                vtryCount++;
                                }
                                //************************************
                int failures() {
                                                return vtotalFailures;
                                }
                void clearFailures() {
                                     vtotalFailures = 0;
                                }
                void incrementFailures() {
                                                vtotalFailures++;
                                }

                                //************************************
                int errorStatus() {
                                                return verrorStatus;
                                }
                void setErrorStatus(int status) {
                                                verrorStatus = status;
                                }



                private:
                                int vstrutPosition;     // Strut Postion:  STRUT_FL _FR _RL _RR                                  
                                int vstate;             // Actual State of the Struts:  HARD, MEDIUM, SOFT
                                int vdesiredState;     // Desired Target State based on last command loop execution: H, M, S
                               
                                int vtransitionStatus;  // Indicate if the Strut in transition from last command mode to a new desired target or
                                                       // Or is stable and in quiet state
                                                                       // Modes Are:
                                               // NOMINAL  =  Strut Actual Mode matches the DesiredCommand Target.
                                                                       //             Motors should be off
                                                                       // CHANGING =  Strut In a commanded transitional state changing
                                                                       //              from Medium to Hard for example. Motors Should be On
                                                                       // Neutral  =  Everything has been shut down and Motors are off.  Command Target is not guaranteed
                                                                       //             Strut is whatever state it is in.
                                                                      
                                int verrorStatus = NO_FAILURES; // Indicates current Error Status of Strut Possible Error Status modes are
                                              //   NO_FAILURES    Means no failure has previously Been detected Since Last Command given
                                                              //   SIGNAL_WIRE_FAILURE     Means a signal wire failure has been previously detected
                                              //   MOTOR_COMMAND_FAILURE   Means a Motor Command failure has been previously detected  
                                                                                     
                                int vtryCount = 0;      // Indicates how many times previously a Strut has has passed thru the command loop
                                                        // while trying to achive the desired targetState
                                                                      
                                int vtotalFailures = 0; // Indicates how many total times previously a Strut has exceeded maxtries/tryCount
                                                        // while trying to achive the desired targetState

                                int vs1Pin;
                                int vs2Pin;
                                int vmotorPin;
                                unsigned long vlastChangeTime;
};

// *********************************************************
//  Controller Object  Class Definition
// *********************************************************
class Controller{
                public:
                                Controller(){


                                }

                                ~Controller(){}

                                int readSwitch(){
                                                return(readControllerSwitch());
                                }
                               
                int mode() {
                                                return vcontrollerMode;
                                }
                int setMode(int mode) {
                                                vcontrollerMode = mode;
                                }
                int strutMode() {
                                                return vcontrollerStrutMode;
                                }
                int setControllerStrutMode(int mode) {
                                                vcontrollerStrutMode = mode;
                                }

                int selectorType() {
                                                return(vselectorType);
                                }
                int setSelectorType(int type) {
                                     vselectorType = type;
                                }

                                //
                // Maybe put read switch as function of Controller??
                                //
                private:
                                int vcontrollerMode;           // Auto, Manual, Diag
                                int vcontrollerStrutMode;      // Hard, Med, Soft
                int vselectorType = ROTARY;    // selectorType captures what kind of selector switch is hooked up to unit
};



//
// ** instantiate & Initialize Strut Array Object **
//
Strut myStruts[4];


//
// ** instantiate & Initialize Controller Array Object **
//
Controller myController;


int ledMode = LED_LEVEL2;


// Previous State of Selector Switch
int currentSelectorState = STARTUP;


///////////////////////////////////////////////////////////////////////////////////////
// setup initializes startup
///////////////////////////////////////////////////////////////////////////////////////
void setup(){

        int strutStateFL;
        int strutStateFR;
        int strutStateRL;
        int strutStateRR;

        int selectorValue;
        int diagLoopNumber;
        int epromValue;

 
        // Initialize the Sport & Tour Light OutPut Pins
        pinMode(SportLight, OUTPUT);
        pinMode(TourLight, OUTPUT);
 
                //
        // intialize all the motor & Switch state pins
                //
        myStruts[0].setSwitch1(STRUT_FL_SWITCH1);
        myStruts[0].setSwitch2(STRUT_FL_SWITCH2);
        myStruts[0].setMotor(STRUT_FL_MOTOR);
        myStruts[0].setStrutPosition(STRUT_FL);
        myStruts[1].setSwitch1(STRUT_FR_SWITCH1);
        myStruts[1].setSwitch2(STRUT_FR_SWITCH2);
        myStruts[1].setMotor(STRUT_FR_MOTOR);
        myStruts[1].setStrutPosition(STRUT_FR);
        myStruts[2].setSwitch1(STRUT_RL_SWITCH1);
        myStruts[2].setSwitch2(STRUT_RL_SWITCH2);
        myStruts[2].setMotor(STRUT_RL_MOTOR);
        myStruts[2].setStrutPosition(STRUT_RL);
        myStruts[3].setSwitch1(STRUT_RR_SWITCH1);
        myStruts[3].setSwitch2(STRUT_RR_SWITCH2);
        myStruts[3].setMotor(STRUT_RR_MOTOR);
        myStruts[3].setStrutPosition(STRUT_RR);

        for (int i=0; i < 4; i++){
            initializeMotorIOPin(myStruts[i].motorPin());
                    initializeStrutIOPin(myStruts[i].switch1());
                    initializeStrutIOPin(myStruts[i].switch2());
                }

                //
                // initialize current led value array
                //
        for (int i=0; i < 4; i++){
            ledCurrentValue[i] = 0x00;
                }
    
        //
        //
        // Initialize Accelerometer
        //
        Wire.begin();  // Start up I2C, required for LSM303 communication
        initAccelerometer();


        // ************************************************************
        //
        // LED Setup & Initialization
        //
        // ************************************************************
               

        FastLED.addLeds(ledColors, NUM_LEDS);

        // Clear Out the LED arrays
        memset(ledColors, 0,  NUM_LEDS * sizeof(struct CRGB));  
        LEDS.show(); //Push the current color frame to the LEDs

                //
                // initialize current led value array
                //
        for (int i=0; i < 4; i++){
            ledCurrentValue[i] = 0x00;
                }

                // analyze Selector TYPE Input Pin To figure out if Rotary of Pushbutton is connected
        // HIGH = Rotary   LOW = PUSHBUTTON
        pinMode(switchTypeSelect, INPUT);
        digitalWrite(switchTypeSelect,HIGH);

        // Read the pin to see if it's HIGH or LOW     
        if(digitalRead(switchTypeSelect) == HIGH){
             myController.setSelectorType(PUSHBUTTON);

                     /*  original non-interrupt switch config
             pinMode(selectorSwitchPin, INPUT);
                     */
                    
             //
             // new code to use interrupt for pushbutton switch
             //
             pinMode(SELECTOR, INPUT_PULLUP);
             attachPinChangeInterrupt(SELECTOR, selectorInterruptfunc , RISING);     // Rising state change will trigger the interrupt.
             // attachPinChangeInterrupt(SELECTOR, selectorInterruptfunc , CHANGE);  // Any state change will trigger the interrupt.
             // attachPinChangeInterrupt(SELECTOR, selectorInterruptfunc , FALLING); // Falling state change will trigger the interrupt.
/*
             digitalWrite(selectorSwitchPin, HIGH);
*/

             lightLoopCounterValue = 900; // the lightloopcounter is timing dependent based on selector type
             diagLoopNumber = 450;
        }else{
             myController.setSelectorType(ROTARY);
             pinMode(selectorSwitchPin, INPUT);
             digitalWrite(selectorSwitchPin, LOW);
             lightLoopCounterValue = 60;  // the lightloopcounter is timing dependent based on selector type
             diagLoopNumber = 40;
        }
        lightLoopCounter = lightLoopCounterValue;


                //
                // Retrieve LED Brigtness level from EPROM - if not within range set it to 2nd level and write it back
                //
        ledMode = eepromReadInt(EPROM_LEDMODE);        

                //
                // Make sure the value is with in range
                //
                if (ledMode != LED_LEVEL1 && ledMode != LED_LEVEL2 && ledMode != LED_LEVEL3 && ledMode != LED_LEVEL4){
                    // It's not within range - set to LEVEL two and write it back
                    ledMode = LED_LEVEL2;
            eepromWriteInt(EPROM_LEDMODE,ledMode);
                }

                //
                // Set the LED brightness values based on EProm value
                //
                setLedValues();

               
        // EPROM_MEDIUM_THRESHOLD
        // EPROM_HARD_THRESHOLD 
               
                //
                // Retrieve medium threshold level from EPROM - if not within range set it to 2nd level and write it back  
                //
        mediumThresholdLevel = eepromReadInt(EPROM_MEDIUM_THRESHOLD );          
                //
                // Make sure the value is with in range
                //
                if (mediumThresholdLevel < 1 ||   mediumThresholdLevel > 7 ){
                    // It's not within range - set to LEVEL two and write it back
                   mediumThresholdLevel = 2 ;
            eepromWriteInt(EPROM_MEDIUM_THRESHOLD, mediumThresholdLevel );
                }
                //
                // Set g threshold level based on EEProm
                //
                mediumThreshold = float( mediumThresholdLevel/10.0)  ;
 
                //
                // Retrieve hard threshold level from EPROM - if not within range set it to 2nd level and write it back    
                //
        hardThresholdLevel = eepromReadInt(EPROM_HARD_THRESHOLD );         
                //
                // Make sure the value is with in range
                //
                if (hardThresholdLevel < 1 ||   hardThresholdLevel > 9 ){
                    // It's not within range - set to LEVEL two and write it back
                   hardThresholdLevel = 4 ;
           eepromWriteInt(EPROM_HARD_THRESHOLD, hardThresholdLevel );
                }
                //
                // Set g threshold level based on EEProm
                //
                hardThreshold = float( hardThresholdLevel/10.0)  ;
 

               
                //
                // Retrieve Strut Mode from EPROM
        epromValue= eepromReadInt(EPROM_STRUT_MODE);

        /*
        Serial.print(F("    Eprom Strut Mode ="));
        Serial.println(epromValue);
        */

                //
                // if not valid eprom value set it to Soft;       
                //
                //
                if (epromValue != HARD && epromValue != MEDIUM && epromValue != SOFT ){
                    // It's not within range - set to Soft as default
                    epromValue = SOFT;
            eepromWriteInt(EPROM_STRUT_MODE,epromValue);
                }
                myController.setControllerStrutMode(epromValue);

               


       //
       // Retrieve X Axis Neg 1g Galibration Value From Eprom if not with in range set to default value and write back
       //
       XAxisNegative1gCalibration= eepromReadInt(EPROM_ACC_X_NEG_1G);  
       if (XAxisNegative1gCalibration >32000 || XAxisNegative1gCalibration < 1  ){
           // It's not within range - set to default   and write it back
                   XAxisNegative1gCalibration = 16380;
           eepromWriteInt(EPROM_ACC_X_NEG_1G , XAxisNegative1gCalibration);
       }

       //
       // Retrieve Y Axis Neg 1g Galibration Value From Eprom if not with in range set to default value and write back
       //
       YAxisNegative1gCalibration= eepromReadInt(EPROM_ACC_Y_NEG_1G);  
       if (YAxisNegative1gCalibration >32000 || YAxisNegative1gCalibration < 1  ){
           // It's not within range - set to default   and write it back
                   YAxisNegative1gCalibration = 16380;
           eepromWriteInt(EPROM_ACC_Y_NEG_1G , YAxisNegative1gCalibration);
       }

       //
       // Retrieve Z Axis Neg 1g Galibration Value From Eprom if not with in range set to default value and write back
       //
       ZAxisNegative1gCalibration= eepromReadInt(EPROM_ACC_Z_NEG_1G);   
       if (ZAxisNegative1gCalibration >32000 || ZAxisNegative1gCalibration < 1  ){
           // It's not within range - set to default   and write it back
                   ZAxisNegative1gCalibration = 16380;
           eepromWriteInt(EPROM_ACC_Z_NEG_1G , ZAxisNegative1gCalibration);
       }

       //
       // Retrieve X Axis Positive 1g Galibration Value From Eprom if not with in range set to default value and write back
       //
       XAxisPositive1gCalibration= eepromReadInt(EPROM_ACC_X_POS_1G);    
       if (XAxisPositive1gCalibration > 32000 || XAxisPositive1gCalibration  < 1  ){
           // It's not within range - set to default   and write it back
                   XAxisPositive1gCalibration  = 16380;
           eepromWriteInt(EPROM_ACC_X_POS_1G , XAxisPositive1gCalibration  );
       }

       //
       // Retrieve Y Axis Positive 1g Galibration Value From Eprom if not with in range set to default value and write back
       //
       YAxisPositive1gCalibration= eepromReadInt(EPROM_ACC_Y_POS_1G);    
       if (YAxisPositive1gCalibration > 32000 || YAxisPositive1gCalibration  < 1  ){
           // It's not within range - set to default   and write it back
                   YAxisPositive1gCalibration  = 16380;
           eepromWriteInt(EPROM_ACC_Y_POS_1G , YAxisPositive1gCalibration  );
       }

       //
       // Retrieve Z Axis Positive 1g Galibration Value From Eprom if not with in range set to default value and write back
       //
       ZAxisPositive1gCalibration= eepromReadInt(EPROM_ACC_Z_POS_1G);     
       if (ZAxisPositive1gCalibration > 32000 || ZAxisPositive1gCalibration  < 1  ){
           // It's not within range - set to default   and write it back
                   ZAxisPositive1gCalibration  = 16380;
           eepromWriteInt(EPROM_ACC_Z_POS_1G , ZAxisPositive1gCalibration  );
       }

        //
        // Calculate Accelerometer global calibration adjustments
        //
        XaxisAccCalibrationRange = (long(XAxisPositive1gCalibration) + long(XAxisNegative1gCalibration))/2;
        YaxisAccCalibrationRange = (long(YAxisPositive1gCalibration) + long(YAxisNegative1gCalibration))/2;
        ZaxisAccCalibrationRange = (long(ZAxisPositive1gCalibration) + long(ZAxisNegative1gCalibration))/2;


        xadj =  -float(XAxisPositive1gCalibration) + float(XaxisAccCalibrationRange);
        yadj =  -float(YAxisPositive1gCalibration) + float(YaxisAccCalibrationRange);
        zadj =  -float(ZAxisPositive1gCalibration) + float(ZaxisAccCalibrationRange);

                //
                // Turn on the Sport / Tour Lights at startup for 1 sec as test to driver to show they are working.
                //
        setLights(SportLight,HIGH);
        setLights(TourLight,HIGH);
        delay(1000);         
        // Turn off Lights
        setLights(SportLight,LOW);
        setLights(TourLight,LOW);
        delay(300);            

               
                //
                // Retrieve Controller Mode from EPROM
                //
        epromValue= eepromReadInt(EPROM_CONTROLLER_MODE);
                /*
        Serial.print(F("Eprom Controller Mode ="));
        Serial.print(epromValue);
                */
                //
                // if controller mode not Auto or Manual set to manual
                //
                if (epromValue != AUTO && epromValue != MANUAL ){
                    // It's not within range - set to Manual as default
                    epromValue = MANUAL;
            eepromWriteInt(EPROM_CONTROLLER_MODE,epromValue);
                }
                myController.setMode(epromValue);

                //
                // Based on Controller default startup mode flash lights accordingly
                //
                if(epromValue == AUTO){

             //
             // Blink both lights alternating(3 times) to indicate auto mode
             //
                     for(int i=0;i<2;i++){
                       setLights(SportLight,HIGH);
               setLED(STRUT_FR,D_WHITE);

               setLights(TourLight,LOW);
               setLED(STRUT_FL,D_OFF);
                      
                       delay(250);
                       setLights(SportLight,LOW);
               setLED(STRUT_FR,D_OFF);

               setLights(TourLight,HIGH);
               setLED(STRUT_FL,D_WHITE);
                       delay(250);
                     }
                }else if (epromValue == MANUAL){
                                                     //
                     // Blink both lights at together (3 times) to indicate manual mode
             //
             blinkBothLights(3,250,1,0,1);
                                                    
                     // Turn off Lights
                     setLights(SportLight,LOW);
             setLights(TourLight,LOW);

                }


        lightCycleStartTime =millis();
        pushCount=0;

                //
                // ********* Set Up for Logging & Print some Initial Values
                //
        Serial.begin(115200);
        while (!Serial) {
            ; // wait for serial port to connect. Needed for Leonardo only
        };
               Serial.println(F("*************************************************************************"));
        Serial.println(F("Renegade Techworks Mitsubishi 3000gt/Stealth ECS Controller"));
        Serial.println(FIRMWARE_VERSION);
        Serial.print(F("Strut G-Force Medium Threshold:  "));
        Serial.println( mediumThreshold);
        Serial.print(F("Strut G-Force Hard Threshold:  "));
        Serial.println( hardThreshold );
                readPrintEpromValues();

}

/////////////////////////////////////////////////////////////////////////////////////
//
//            main loop 
//
/////////////////////////////////////////////////////////////////////////////////////

void loop()
{
               
   //
   // Check for updates to the state of the Controller Mode
   //  
   //
   myController.readSwitch();


   if(logMode) doLogMode(LOG_TIME,0,0);

   switch(myController.mode()){

                case(MANUAL):
            if(logMode) doLogMode(LOG_ACCELEROMETER,0,0);
                    setManualStrutTargets();  
            setStruts();      
            // setManualStruts();       
                    break;

                case(AUTO):
            readAccelerometerValues();   
            setAccelerometerTargets();
                    setAutoStrutTargets();
            setStruts();      
                    break;

                case(SETLED):
            setLed();
                    break;

                case(SETSTARTUP):
            setStartup();
                    break;
                   
                case(DIAG):
            runDiagnostics();
                    break;

                case(READDIAG):
            readDiagnostics();
                    break;
                   
                case(SETTHRESHOLD):
            setAutoThreshold();
                    break;

                case(CALIBRATEACCEL):
            calibrateAccelerometer();
                    break;

   }

   //
   // display current state of Sport-Tour lights
   //
   setLoopTourSportLights();
}

//*************************************************************************************
//
//
//    setAutoThreshold()
//
//
//************************************************************************************
void setAutoThreshold(){
                            
        // Turn off Lights while setting LEDs
        setLights(SportLight,LOW);
        setLights(TourLight,LOW);

                int autoThresholdSet = 0;

        mediumThresholdLevel = 2;
                hardThresholdLevel = 4;
               
        /*
                 * Stay in this loop until button is pressed multple times to indicate save
                 */          
                while(1){
                               
                                //read the selector PUSHBUTTON or Rotary and see if they have it in DIAGMODE
                switch(readPushButtonSwitch()){

                            case NOCHANGE:
                            break;

                    case 1:
                                          //
                                          // Ok they want to change to next Threshold Values
                                          //

                                          if(autoThresholdSet == 5) autoThresholdSet =1;
                                          else autoThresholdSet++;

                                          //
                                          // Blink both lights at together  x times to indicate which threshold set is selected
                                          //
                          blinkBothLights(autoThresholdSet,500,1,0,1);
                          switch(autoThresholdSet){
                                                                  case 1:
                                                                                  mediumThresholdLevel = 1;
                                                                                  hardThresholdLevel = 3;
                                                                                  break;

                                                                  case 2:
                                                                                  mediumThresholdLevel = 2;
                                                                                  hardThresholdLevel = 4;
                                                                                  break;
                                                                                 
                                                                  case 3:
                                                                                  mediumThresholdLevel = 3;
                                                                                  hardThresholdLevel = 5;
                                                                                  break;

                                                                  case 4:
                                                                                  mediumThresholdLevel = 4;
                                                                                  hardThresholdLevel = 6;
                                                                                  break;
                                                                                 
                                                                  case 5:
                                                                                  mediumThresholdLevel = 5;
                                                                                  hardThresholdLevel = 7;
                                                                                  break;
                                                                                 
                                                  }

                          break;

                    case 2:
                    case 3:
                    case 4:
                                          //
                                          // Ok they want to save the auto Threshhold values and exit
                                          //
                          eepromWriteInt(EPROM_MEDIUM_THRESHOLD,mediumThresholdLevel);
                          eepromWriteInt(EPROM_HARD_THRESHOLD,hardThresholdLevel);

                                  mediumThreshold = float( mediumThresholdLevel/10.0)  ;
                                  hardThreshold = float( hardThresholdLevel/10.0)  ;
                                 
                                          //
                                          // Blink both lights at together (7 times) to indicate saving Threshold mode
                                          //
                          blinkBothLights(7,500,1,0,1);
                          delay(500);
                                  //
                                  // Restore the previous state of the controller
                                  //
                                          myController.setMode(previousMode);
                                          return;

                          break;
                                }
                }
}

//*************************************************************************************
//
//
//    setLed()
//
//
//************************************************************************************
void setLed(){
                            
        // Turn off Lights while setting LEDs
        setLights(SportLight,LOW);
        setLights(TourLight,LOW);

                /*
                 *  Display LedValues
                 */
        setLedValues();

               
        /*
                 * Stay in this loop until button is pressed multple times to indicate save
                 */          
                while(1){
                               
                                //read the selector PUSHBUTTON or Rotary and see if they have it in DIAGMODE
                switch(readPushButtonSwitch()){

                            case NOCHANGE:
                            break;

                    case 1:
                                          //
                                          // Ok they want to change to next LED Value
                                          //

                                          if(ledMode == 4) ledMode =1;
                                          else ledMode++;
                                 
                                          setLedValues();
                          break;

                    case 2:
                    case 3:
                    case 4:
                                          //
                                          // Ok they want to save the current LED Brightness value and exit
                                          //
                          eepromWriteInt(EPROM_LEDMODE,ledMode);
                                 
                                          //
                                          // Blink both lights at together (3 times) to indicate LED mode
                                          //
                          blinkBothLights(3,500,1,0,1);

                                  //
                                  // Restore the previous state of the controller
                                  //
                                          myController.setMode(previousMode);
                                          return;

                          break;
                                }
                }
}


//*************************************************************************************
//
//
//   setLedValues
//
//
//************************************************************************************
void setLedValues(){
               
           switch(ledMode){
                      case LED_LEVEL1:
                                       //SET the LEDs to Minimum Brightness Mode
                         ledHardValue = rgbEncode(2,0,2);
                         ledMediumValue = rgbEncode(0,2,2);
                         ledSoftValue = rgbEncode(0,0,2);
                         ledRedValue = rgbEncode(2,0,0);
 
                         setLED(FL_LED, D_HARD);
                         setLED(FR_LED, D_MEDIUM);
                         setLED(RL_LED, D_SOFT);
                         setLED(RR_LED, D_RED);
                      break;

                      case LED_LEVEL2:
                                       //SET the LEDs to 2nd to Minimum Brightness Mode 
                         ledHardValue = rgbEncode(10,0,10);
                         ledMediumValue = rgbEncode(0,10,5);
                         ledSoftValue = rgbEncode(0,0,10);
                         ledRedValue = rgbEncode(10,0,0);
 
                         setLED(FL_LED, D_HARD);
                         setLED(FR_LED, D_MEDIUM);
                         setLED(RL_LED, D_SOFT);
                         setLED(RR_LED, D_RED);
                      break;

                      case LED_LEVEL3:
                                       //SET the LEDs to Medium Brightness Mode   
                         ledHardValue = rgbEncode(70,0,70);
                         ledMediumValue = rgbEncode(0,70,35);
                         ledSoftValue = rgbEncode(0,0,70);
                         ledRedValue = rgbEncode(70,0,0);
 
                         setLED(FL_LED, D_HARD);
                         setLED(FR_LED, D_MEDIUM);
                         setLED(RL_LED, D_SOFT);
                         setLED(RR_LED, D_RED);
                      break;
                                                  
                                      case LED_LEVEL4:
                                       //SET the LEDs to Maximum Brightness Mode
                         ledHardValue = rgbEncode(120,0,120);
                         ledMediumValue = rgbEncode(0,120,60);
                         ledSoftValue = rgbEncode(0,0,150);
                         ledRedValue = rgbEncode(120,0,0);
 
                         setLED(FL_LED, D_HARD);
                         setLED(FR_LED, D_MEDIUM);
                         setLED(RL_LED, D_SOFT);
                         setLED(RR_LED, D_RED);
                                                     break;
       }

}

//*************************************************************************************
//
//
//    setSTARTUP()
//
//
//************************************************************************************
void setStartup(){
        int smode;
                //
                // Save current controller mode
                //
        eepromWriteInt(EPROM_CONTROLLER_MODE,previousMode);
       
                /*
        Serial.print(F("    Saving Controller Mode ="));
        Serial.print(previousMode);
        */                   

                //
                // Save current current controller strut mode
                //
                smode = myController.strutMode();
        eepromWriteInt(EPROM_STRUT_MODE,smode);

                /*
        Serial.print(F("    Saving Strut Mode ="));
        Serial.println(smode);
        */                   

                //
                // Restore the previous state of the controller
                //
                myController.setMode(previousMode);   

               
}
//*************************************************************************************
//
//
//    setStruts()
//
//
//************************************************************************************
void setStruts(){

     for(int strut = 0; strut < 4; strut++){
                     if(logMode) doLogMode(LOG_STRUTS_NAME , strut,0);
                     setStrutMode(strut);
     }
               
}

//*************************************************************************************
//
//
//    setManualStruts()
//
//
//************************************************************************************
void setManualStruts(){

     for(int strut = 0; strut < 4; strut++){
             setStrutDirectMode( strut, myStruts[strut].desiredState(), DONT_CLEAR);
     }
               
}

//*************************************************************************************
//
//
//    setManualStrutsTargets()
//
//
//************************************************************************************
void setManualStrutTargets(){

     for(int strut = 0; strut < 4; strut++){

       
                 //
                 // See if we are changing states
                 //
                 if (myStruts[strut].desiredState() != myController.strutMode()){

                     //
                     // Yes the human wants a new state - put it in motion
                     //
             myStruts[strut].setDesiredState(myController.strutMode());
                     myStruts[strut].setTransitionStatus(CHANGING);
             myStruts[strut].setLastChangeTime();
             myStruts[strut].clearTryCount();
             myStruts[strut].setErrorStatus(NO_FAILURES);
                 }else {
             //
                     // No Change - do nothing
             //
                 }
     }
               
}
//*************************************************************************************
//
//
//    setAutoStrutsTargets()
//
//
//************************************************************************************
void setAutoStrutTargets(){


     for(int strut = 0; strut < 4; strut++){
                 //
                 // See if we are changing states
                 //
                 if (myStruts[strut].desiredState() != accelerometerStrutTargets[strut]){

                     //
                     // Yes the accelerometer readings dictate a new state - put it in motion
                     //
             myStruts[strut].setDesiredState(accelerometerStrutTargets[strut]);
                     myStruts[strut].setTransitionStatus(CHANGING);
             myStruts[strut].clearTryCount();
             myStruts[strut].setErrorStatus(NO_FAILURES);
                 }else {
             //
                     // No Change - do nothing
             //
                 }
     }
               
}



//*******************************************************************************************
//
//
// Rountine to only allow a strut downward change to Medium or Soft
// until after the hold interval has passed
//
//
//*******************************************************************************************
void setAutoStrutState(int strut, int mode){
               
unsigned long currentTime = millis();

                if ( mode == SOFT && myController.strutMode() == MEDIUM) mode = MEDIUM;

                if (mode == HARD){
                                      
                                accelerometerStrutTargets[strut] = mode;
                myStruts[strut].setLastChangeTime();
               
                } else if (mode == MEDIUM){
                                if( accelerometerStrutTargets[strut] == HARD &&  currentTime < (myStruts[strut].lastChangeTime() + STRUT_HOLD_INTERVAL)) return;

                                accelerometerStrutTargets[strut] = MEDIUM;
                myStruts[strut].setLastChangeTime();

                               
                } else if (mode == SOFT){

                                if( (accelerometerStrutTargets[strut] == HARD ||accelerometerStrutTargets[strut] == MEDIUM)
                                     &&  currentTime < (myStruts[strut].lastChangeTime() + STRUT_HOLD_INTERVAL)) return;

                                accelerometerStrutTargets[strut] = SOFT;
                myStruts[strut].setLastChangeTime();
        }
}

//*******************************************************************************************
//
//
// Rountine to read current Accelerometer values and set Strut Targets
//
//
//*******************************************************************************************
void setAccelerometerTargets(){

    
     /*
      ForwardAcceleration  = 0.0;
      LeftRightAcceleration = 0.0;
      UpDownAcceleration = 0.0;
      STRUT_FL
      STRUT_FR
      STRUT_RL
      STRUT_RR
     */

      /*
      Serial.print(F("Forward Accleration ="));
      Serial.print(ForwardAcceleration );
      Serial.print(F("    Left Right Accleration ="));
      Serial.println(LeftRightAcceleration );
      */

     //
     // Check to see if we doing NO Acceleration or Braking ?
     //
     if (ForwardAcceleration > (-1)*mediumThreshold  && ForwardAcceleration < mediumThreshold ){
            //
            // No Foward or Braking Acceleration
            //

                   /*
           Serial.println(F("No Forward or Braking Acceleration"));
           */                   

                    if (LeftRightAcceleration > (-1)*mediumThreshold && LeftRightAcceleration                 //
                // No lateral Acceleration - set all struts to soft
                //
                        setAutoStrutState(STRUT_FL, SOFT);
                setAutoStrutState(STRUT_FR, SOFT);
                setAutoStrutState(STRUT_RL, SOFT);
                setAutoStrutState(STRUT_RR, SOFT);

            }else if (LeftRightAcceleration >= mediumThreshold && LeftRightAcceleration                 //
                //  Doing a MEDIUM  Left Turn at same time
                                //  Set right side struts to medium &
                //
                setAutoStrutState(STRUT_FL, SOFT);
                setAutoStrutState(STRUT_FR, MEDIUM);
                setAutoStrutState(STRUT_RL, SOFT);
                setAutoStrutState(STRUT_RR, MEDIUM);

            }else if (LeftRightAcceleration >= hardThreshold ) {
                //
                //  Doing a HARD Left Turn at same time-
                                //  set rear struts to SOFT & right side struts to Hard
                //
                setAutoStrutState(STRUT_FL, SOFT);
                setAutoStrutState(STRUT_FR, HARD);
                setAutoStrutState(STRUT_RL, SOFT);
                setAutoStrutState(STRUT_RR, HARD);
                    
            }else if (LeftRightAcceleration > (-1)*hardThreshold && LeftRightAcceleration <= (-1)*mediumThreshold ) {
                //
                //  Doing a MEDIUM RIGHT Turn at same time-
                                //  set rear to SOFT & left side struts to medium &
                //
                setAutoStrutState(STRUT_FL, MEDIUM);
                setAutoStrutState(STRUT_FR, SOFT);
                setAutoStrutState(STRUT_RL, MEDIUM);
                setAutoStrutState(STRUT_RR, SOFT);

            }else if (LeftRightAcceleration <= (-1)*hardThreshold ) {
                //
                //  Doing a HARD RIGHT Turn at same time-
                                //  set rear struts to SOFT & left side struts to Hard
                //
                setAutoStrutState(STRUT_FL, HARD);
                setAutoStrutState(STRUT_FR, SOFT);
                setAutoStrutState(STRUT_RL, HARD);
                setAutoStrutState(STRUT_RR, SOFT);
                    }
               

     //
     //Check to see if we are doing MEDIUM range  Forward Acceleration ?
     //
     } else if(ForwardAcceleration >= mediumThreshold  && ForwardAcceleration < hardThreshold ){
            //
            // Doing MEDIUM Forward Acceleration between mediumThresholdg's and hardThreshold
                    // In this range the Rear Struts will be set to Medium Mode
                    // And any Lateral accelerations due to turns will set the LEFT or RIGHT Struts
            //

                    /*
            Serial.println(F("MEDIUM Forward Acceleration"));
            */

                    if (LeftRightAcceleration > (-1)*mediumThreshold && LeftRightAcceleration                 //
                // No lateral Acceleration - set just rear struts to medium
                //
                setAutoStrutState(STRUT_FL, SOFT);
                setAutoStrutState(STRUT_FR, SOFT);
                setAutoStrutState(STRUT_RL, MEDIUM);
                setAutoStrutState(STRUT_RR, MEDIUM);

            }else if (LeftRightAcceleration >= mediumThreshold && LeftRightAcceleration                 //
                //  Doing a MEDIUM  Left Turn at same time
                                //  Set RIGHT side struts to medium &
                //
                setAutoStrutState(STRUT_FL, SOFT);
                setAutoStrutState(STRUT_FR, MEDIUM);
                setAutoStrutState(STRUT_RL, MEDIUM);
                setAutoStrutState(STRUT_RR, MEDIUM);

            }else if (LeftRightAcceleration >= hardThreshold ) {
                //
                //  Doing a HARD Left Turn at same time-
                                //  set RIGHT side struts to Hard
                //
                setAutoStrutState(STRUT_FL, SOFT);
                setAutoStrutState(STRUT_FR, HARD);
                setAutoStrutState(STRUT_RL, MEDIUM);
                setAutoStrutState(STRUT_RR, HARD);
                    
            }else if (LeftRightAcceleration > (-1)*hardThreshold && LeftRightAcceleration <= (-1)*mediumThreshold ) {
                //
                //  Doing a MEDIUM RIGHT Turn at same time-
                                //  set LEFT side struts to medium 
                //
                setAutoStrutState(STRUT_FL, MEDIUM);
                setAutoStrutState(STRUT_FR, SOFT);
                setAutoStrutState(STRUT_RL, MEDIUM);
                setAutoStrutState(STRUT_RR, MEDIUM);

            }else if (LeftRightAcceleration <= (-1)*hardThreshold ) {
                //
                //  Doing a HARD RIGHT Turn at same time-
                                //  set LEFT side struts to Hard
                //
                setAutoStrutState(STRUT_FL, HARD);
                setAutoStrutState(STRUT_FR, SOFT);
                setAutoStrutState(STRUT_RL, HARD);
                setAutoStrutState(STRUT_RR, MEDIUM);
                    }
 
     //
     //Check to see if we are doing Hard range  Forward Acceleration ?
     //
     } else if(ForwardAcceleration >= hardThreshold ){
                    //
            // Doing HARD Forward Acceleration greater than hardThresholdgs
                    // In this range the Rear Struts will be set to HARD Mode
                    // And any Lateral accelerations due to turns will set the LEFT or RIGHT Struts
            //

            /*
                     Serial.println(F("HARD Forward Acceleration"));
                    */

                    if (LeftRightAcceleration > (-1)*mediumThreshold && LeftRightAcceleration                 //
                // No lateral Acceleration - set just rear struts to HARD
                //
                setAutoStrutState(STRUT_FL, SOFT);
                setAutoStrutState(STRUT_FR, SOFT);
                setAutoStrutState(STRUT_RL, HARD);
                setAutoStrutState(STRUT_RR, HARD);

            }else if (LeftRightAcceleration >= mediumThreshold && LeftRightAcceleration                 //
                //  Doing a MEDIUM  Left Turn at same time
                                //  Set right side struts to medium 
                //
                setAutoStrutState(STRUT_FL, SOFT);
                setAutoStrutState(STRUT_FR, MEDIUM);
                setAutoStrutState(STRUT_RL, HARD);
                setAutoStrutState(STRUT_RR, HARD);

            }else if (LeftRightAcceleration >= hardThreshold ) {
                //
                //  Doing a HARD Left Turn at same time-
                                //  set rear struts to medium & right side struts to Hard
                //
                setAutoStrutState(STRUT_FL, SOFT);
                setAutoStrutState(STRUT_FR, HARD);
                setAutoStrutState(STRUT_RL, HARD);
                setAutoStrutState(STRUT_RR, HARD);
                    
            }else if (LeftRightAcceleration > (-1)*hardThreshold && LeftRightAcceleration <= (-1)*mediumThreshold ) {
                //
                //  Doing a MEDIUM RIGHT Turn at same time-
                                //  set rear & left side struts to medium &
                //
                setAutoStrutState(STRUT_FL, MEDIUM);
                setAutoStrutState(STRUT_FR, SOFT);
                setAutoStrutState(STRUT_RL, HARD);
                setAutoStrutState(STRUT_RR, HARD);

            }else if (LeftRightAcceleration <= (-1)*hardThreshold ) {
                //
                //  Doing a HARD RIGHT Turn at same time-
                                //  set rear struts to medium & left side struts to Hard
                //
                setAutoStrutState(STRUT_FL, HARD);
                setAutoStrutState(STRUT_FR, SOFT);
                setAutoStrutState(STRUT_RL, HARD);
                setAutoStrutState(STRUT_RR, HARD);
                    }
 
     //
     //Check to see if we are doing MEDIUM range Braking
     //
     } else if(ForwardAcceleration > (-1)*hardThreshold  && ForwardAcceleration < (-1)*mediumThreshold){
                    //
            // Doing MEDIUM range Braking between 2.0 & 4.0 g's
                    // In this range the Front Struts will be set to MEDIUM Mode
                    // And any Lateral accelerations due to turns will set the LEFT or RIGHT Struts
            //

            /*
                     * Serial.println(F("MEDIUM Braking"));
             */
                    if (LeftRightAcceleration > (-1)*mediumThreshold && LeftRightAcceleration                 //
                // No lateral Acceleration - set just Front struts to HARD
                //
                setAutoStrutState(STRUT_FL, MEDIUM);
                setAutoStrutState(STRUT_FR, MEDIUM);
                setAutoStrutState(STRUT_RL, SOFT);
                setAutoStrutState(STRUT_RR, SOFT);

            }else if (LeftRightAcceleration >= mediumThreshold && LeftRightAcceleration                 //
                //  Doing a MEDIUM  Left Turn at same time
                                //  Set right side struts to medium 
                //
                setAutoStrutState(STRUT_FL, MEDIUM);
                setAutoStrutState(STRUT_FR, MEDIUM);
                setAutoStrutState(STRUT_RL, SOFT);
                setAutoStrutState(STRUT_RR, MEDIUM);

            }else if (LeftRightAcceleration >= hardThreshold ) {
                //
                //  Doing a HARD Left Turn at same time-
                                //  Set right side struts to Hard
                //
                setAutoStrutState(STRUT_FL, MEDIUM);
                setAutoStrutState(STRUT_FR, HARD);
                setAutoStrutState(STRUT_RL, SOFT);
                setAutoStrutState(STRUT_RR, HARD);
                    
            }else if (LeftRightAcceleration > (-1)*hardThreshold && LeftRightAcceleration <= (-1)*mediumThreshold ) {
                //
                //  Doing a MEDIUM RIGHT Turn at same time-
                                //  Set left side struts to medium &
                //
                setAutoStrutState(STRUT_FL, MEDIUM);
                setAutoStrutState(STRUT_FR, MEDIUM);
                setAutoStrutState(STRUT_RL, MEDIUM);
                setAutoStrutState(STRUT_RR, SOFT);

            }else if (LeftRightAcceleration <= (-1)*hardThreshold ) {
                //
                //  Doing a HARD RIGHT Turn at same time-
                                //  left side struts to Hard
                //
                setAutoStrutState(STRUT_FL, HARD);
                setAutoStrutState(STRUT_FR, MEDIUM);
                setAutoStrutState(STRUT_RL, HARD);
                setAutoStrutState(STRUT_RR, SOFT);
                    }
 
     //
     //Check to see if we are doing HARD range Braking
     //
     } else if(ForwardAcceleration <= (-1)*hardThreshold ){
                    //
            // Doing HARD range Braking greater than  4.0 g's
                    // In this range the Front Struts will be set to Hard Mode
                    // And any Lateral accelerations due to turns will set the LEFT or RIGHT Struts
            //
                   
            /*
                     Serial.println(F("Hard Braking"));
             */

                    if (LeftRightAcceleration > (-1)*mediumThreshold && LeftRightAcceleration                 //
                // No lateral Acceleration - set just Front struts to HARD
                //
                setAutoStrutState(STRUT_FL, HARD);
                setAutoStrutState(STRUT_FR, HARD);
                setAutoStrutState(STRUT_RL, SOFT);
                setAutoStrutState(STRUT_RR, SOFT);

            }else if (LeftRightAcceleration >= mediumThreshold && LeftRightAcceleration                 //
                //  Doing a MEDIUM  Left Turn at same time
                                //  Set right side struts to medium 
                //
                setAutoStrutState(STRUT_FL, HARD);
                setAutoStrutState(STRUT_FR, HARD);
                setAutoStrutState(STRUT_RL, SOFT);
                setAutoStrutState(STRUT_RR, MEDIUM);

            }else if (LeftRightAcceleration >= hardThreshold ) {
                //
                //  Doing a HARD Left Turn at same time-
                                //  Set right side struts to Hard
                //
                setAutoStrutState(STRUT_FL, HARD);
                setAutoStrutState(STRUT_FR, HARD);
                setAutoStrutState(STRUT_RL, SOFT);
                setAutoStrutState(STRUT_RR, HARD);
                    
            }else if (LeftRightAcceleration > (-1)*hardThreshold && LeftRightAcceleration <= (-1)*mediumThreshold ) {
                //
                //  Doing a MEDIUM RIGHT Turn at same time-
                                //  Set left side struts to medium &
                //
                setAutoStrutState(STRUT_FL, HARD);
                setAutoStrutState(STRUT_FR, HARD);
                setAutoStrutState(STRUT_RL, MEDIUM);
                setAutoStrutState(STRUT_RR, SOFT);

            }else if (LeftRightAcceleration <= (-1)*hardThreshold ) {
                //
                //  Doing a HARD RIGHT Turn at same time-
                                //  left side struts to Hard
                //
                setAutoStrutState(STRUT_FL, HARD);
                setAutoStrutState(STRUT_FR, HARD);
                setAutoStrutState(STRUT_RL, HARD);
                setAutoStrutState(STRUT_RR, SOFT);
                    }
 
     }
}


///////////////////////////////////////////////////////////////////////////////////////
//
//
// Routine to make one pass thru a Strut to Set the desired strut to a particular mode       
// this routine must be called in a loop
//
/////////////////////////////////////////////////////////////////////////////////////
void setStrutMode( int strut ){

  int maxTries;
  int currentState;
  unsigned long currentTime;

                currentTime = millis();

               if (myController.mode() == MANUAL) maxTries = MANUAL_MAXTRIES;
                       else if (myController.mode() == AUTO) maxTries = AUTO_MAXTRIES;

                //
                                // If this strut has exceeded maximum allowed failures bail                  
                //
                                if (myStruts[strut].failures() > MAXFAILURES){
                                       // Make sure Motor is Off
                      myStruts[strut].motorOff();
                               if(logMode){
                                                Serial.print("MAXFAILS,");
                                                        Serial.print("FAILED,");
                                                        Serial.println("FAILED");
                                      }
                      return;
                        }

                //
                                // Jump to strut state
                //
                                switch (myStruts[strut].transitionStatus()){
                               

                                                case OLDSTRUTS:
                                //
                                                                // This case deals with old struts where the motors & mechansim
                                                                // can cause the strut to have problems getting into mode
                                                                // This state will purposely run this strut motor
                                                                // for an additional 20msec to try an force it into mode
                                                                // turn off the motors and then see if the strut is in mode
                                                                // and then set state accordingly
                                               

                               
                                if( currentTime < myStruts[strut].lastChangeTime() + 20) break;
                               
                                                                //
                                                                // OK the motors for this strut have been forced on for 20 msecs
                                                // Turn them off or make sure their still off. 
                               myStruts[strut].motorOff();
                                                               
                                                                //
                                                                // Wait for an additional 10 msecs( 20+10) for strut & switches to stablize
                                                                //
                                if( currentTime < myStruts[strut].lastChangeTime() + 30) break;
                                                               
                                currentState =  myStruts[strut].actualState(ERROR);          
                                                               
                                                                if( currentState == myStruts[strut].desiredState()  ){

                                              // Ok Struts have held desired mode for 5 msecs
                                                                      // Switch to Nominal
                                                                    
                                      myStruts[strut].setErrorStatus(NO_FAILURES);
                                      myStruts[strut].setTransitionStatus(NOMINAL);
                                      myStruts[strut].setLastChangeTime();

                                                                }else{
                                                                       //
                                                                       // Ok we were not able to get the struts into the correct
                                                                       // mode using old struts mode
                                                                       // flag them as bad
                                                                       //
                                      myStruts[strut].motorOff();
                                       myStruts[strut].setLastChangeTime();
                                       myStruts[strut].setTransitionStatus(FAILED);
                                                      myStruts[strut].setErrorStatus(MOTOR_COMMAND_FAILURE);
                                       myStruts[strut].incrementFailures();
                                       setLED(strut,D_GREENYELLOW);

                                                                }
                                break;
                                                               
                                                case PRENOMINAL:

                                                                 //
                                                                 // This state is used just after a strut has indicated it
                                                                 // has achieved the desired mode
                                                                 // it will double check the state of after 5 msec
                                                                 // and declare it Nominal if it stays in the desired mode
                                                                 //

                                                                 //
                                                                 // wait 10 msec before checking status
                                                                 //
                                                                 if(currentTime < myStruts[strut].lastChangeTime() + 10) break;

                                 currentState =  myStruts[strut].actualState(ERROR);          

                                                                 if( currentState == myStruts[strut].desiredState()  ){

                                              // Ok Struts have held desired mode
                                                                      // Switch to Nominal
                                                                    
                                      myStruts[strut].setErrorStatus(NO_FAILURES);
                                      myStruts[strut].setTransitionStatus(NOMINAL);
                                      myStruts[strut].setLastChangeTime();

                                                                 }else{
                                                                       //
                                                                       // Ok it looks like we might be dealing with old struts
                                                                       // turn the motors back on and set to Old Struts mode
                                                                       //
                                      myStruts[strut].motorOn();
                                       myStruts[strut].incrementTryCount();
                                       myStruts[strut].setLastChangeTime();
                                       myStruts[strut].setTransitionStatus(OLDSTRUTS);

                                                                 }
                                                     break;

                                                case FAILED:
                                                     if (myStruts[strut].errorStatus() == SIGNAL_WIRE_FAILURE){
                                               setLED(strut,D_YELLOW);
                                                     }else if (myStruts[strut].errorStatus() == MOTOR_COMMAND_FAILURE){
                                setLED(strut,D_RED);
                                                     }
                                                     break;

                                                case NOMINAL:
                             currentState =  myStruts[strut].actualState(ERROR);          
                                                    
                             if (currentState != myStruts[strut].desiredState()) {
                                               setLED(strut,D_YELLOW);
                                               myStruts[strut].setErrorStatus(SIGNAL_WIRE_FAILURE);
                                myStruts[strut].setTransitionStatus(FAILED);
                                myStruts[strut].incrementFailures();
                                             }

                                                     break;

                                                case CHANGING:
                                                    
                             currentState =  myStruts[strut].actualState(ERROR);          
                                                    
                                     if(  myStruts[strut].tryCount() < maxTries   ){


                                          if( currentState == myStruts[strut].desiredState()  ){
                                                                       //
                                                                       // Ok.. we just got the strut into the desired state
                                                                       //
                                                                       //
                                                                      
                                                       // make sure the motors are off
                                      myStruts[strut].motorOff();
                                                 
                                               // Strut looks like it is now in desired mode
                                                                       // change state to PRENOMINAL - that state will figure out if strut
                                                                       // really stayed in desired state
                                                                       //
                                       myStruts[strut].setErrorStatus(NO_FAILURES);
                                       myStruts[strut].setTransitionStatus(PRENOMINAL);
                                       myStruts[strut].setLastChangeTime();

                                                  }else {
                                                                       //
                                                                       // Ok still not in correct state keep motors on
                                      myStruts[strut].motorOn();
                                       myStruts[strut].incrementTryCount();
                                                  }

                                             }else {
                                                 //
                                                 // Maxtries was exceeded - display RED
                                                 //

                                                 // Turn the Motor Off
                                myStruts[strut].motorOff();

                                 setLED(strut,D_RED);
                                 myStruts[strut].setErrorStatus(MOTOR_COMMAND_FAILURE);
                                 myStruts[strut].setTransitionStatus(FAILED);
                                 myStruts[strut].incrementFailures();

                                             }
                                                     break;
                                }

                        if(logMode){
                                      if (myStruts[strut].errorStatus() == SIGNAL_WIRE_FAILURE){
                                           Serial.print("WIRE_FAIL,");
                                      }else if (myStruts[strut].errorStatus() == MOTOR_COMMAND_FAILURE){
                                                   Serial.print("MOTOR_FAIL,");
                                      }else if (myStruts[strut].errorStatus() == NO_FAILURES){
                                                        Serial.print("STRUT_OK,");
                                      }

                                      if (myStruts[strut].desiredState() == HARD){
                                           Serial.print("T=HARD,");
                                      }else if (myStruts[strut].desiredState() == MEDIUM){
                                                   Serial.print("T=MEDIUM,");
                                      }else if (myStruts[strut].desiredState() == SOFT){
                                                   Serial.print("T=SOFT,");
                                      }
                      if(strut == 3){
                                         if (currentState == HARD){
                                           Serial.println("HARD");
                                         }else if (currentState == MEDIUM){
                                                   Serial.println("MEDIUM");
                                         }else if (currentState == SOFT){
                                                   Serial.println("SOFT");
                                         }else Serial.println("Unknown"); 
                                      }else{
                                         if (currentState == HARD){
                                           Serial.print("HARD, ");
                                         }else if (currentState == MEDIUM){
                                                   Serial.print("MEDIUM, ");
                                         }else if (currentState == SOFT){
                                                   Serial.print("SOFT, ");
                                         }else Serial.print("Unknown, "); 
                                      }
                        }


}
///////////////////////////////////////////////////////////////////////////////////////
//
//
// Routine to Read actual status of strut , set LED color apprropriately and return value
//
//
///////////////////////////////////////////////////////////////////////////////////////
int readStrutState( int strut, int displayMode){

  int strutS1;
  int strutS2;
 
  int switch1;
  int switch2;
  int switch1b;
  int switch2b;

  int timeout = 0;
  int numberTries = 0;
/*
  unsigned long timestart;
  unsigned long timenow;
  timestart = millis();
  */

  strutS1 = myStruts[strut].switch1();
  strutS2 = myStruts[strut].switch2();

  //
  // intial switch reads
  //
  switch1 = digitalRead(strutS1);
  switch2 = digitalRead(strutS2);
  delayMicroseconds(50); //Wait for 100us to debounce contacts
  switch1b = digitalRead(strutS1);
  switch2b = digitalRead(strutS2);


  //
  // See if the switches are reading the same
  //
  while ((switch2 != switch2b || switch1 != switch1b)&& numberTries < 40){
      switch1 = digitalRead(strutS1);
      switch2 = digitalRead(strutS2);
      delayMicroseconds(50); //Wait for 50us to debounce contacts
      switch1b = digitalRead(strutS1);
      switch2b = digitalRead(strutS2);
      numberTries++;
     
  }
 
 
  if (switch2 == HIGH && switch1 == LOW && numberTries <40 ){
    // Strut is in HARD mode
    setLED(strut, D_HARD);
    return(HARD);
   
  }else if (switch2 == LOW &&  switch1 == LOW && numberTries <40 ){
    // If Strut is in MEDIUM mode 
    setLED(strut, D_MEDIUM);
    return(MEDIUM);
   
  }else if (switch2 == LOW && switch1 == HIGH && numberTries <40 ){
    // Strut is in SOFT Mode
    setLED(strut, D_SOFT);
    return(SOFT);
  }else {
    // setLED(strut, D_UNKNOWN);
    setLED(strut, D_OFF);
    return(UNKNOWN);
  }
}


///////////////////////////////////////////////////////////////////////////////////////
//
//
// Routine to set a Strut to a desired mode       
// this routine has it's own internal loop
//
/////////////////////////////////////////////////////////////////////////////////////
void setStrutDirectMode( int strut, int Mode,int Clear ){
               
  int currentState;
  int maxTries = 20000;


             /*
                SIGNAL_WIRE_FAILURE  
                MOTOR_COMMAND_FAILURE
                NO_FAILURES
                       if (myStruts[strut].desiredState() != myController.strutMode()){*
               myStruts[strut].setErrorStatus(NO_FAILURES);
               myStruts[strut].setDesiredState(myController.strutMode());
                       myStruts[strut].setTransitionStatus(CHANGING);
               STRUT_FL 0
               STRUT_FR 1
               STRUT_RL 2
               STRUT_RR 3
                     */

          if(Clear == CLEAR){
             myStruts[strut].clearFailures();
             myStruts[strut].clearTryCount();
             myStruts[strut].setErrorStatus(NO_FAILURES);
                   
          } 

          myStruts[strut].setDesiredState(Mode);
                  myStruts[strut].setTransitionStatus(CHANGING);
                               
          while (myStruts[strut].transitionStatus() == CHANGING ){
                                 
                                if (myStruts[strut].failures() > MAXFAILURES){
                                       // Make sure Motor is Off
                      myStruts[strut].motorOff();
                       return;
                                }

                                switch (myStruts[strut].transitionStatus()){

                                                case FAILED:
                             setLED(strut,D_RED);
                                                     return;
                                                     break;

                                                case NOMINAL:
                             currentState =  myStruts[strut].actualState(ERROR);          
                                                    
                             if (currentState != myStruts[strut].desiredState()) {
                                               setLED(strut,D_YELLOW);
                                               myStruts[strut].setErrorStatus(SIGNAL_WIRE_FAILURE);
                                myStruts[strut].setTransitionStatus(FAILED);
                                myStruts[strut].incrementFailures();
                                             }
                                                     return;

                                                case CHANGING:
                                                    
                             currentState =  myStruts[strut].actualState(ERROR);          
                                                    
                                     if(  myStruts[strut].tryCount() < maxTries   ){


                                          if( currentState == myStruts[strut].desiredState()  ){
                                                                       //
                                                                       // Ok.. we just got the strut into the desired state

                                                                       // delay for X milliseconds to make sure into mode.
                                               // delay(30);
                                                                      
                                                       // Turn the Motor Off
                                      myStruts[strut].motorOff();
                                                 
                                               // Strut is now in desired mode
                                        myStruts[strut].setErrorStatus(NO_FAILURES);
                                        myStruts[strut].setTransitionStatus(NOMINAL);

                                                                                //
                                                                                // Pause for 1 second and do a final check to see if strut stayed in the desired mode
                                                                                //
                                                                                delay(1000);

                                        currentState =  myStruts[strut].actualState(ERROR);          
                                                    
                                        if (currentState != myStruts[strut].desiredState()) {
                                                          setLED(strut,D_YELLOW);
                                                          myStruts[strut].setErrorStatus(SIGNAL_WIRE_FAILURE);
                                           myStruts[strut].setTransitionStatus(FAILED);
                                           myStruts[strut].incrementFailures();
                                                        }

                                                  }else {
                                                                       //
                                                                       // Ok still not in correct state keep motors on
                                              myStruts[strut].motorOn();
                                       myStruts[strut].incrementTryCount();
                                                  }

                                             }else {
                                                 //
                                                 // Maxtries was exceeded - display RED
                                                 //

                                                 // Turn the Motor Off
                                myStruts[strut].motorOff();

                                 setLED(strut,D_RED);
                                 myStruts[strut].setErrorStatus(MOTOR_COMMAND_FAILURE);
                                 myStruts[strut].setTransitionStatus(FAILED);
                                 myStruts[strut].incrementFailures();

                                             }
                                                     break;
                                }
         }
}

//*********************************************************************************************
//
//
//  Logging Routine
//
//
//*********************************************************************************************

void doLogMode(int mode,int val1,int val2){
unsigned long currentTime = millis();
         /*
          *         Time, Controller Mode, Controller Target , Acc FWD Avg, Acc LeftRight Avg , Acc UP Avg,
          *         Strut Name, FL Strut STatus, FL Target, FL Strut Actual
          *         Strut Name, FR Strut STatus, FR Target, FR Strut Actual
          *         Strut Name, RL Strut STatus, RL Target, RL Strut Actual
          *         Strut Name, RR Strut STatus, RR Target, RR Strut Actual
          *
          */

                switch (mode){

                                case LOG_TIME:
                        Serial.print(currentTime);
                        Serial.print(F(","));
                                               
                        if(myController.mode() == MANUAL){
                            Serial.print(F("MANUAL,"));

                                                }else if(myController.mode() == AUTO){
                            Serial.print(F("AUTO,"));
                                                }else{
                            Serial.print(F("MODE?,"));
                                                }
                                if (myController.strutMode() == HARD){
                            Serial.print(F("HARD,"));
                                                }else if (myController.strutMode() == MEDIUM){
                            Serial.print(F("MEDIUM,"));
                                                }else if (myController.strutMode() == SOFT){
                            Serial.print(F("SOFT,"));
                                                }

                                                break;
                                               
                                case LOG_STRUTS_NAME:
                                        if (val1 == 0) Serial.print("FL,");
                                        else if (val1 == 1) Serial.print("FR,");
                                        else if (val1 == 2) Serial.print("RL,");
                                        else if (val1 == 3) Serial.print("RR,");
                                                break;

                                case LOG_ACCELEROMETER:
                         readAccelerometerValues();   


                                                break;
                }
}



//**************************************************************************
// this routine converts three seperate RGB values in to a correctly encoded RGB integer
//**************************************************************************
long rgbEncode(int red,int green,int blue){
 long rgb;

 //if (red > 255) red=255;
 //if (green > 255) green=255;
 //if (blue > 255) blue=255;

 rgb = red*65536 + green*256 + blue;

 return (rgb);
 
}




///////////////////////////////////////////////////////////////////////////////////////
// Routine to Blink one of the lights
///////////////////////////////////////////////////////////////////////////////////////
void blinkLights(int lightaddress, int numberBlinks,int blinkDelay,int numberGroupBlinks,int groupDelay){
   
    setLights(lightaddress,LOW);
   
    for (int groupcount = numberGroupBlinks; groupcount > 0; groupcount--){
       
                for (int count = numberBlinks; count > 0; count--){
                   delay(blinkDelay);
                   setLights(lightaddress,HIGH);
                   delay(blinkDelay);
                   setLights(lightaddress,LOW);
        }
                delay(groupDelay);
    }
}
///////////////////////////////////////////////////////////////////////////////////////
// Routine to Blink Both of the lights at the same time
///////////////////////////////////////////////////////////////////////////////////////
void blinkBothLights(int numberBlinks,int blinkDelay,int numberGroupBlinks,int groupDelay,int doLed){
   
    // Turn off both Lights
    setLights(SportLight,LOW);
    setLights(TourLight,LOW);
   
    for (int groupcount = numberGroupBlinks; groupcount > 0; groupcount--){
       
                for (int count = numberBlinks; count > 0; count--){
                   delay(blinkDelay);
           setLights(SportLight,HIGH);
           setLights(TourLight,HIGH);
                   if(doLed){
              setLED(STRUT_FL,D_WHITE);
              setLED(STRUT_FR,D_WHITE);
                   }
                  
                   delay(blinkDelay);
           setLights(SportLight,LOW);
           setLights(TourLight,LOW);
                   if(doLed){
              setLED(STRUT_FL,D_OFF);
              setLED(STRUT_FR,D_OFF);
                   }
        }
                delay(groupDelay);
    }
}
///////////////////////////////////////////////////////////////////////////////////////
// Routine to Set TourSport Lights during command loop
///////////////////////////////////////////////////////////////////////////////////////

void setLoopTourSportLights(){

     int strutMode;
     unsigned long currentTime = millis();
 
     if(currentTime <= lightCycleStartTime + 30000){
          //
          // time to do Standard Lights Status
          //

                  if(myController.mode() == MANUAL) strutMode = myController.strutMode();
                  else if (myController.mode() == AUTO) {
              strutMode = SOFT;
              for(int strut = 0; strut < 4; strut++){
                                      if(myStruts[strut].desiredState() == MEDIUM && strutMode == SOFT ) strutMode = MEDIUM;
                                      if(myStruts[strut].desiredState() == HARD && ( strutMode == MEDIUM || strutMode == SOFT)) strutMode = HARD;
                      }
                  }
          switch (strutMode){
                                                   case(HARD):
                                     setLights(SportLight,HIGH);
                             setLights(TourLight,LOW);
                                                     break;

                                                   case(MEDIUM):
                                     setLights(SportLight,HIGH);
                             setLights(TourLight,HIGH);
                                                     break;

                                                   case(SOFT):
                                     setLights(SportLight,LOW);
                             setLights(TourLight,HIGH);
                                                     break;
                                                    
                                                   case(UNKNOWN):
                                     setLights(SportLight,LOW);
                             setLights(TourLight,LOW);
                                                     break;
                  }

          signalWireFailureCycleStartTime = millis();
                 
     }else if(lightCycleStartTime+30000 < currentTime && currentTime <= lightCycleStartTime + 31500){
          //
          // time to do Signal Wire Status
          //

 
                  // if there was a problem start flashing shit.. to the do sport/tourlight error thing
                  if (myStruts[STRUT_FL].errorStatus() == SIGNAL_WIRE_FAILURE ||
                      myStruts[STRUT_FR].errorStatus() == SIGNAL_WIRE_FAILURE ||
                      myStruts[STRUT_RL].errorStatus() == SIGNAL_WIRE_FAILURE ||
                      myStruts[STRUT_RR].errorStatus() == SIGNAL_WIRE_FAILURE ) {
                                 

                       if(currentTime < signalWireFailureCycleStartTime + 250 ){
                              setLights(SportLight,LOW);
                      setLights(TourLight,HIGH);
                       }else if(currentTime < signalWireFailureCycleStartTime + 500 ){
                              setLights(SportLight,HIGH);
                      setLights(TourLight,LOW);
                       }else if(currentTime < signalWireFailureCycleStartTime + 750 ){
                              setLights(SportLight,LOW);
                      setLights(TourLight,HIGH);
                       }else if(currentTime < signalWireFailureCycleStartTime + 1000 ){
                              setLights(SportLight,HIGH);
                      setLights(TourLight,LOW);
                       }else if(currentTime < signalWireFailureCycleStartTime + 1250 ){
                              setLights(SportLight,LOW);
                      setLights(TourLight,HIGH);
                       }else if(currentTime < signalWireFailureCycleStartTime + 1500 ){
                              setLights(SportLight,HIGH);
                      setLights(TourLight,LOW);
               }
           }
          commandFailureCycleStartTime = millis();

     }else if(lightCycleStartTime+31500 < currentTime && currentTime <= lightCycleStartTime + 33000){
          //
          // time to do Motor Command Status
          //

                  // if there was a problem start flashing shit.. to the do sport/tourlight error thing
                  if (myStruts[STRUT_FL].errorStatus() == MOTOR_COMMAND_FAILURE ||
                      myStruts[STRUT_FR].errorStatus() == MOTOR_COMMAND_FAILURE ||
                      myStruts[STRUT_RL].errorStatus() == MOTOR_COMMAND_FAILURE ||
                      myStruts[STRUT_RR].errorStatus() == MOTOR_COMMAND_FAILURE ) {
                                 

                       if(currentTime < commandFailureCycleStartTime + 250 ){
                              setLights(SportLight,LOW);
                      setLights(TourLight,LOW);
                       }else if(currentTime < commandFailureCycleStartTime + 500 ){
                              setLights(SportLight,HIGH);
                      setLights(TourLight,HIGH);
                       }else if(currentTime < commandFailureCycleStartTime + 750 ){
                              setLights(SportLight,LOW);
                      setLights(TourLight,LOW);
                       }else if(currentTime < commandFailureCycleStartTime + 1000 ){
                              setLights(SportLight,HIGH);
                      setLights(TourLight,HIGH);
                       }else if(currentTime < commandFailureCycleStartTime + 1250 ){
                              setLights(SportLight,LOW);
                      setLights(TourLight,LOW);
                       }else if(currentTime < commandFailureCycleStartTime + 1500 ){
                              setLights(SportLight,HIGH);
                      setLights(TourLight,HIGH);
               }
           }
     } else if (currentTime > lightCycleStartTime + 33000) lightCycleStartTime = millis();
    
}

///////////////////////////////////////////////////////////////////////////////////////
// Routine to intialiaze IOPins for Strut Motors
///////////////////////////////////////////////////////////////////////////////////////
void initializeMotorIOPin(int pin){

                // Set pin used for strut motor as output
                pinMode(pin, OUTPUT);

        // Make sure Motor is off
                digitalWrite(pin,LOW);
}

///////////////////////////////////////////////////////////////////////////////////////
// Routine to intialiaze IOPins for Strut Switches
///////////////////////////////////////////////////////////////////////////////////////
void initializeStrutIOPin(int pin){

                // Set pin used for strut motor as output
                pinMode(pin, INPUT);

        // Pull Pin HIGH        
                digitalWrite(pin,HIGH);
}


///////////////////////////////////////////////////////////////////////////////////////
// Routine to set an LED  to a color by Address
///////////////////////////////////////////////////////////////////////////////////////
void setLED(int ledaddress, int color){
                //
        // rgbEncode(int red,int green,int blue)
                // red,green,blue =  0, 255
                //

                switch(color){
                                case D_HARD:
                                                setLedColors(ledaddress,ledHardValue);
                                        break;        
                                case D_MEDIUM:
                                                setLedColors(ledaddress,ledMediumValue);
                                        break;        
                                case D_SOFT:
                                                setLedColors(ledaddress,ledSoftValue);
                                        break;        
                                case D_RED:
                                                setLedColors(ledaddress,ledRedValue);
                                        break;        
                                case D_GREEN:
                                                setLedColors(ledaddress,rgbEncode(0,10,0));
                                        break;        
                                case D_BLUE:
                                                setLedColors(ledaddress,rgbEncode(0,0,25));
                                        break;        
                                case D_TURQUOIS:
                                                setLedColors(ledaddress,rgbEncode(0,40,64));
                                        break;        
                                case D_PURPLE:
                                                setLedColors(ledaddress,rgbEncode(10,0,10));
                                        break;        
                                case D_YELLOW:
                                                setLedColors(ledaddress,rgbEncode(15,15,0));
                                        break;
                                case D_GREENYELLOW:
                                                setLedColors(ledaddress,rgbEncode(55,128,0));
                                        break;
                                case D_STARTUP:
                                                setLedColors(ledaddress,rgbEncode(2,2,2));
                                        break;        
                                case D_WHITE:
                                                setLedColors(ledaddress,rgbEncode(15,15,15));
                                        break;        
                                case D_OFF:
                                                setLedColors(ledaddress,rgbEncode(0,0,0));
                                        break;        
                                case D_UNKNOWN:
                                                setLedColors(ledaddress,rgbEncode(53,18,0));
                                        break;        

                        
                                }
         // delayMicroseconds(600); //Wait for 200us to go into reset
                 
}


///////////////////////////////////////////////////////////////////////////////////////
// Routine to turn an LED off
//////////////////////////////////////////////////////////////////////////////////////
void offLED(int ledaddress){
       
                setLedColors(ledaddress,LEDOFF);
}
///////////////////////////////////////////////////////////////////////////////////////
// Routine to set either the Tour Or Sport light to an HIGH/ON or LOW/OFF state
///////////////////////////////////////////////////////////////////////////////////////
void setLights(int light, int state){
  digitalWrite(light,state);
}


///////////////////////////////////////////////////////////////////////////////////////
// Routine to turn off all strut motors
///////////////////////////////////////////////////////////////////////////////////////
void motorsOff(){
  myStruts[0].motorOff();
  myStruts[1].motorOff();
  myStruts[2].motorOff();
  myStruts[3].motorOff();
}

//****************************************************
//
// Convert Desired HEX color into  rgb values and store in LED array
//
//****************************************************

void setLedColors(int LED,long hexValue) {
 
    if( ledCurrentValue[LED] == hexValue ) return;
    ledCurrentValue[LED] = hexValue ;
    ledColors[LED].r = ((hexValue >> 16) & 0xFF) ;  // Extract the RR byte
    ledColors[LED].g = ((hexValue >> 8) & 0xFF) ;   // Extract the GG byte
    ledColors[LED].b = ((hexValue) & 0xFF) ;        // Extract the BB byte

    ledColors[LED+4].r = ledColors[LED].r ;
    ledColors[LED+4].g = ledColors[LED].g ;
    ledColors[LED+4].b = ledColors[LED].b ;


    LEDS.show(); //PUSH the current color definitions of ALL the LEDs out
}


/////////////////////////////////////////////////////////////////////////////////////
//
//
// routine to read the switch pushes while setting LED brightness
//
//
//
/////////////////////////////////////////////////////////////////////////////////////
int readPushButtonSwitch(){
            int numberPushes =0;
            unsigned long currentTime;
            currentTime       = millis();

            //
                    // If no presses have occurred or < 0.5 secs since push then return
                    //
            if(pushCount == 0) return(NOCHANGE);
            if(currentTime < (pushTime + 1500)) {
                          /*
                  Serial.print(F("PushCount < 2000ms ="));
                  Serial.println(pushCount);
                          */
                                  return(NOCHANGE);
                    }

                    /*
            Serial.print(F("PushCount >2000 ="));
            Serial.println(pushCount);
                    */


            //
                    // Make sure pushCount is in valid range - if out of range set back to zero
                    //
            if(pushCount > 4){
                                   pushCount = 0;
                           return (NOCHANGE);
                    }
            numberPushes = pushCount;
                    pushCount = 0;
                    return(numberPushes);

           /*
            *  Alternate code for more complex return
            //
                    //  Ok button has been pressed one or more times and 0.5 seconds has passed since last push
                    //  now decide what new state we are in
                    //
            switch (pushCount){
                                    case 1:
                                        pushCount = 0;
                        return(1);
                                        break;

                                     case 2:
                                        pushCount = 0;
                        return(2);
                                        break;
                                               
                                     case 3:
                                        pushCount = 0;
                        return(3);
                                        break;
                                               
                                     case 4:
                                        pushCount = 0;
                        return(4);
                                        break;
                                     break;


                    }
           */

}



/////////////////////////////////////////////////////////////////////////////////////
// Reference 5v = 1023
//
// Hard     0.0v     / xxx
//    Cutoff  0.415  / 85
// Medium   0.83v    / xxx
//    Cutoff   1.245   / 255
// Soft     1.66v    / xxx
//    Cutoff   2.075   / 424
// Diag     2.5      / xxx
//    Cutoff   2.915   / 596
/////////////////////////////////////////////////////////////////////////////////////
int readControllerSwitch(){
                int voltage;
                int voltage2;
                float voltdiff;

        unsigned long currentTime ;

                // Based on the selector switch type do the appropriate read
                if( myController.selectorType() == ROTARY){
                               
                   voltage = analogRead(selectorSwitchPin)+1; // read the voltage             
                   delay(50);                               // debounce for 50 msec
                   voltage2 = analogRead(selectorSwitchPin)+1;// read the voltage again
                   voltdiff = abs(1.0-voltage/voltage2);     // Calculate the percentage difference

                   //
                   // as long as voltage difference > 10% keep reading
                   //
                   while(voltdiff > .1 ){
                      voltage = analogRead(selectorSwitchPin)+1; // read the voltage         
                      delay(50);                               // debounce for 50 msec
                      voltage2 = analogRead(selectorSwitchPin)+1;// read the voltage again
                      voltdiff = abs(1.0-voltage/voltage2);   // Calculate the percentage difference
                   }

           //
                   // based on the converted voltage value and the range return the value
           //
                   if(voltage < 85){                         
             // Selector is in HARD mode
             if (myController.strutMode() == HARD) return (NOCHANGE) ;
                     else {
                       myController.setMode(MANUAL);
                       myController.setControllerStrutMode(HARD);
               return(HARD);
                     }
                 
                   }else if (voltage >= 85 && voltage < 255){
                     // Selector is in MEDIUM mode
             if (myController.strutMode() == MEDIUM) return (NOCHANGE) ;
                     else {
                       myController.setMode(MANUAL);
                       myController.setControllerStrutMode(MEDIUM);
                       return(MEDIUM);
                     }

                   }else if (voltage >= 255 && voltage < 424){
             // Selector is in SOFT mode
             if (myController.strutMode() == SOFT) return (NOCHANGE) ;
                     else {
                       myController.setMode(MANUAL);
                       myController.setControllerStrutMode(SOFT);
                       return(SOFT);
                     }

                   }else if(voltage >= 424 && voltage <596){
             // Selector is in DIAG mode
             if (myController.mode() == DIAG) return (NOCHANGE) ;
                     else {
               myController.setMode(DIAG);
                       return(DIAG);
                     }

                   }else{
              myController.setMode(UNKNOWN);
                      return (UNKNOWN);

                   }
                }else if( myController.selectorType() == PUSHBUTTON){
            currentTime = millis();
            //
                    // If no presses have occurred or < 1.5 secs since last push then return
                    //
            if(pushCount ==0) return(NOCHANGE);
            if(currentTime < (pushTime + 1500)) {
                          /*
                  Serial.print(F("PushCount < 2000ms ="));
                  Serial.println(pushCount);
                          */
                                  return(NOCHANGE);

                    }
                    /*
            Serial.print(F("PushCount >2000 ="));
            Serial.println(pushCount);
                    */
            //
                    // Make sure pushCount is in valid range - if out of range set back to zero
                    //
            if(pushCount > 9){
                                   pushCount = 0;
                           return (NOCHANGE);
                    }

            //
                    //  Ok button has been pressed one or more times and 1.5 seconds has passed since last push
                    //  now decide what new state we are in
                    //
            switch (pushCount){
                                    //
                                    //  Changing to next strut mode using sequence  H -> S -> M -> H -> S -> M ->....
                                    case 1:

                                        pushCount = 0;

                                                //
                                                // If in Diag or readDiag mode and single push occurs treat this as an abort to go back to manual mode
                                                //
                       if(myController.mode() == DIAG || myController.mode() == READDIAG){
                             myController.setMode(MANUAL);
                             return(myController.strutMode());
                                       }
                       
                                               
                                                switch(myController.strutMode()){
                           case HARD:
                             myController.setControllerStrutMode(SOFT);
                             return(SOFT);
                           break;

                           case MEDIUM:
                                                     if(myController.mode() == AUTO) myController.setControllerStrutMode(SOFT);
                                                     else myController.setControllerStrutMode(HARD);
                             return(HARD);
                           break;

                           case SOFT:
                             myController.setControllerStrutMode(MEDIUM);
                             return(MEDIUM);
                           break;
                                                }
                                        break;
                                     //
                                     //  Changing Controller mode from Auto to Manual or vice versa
                                     case 2:
                       motorsOff();

                                       pushCount = 0;
                       switch(myController.mode()){
                           case MANUAL:

                             myController.setMode(AUTO);     // Change controller to Auto MODE
                                                     myController.setControllerStrutMode(SOFT);

                                                     //
                                                     // Blink both lights alternating(3 times) to indicate auto mode
                                                     //
                                     for(int i=0;i<2;i++){
                                        setLights(SportLight,HIGH);
                                setLED(STRUT_FR,D_WHITE);
                                                               
                                setLights(TourLight,LOW);
                                setLED(STRUT_FL,D_OFF);
                                                               
                                                        delay(250);
                                        setLights(SportLight,LOW);
                                setLED(STRUT_FR,D_OFF);
                                                               
                                setLights(TourLight,HIGH);
                                setLED(STRUT_FL,D_WHITE);
                                                        delay(250);
                                             }

                                             // Turn off Lights
                                     setLights(SportLight,LOW);
                             setLights(TourLight,LOW);
                             setLED(STRUT_FL,D_OFF);
                             setLED(STRUT_FR,D_OFF);
                                                    
                             return(myController.strutMode());
                           break;

                           case AUTO:
                                                   case DIAG:
                                                   case READDIAG:
                                                  
                             myController.setMode(MANUAL);    // Change Controller to Manual MODE

                                                     //
                                                     // Blink both lights at together (3 times) to indicate manual mode
                                                     //
                             blinkBothLights(3,250,1,0,1);
                                                    
                                             // Turn off Lights
                                     setLights(SportLight,LOW);
                             setLights(TourLight,LOW);
                             return(myController.strutMode());

                           break;

                                        }
                                        break;

                                     //
                                     //  Changing Controller LED Settings
                                     //
                                     case 3:
                        motorsOff();
                                        pushCount = 0;
                        previousMode = myController.mode();
                                                myController.setMode(SETLED);    // Change Controller to change LED MODE

                                                //
                                                // Blink both lights at together (3 times) to indicate LED mode
                                                //
                        blinkBothLights(3,500,1,0,1);
                                               
                                        // Turn off Lights
                                setLights(SportLight,LOW);
                        setLights(TourLight,LOW);

                                        break;        

                                     //
                                     //  Set default startup Mode
                                     //
                                     case 4:
                        motorsOff();
                                        pushCount = 0;
                        previousMode = myController.mode();

                                        // Turn off Lights
                                setLights(SportLight,LOW);
                        setLights(TourLight,LOW);

                                                //
                                                // Blink both lights at together (4 times) to indicate saving startup mode
                                                //
                        blinkBothLights(4,500,1,0,1);

                                                myController.setMode(SETSTARTUP);    // Change Controller to change LED MODE
                                        break;        

                                     //
                                     //  Changing Controller to Read Diagnostics Mode
                                     //
                                     case 5:
                        motorsOff();
                        myController.setMode(READDIAG);
                                        pushCount = 0;
                                               
                                        // Turn off Lights
                                setLights(SportLight,LOW);
                        setLights(TourLight,LOW);

                                                //
                                                // Blink both lights at together (5 times) to indicate read diag mode
                                                //
                        blinkBothLights(5,500,1,0,1);
                                                delay(500);

                        return(READDIAG);

                                                break;
                               
                                     //
                                     //  Changing Controller to RUN Diagnostics Mode
                                     //
                                     case 6:
                        motorsOff();
                        myController.setMode(DIAG);
                                        pushCount = 0;
                                               
                                        // Turn off Lights
                                setLights(SportLight,LOW);
                        setLights(TourLight,LOW);

                                                //
                                                // Blink both lights at together (6 times) to indicate saving startup mode
                                                //
                        blinkBothLights(6,500,1,0,1);
                                                delay(500);

                        return(DIAG);
                                                break;

                                     //
                                     //  Changing Controller to Set Thresholds  Mode
                                     //
                                     case 7:

                        motorsOff();
                                        pushCount = 0;
                        previousMode = myController.mode();
                                                myController.setMode(SETTHRESHOLD);    // Change Controller to change LED MODE

                                                //
                                                // Blink both lights at together (7 times) to indicate Set Threshold mode
                                                //
                        blinkBothLights(7,500,1,0,1);
                                               
                                        // Turn off Lights
                                setLights(SportLight,LOW);
                        setLights(TourLight,LOW);

                        return(SETTHRESHOLD);
                                                break;


                                     //
                                     //  Toggle Controller in out of logging mode
                                     //
                                     case 8:
                                                //
                                                // Blink both lights at together (9 times) to indicate logging mode on or Off
                                                //
                                        pushCount = 0;
                        blinkBothLights(8,500,1,0,1);
                        if(logMode == 0) {
                                                                logMode =1;
                                                                logSequence++;
                                                Serial.println(F("**********************************************************************"));
                                                                Serial.print(F("LogSequence #:"));
                                                                Serial.println(logSequence);
                                                Serial.println(F("******** Strut GForce Transition Thresholds ********"));
                                Serial.print(F("Medium Threshold:  "));
                                Serial.println( mediumThreshold);
                                Serial.print(F("Hard Threshold:  "));
                                Serial.println( hardThreshold );
                                                        readPrintEpromValues();
                                Serial.print(F("1:Time, 2:Controller Mode, 3:Controller Target , 4:AccFWD, 5:AccLeftRight,6:AccUP,"));
                                Serial.print(F("7:Strut Name, 8:FL Strut STatus, 9:FL Target, 10:FL Strut Actual,"));
                                Serial.print(F("11:Strut Name, 12:FR Strut STatus, 13:FR Target, 14:FR Strut Actual,"));
                                Serial.print(F("15:Strut Name, 16:RL Strut STatus, 17:RL Target, 18:RL Strut Actual"));
                                Serial.println(F("19:Strut Name, 20:RR Strut STatus, 21:RR Target, 22:RR Strut Actual "));
 

                                                }
                                                else if (logMode ==1) logMode =0;
                                                break;
                                               

                                     //
                                     //  Changing Controller to Calibrate Accelerometer Mode
                                     //
                                     case 9:

                        motorsOff();
                                        pushCount = 0;
                        previousMode = myController.mode();
                                                myController.setMode(CALIBRATEACCEL);    // Change Controller to

                                                //
                                                // Blink both lights at together (8 times) to indicate Calibration mode
                                                //
                        blinkBothLights(9,500,1,0,1);
                                               
                                        // Turn off Lights
                                setLights(SportLight,LOW);
                        setLights(TourLight,LOW);

                        return(CALIBRATEACCEL);
                                                break;
              }

          }
}

//*******************************************************************************************
//
// Read Diagnostic routine
//
//
//*******************************************************************************************
void readDiagnostics(){

     while(readControllerSwitch()== NOCHANGE){

         // Turn Off All LEDS
         offLED(FL_LED);
         offLED(FR_LED);
         offLED(RL_LED);
         offLED(RR_LED);


         for(int strut = 0; strut < 4; strut++){

             // Turn off Lights
                     setLights(SportLight,LOW);
             setLights(TourLight,LOW);
                   
             //SET strut LED 1->4 to White to indicate testing Front Left Strut
             setLED(strut,D_WHITE);
             delay(1000);
                                  
                     // Blink Sport Light number of times based on strut nuber as group 4 times To indicate strut
             blinkLights(SportLight, (strut+1),300,4,1000); 

                     // See if abort desired
             if(readControllerSwitch()!= NOCHANGE) return;

                  
                     // See if abort desired
                     if(readControllerSwitch()!= NOCHANGE) return;

                     // Display Final test results for this Strut
                     if (myStruts[strut].errorStatus() == NO_FAILURES){
                       setLED(strut,D_GREEN);
                                       setLights(TourLight,HIGH);
                                       setLights(SportLight,HIGH);
                       delay(2500);

                      }else if (myStruts[strut].errorStatus() == MOTOR_COMMAND_FAILURE){
                      blinkBothLights(3,300,3,1000,0);
                      setLED(strut,D_RED);

                      }else if (myStruts[strut].errorStatus() == SIGNAL_WIRE_FAILURE){
                              setLED(strut,D_YELLOW);

                              for(int i=0;i<8;i++){
                                 setLights(SportLight,HIGH);
                         setLights(TourLight,LOW);
                                                 delay(250);
                                 setLights(SportLight,LOW);
                         setLights(TourLight,HIGH);
                                                 delay(250);
                                      }
                                      // Turn off Lights
                              setLights(SportLight,LOW);
                      setLights(TourLight,LOW);
                       }

              // See if abort desired
              if(readControllerSwitch()!= NOCHANGE) return;
                                  
              delay(2000);
         }
     }
}

//*******************************************************************************************
//
// Diagnostic routine
//
//
//*******************************************************************************************
void runDiagnostics(){
               
     while(readControllerSwitch()== NOCHANGE){

         // Turn Off All LEDS
         offLED(FL_LED);
         offLED(FR_LED);
         offLED(RL_LED);
         offLED(RR_LED);



         for(int strut = 0; strut < 4; strut++){
             // Turn off Lights
                     setLights(SportLight,LOW);
             setLights(TourLight,LOW);

             //SET strut LED 1->4 to White to indicate testing Front Left Strut
             setLED(strut,D_WHITE);
                                  
             delay(1000);
                   
                     // Blink Sport Light number of times based on strut nuber as group 4 times To indicate strut
             blinkLights(SportLight, (strut+1),300,4,1000); 

                     // See if abort desired
             if(readControllerSwitch()!= NOCHANGE) return;

             //Attempt to set   Strut HARD MODE
             setStrutDirectMode( strut, HARD, CLEAR);
             delay(1000);
                  
                     // See if abort desired
                     if(readControllerSwitch()!= NOCHANGE) return;

             //Attempt to set  Front Left Strut Medium MODE
             setStrutDirectMode( strut, MEDIUM, DONT_CLEAR);
             delay(1000);
                                     
                     // See if abort desired
                     if(readControllerSwitch()!= NOCHANGE) return;
                                     
             //Attempt to set  Front Left Strut Soft MODE
             setStrutDirectMode( strut, SOFT, DONT_CLEAR);
             delay(1000);

                     // See if abort desired
                     if(readControllerSwitch()!= NOCHANGE) return;

                     // Display Final test results for this Strut
                     if (myStruts[strut].errorStatus() == NO_FAILURES){
                       setLED(strut,D_GREEN);
                                       setLights(TourLight,HIGH);
                                       setLights(SportLight,HIGH);
                       delay(2500);
              }else{
                      blinkBothLights(3,300,3,1000,0);
                      setLED(FL_LED,D_RED);
                      }

              // See if abort desired
              if(readControllerSwitch()!= NOCHANGE) return;
                                  
              delay(2000);
        }
   }
}

/*************************************************************
 *
 *
 * Routine to read int from EProm
 *
 *
 *************************************************************/
int eepromReadInt(int address){
   int value = 0x0000;
   value = value | (EEPROM.read(address) << 8);
   value = value | EEPROM.read(address+1);
   return value;
}

/*************************************************************
 *
 *
 * Routine to Write int to EProm
 *
 *
 *************************************************************/
void eepromWriteInt(int address, int value){
   EEPROM.write(address, (value >> 8) & 0xFF );
   EEPROM.write(address+1, value & 0xFF);
}


//******************************************************************************************
//
//LSM303DLHC I2C Device Routines
//
//******************************************************************************************
 
// Send register address and the byte value you want to write the accelerometer and
// loads the destination register with the value you send

void WriteDevRegister(int devAddress, byte data, int regaddress) {
    Wire.beginTransmission(devAddress);   // device Address
    Wire.write(regaddress);
    Wire.write(data); 
    Wire.endTransmission();    
}

//
//Send register address to this function and it returns byte value
//for the accelerometer register's contents
byte ReadDevRegister(int devAddress, int regaddress) {

    byte data;
    Wire.beginTransmission(devAddress);   // device Address                             
    Wire.write(regaddress);
    Wire.endTransmission();
 
    delayMicroseconds(100);

    Wire.requestFrom(devAddress,1);   // device Address                             
    data = Wire.read();
    Wire.endTransmission();  

    delayMicroseconds(100);

    return data; 
} 

//**************************************************************************
//
// Accelerometer Initialization Routine
//
//**************************************************************************

void initAccelerometer(void) {

    WriteDevRegister(LSM303_ADDRESS_ACC,0x67,0x20);  // Enable accelerometer, 200Hz data output

    WriteDevRegister(LSM303_ADDRESS_MAG,0x9c,0x00);  // Enable temperature sensor, 220Hz data output
    WriteDevRegister(LSM303_ADDRESS_MAG,0x20,0x01);  // set gain to +/-1.3Gauss
    WriteDevRegister(LSM303_ADDRESS_MAG,0x00,0x02);  // Enable magnetometer constant conversions
}

//**************************************************************************
//
//    Read  the X,Y,Z axis values from the accelerometer
//
//**************************************************************************
void readAccelerometerValues() {

    float Acc[3];
    //
    //  ForwardAxis    axis
    //  ForwardSign    +Acc = Acceleration

    //  LeftRightAxis  axis
    //  LeftRightSign      +Acc = Left Turn

    //  UpDownAxis     axis
    //  UpDownSign     +Acc = Up

    //
    //  reinitialize globals to prepare for new averages
    //
    ForwardAcceleration  = 0.0;
    LeftRightAcceleration = 0.0;
    UpDownAcceleration = 0.0;

    //
    //  Shift all the previous readings down in the array & Start summing the existing valid entries for the average calculation
    //
    for ( int i = accelDataPoints -1;  i > 0 ; i--){
        ForwardAccelerationArray[i] =    ForwardAccelerationArray[i-1];
        ForwardAcceleration = ForwardAcceleration + ForwardAccelerationArray[i];

        LeftRightAccelerationArray[i] =  LeftRightAccelerationArray[i-1];
        LeftRightAcceleration = LeftRightAcceleration + LeftRightAccelerationArray[i];

        UpDownAccelerationArray[i] =     UpDownAccelerationArray[i-1];
        UpDownAcceleration = UpDownAcceleration + UpDownAccelerationArray[i];

    }

    //
    // get the raw Accelerometer current values off the I2C device
    //
    getAccelerometer(Acc);

    //
    //  Alias and Assign the current reading to the correct mappings in the normilized array [0]
    //
    ForwardAccelerationArray[0]   =    float (ForwardSign * Acc[ForwardAxis]);
    LeftRightAccelerationArray[0] =    float (LeftRightSign * Acc[LeftRightAxis]);
    UpDownAccelerationArray[0]    =    float (UpDownSign * Acc[UpDownAxis]);


    //
    // add the current acceleration reading to the global value and finish average calculation
    //
    ForwardAcceleration   = (ForwardAcceleration + ForwardAccelerationArray[0]) / accelDataPoints;
    LeftRightAcceleration = (LeftRightAcceleration + LeftRightAccelerationArray[0]) / accelDataPoints;
    UpDownAcceleration    = (UpDownAcceleration + UpDownAccelerationArray[0]) / accelDataPoints;
   
    if(logMode) {
                        Serial.print(ForwardAcceleration);
                        Serial.print(F(","));
                        Serial.print(LeftRightAcceleration);
                        Serial.print(F(","));
                        Serial.print(UpDownAcceleration);
                        Serial.print(F(","));
    }

} 

// ******************************************************************************k
//Readsthe X,Y,Z axis values from the accelerometer and sends the values to the
// ******************************************************************************k
void getAccelerometer(float *Acc) {

  // accelerometer values
  byte xh = ReadDevRegister(LSM303_ADDRESS_ACC,0x29);
  byte xl = ReadDevRegister(LSM303_ADDRESS_ACC,0x28);
  byte yh = ReadDevRegister(LSM303_ADDRESS_ACC,0x2B);
  byte yl = ReadDevRegister(LSM303_ADDRESS_ACC,0x2A);
  byte zh = ReadDevRegister(LSM303_ADDRESS_ACC,0x2D);
  byte zl = ReadDevRegister(LSM303_ADDRESS_ACC,0x2C);
 
  // need to convert the register contents into a righ-justified 16 bit value
  Acc[0] = (xh<<8|xl);
  Acc[1] = (yh<<8|yl);
  Acc[2] = (zh<<8|zl);

  /*
  Serial.print(" X ");
  Serial.print(Acc[0] ); 

  Serial.print(" Y ");
  Serial.print(Acc[1] ); 
 
  Serial.print(" Z ");
  Serial.println(Acc[2] );
  */

  //
  // Convert raw Accelerometer readings to g's using calibration values
  //


  //*  original M03 v3.3 calibration code
/*
    if( Acc[0] >= 0) Acc[0] = float( Acc[0] / XAxisPositive1gCalibration) ;
    else Acc[0] = float(Acc[0] / XAxisNegative1gCalibration );

    if( Acc[1] >= 0) Acc[1] = float( Acc[1] / YAxisPositive1gCalibration) ;
    else Acc[1] = float(Acc[1] / YAxisNegative1gCalibration );

    if( Acc[2] >= 0) Acc[2] = float( Acc[2] / ZAxisPositive1gCalibration) ;
    else Acc[2] = float(Acc[2] / ZAxisNegative1gCalibration );
*/

   //
   // v3.5 Calibration code
   //
   Acc[0] =float((Acc[0] + xadj)/float(XaxisAccCalibrationRange));
   Acc[1] =float((Acc[1] + yadj)/float(YaxisAccCalibrationRange));
   Acc[2] =float((Acc[2] + zadj)/float(ZaxisAccCalibrationRange));

} 
// ******************************************************************************k
//Readsthe X,Y,Z axis values from the accelerometer and sends the values to the
// ******************************************************************************k
void getRawAccelerometer(float *Acc) {

  // accelerometer values
  byte xh = ReadDevRegister(LSM303_ADDRESS_ACC,0x29);
  byte xl = ReadDevRegister(LSM303_ADDRESS_ACC,0x28);
  byte yh = ReadDevRegister(LSM303_ADDRESS_ACC,0x2B);
  byte yl = ReadDevRegister(LSM303_ADDRESS_ACC,0x2A);
  byte zh = ReadDevRegister(LSM303_ADDRESS_ACC,0x2D);
  byte zl = ReadDevRegister(LSM303_ADDRESS_ACC,0x2C);
 
  // need to convert the register contents into a righ-justified 16 bit value
  Acc[0] = (xh<<8|xl);
  Acc[1] = (yh<<8|yl);
  Acc[2] = (zh<<8|zl);

} 

//******************************************************************
//Read & Print Eprom Calibration Values
//******************************************************************
void readPrintEpromValues(){
           //
           // Now Read Back all the Values
           //
           XAxisNegative1gCalibration= eepromReadInt(EPROM_ACC_X_NEG_1G);              
           YAxisNegative1gCalibration= eepromReadInt(EPROM_ACC_Y_NEG_1G);              
           ZAxisNegative1gCalibration= eepromReadInt(EPROM_ACC_Z_NEG_1G);              
           XAxisPositive1gCalibration= eepromReadInt(EPROM_ACC_X_POS_1G);
           YAxisPositive1gCalibration= eepromReadInt(EPROM_ACC_Y_POS_1G);
           ZAxisPositive1gCalibration= eepromReadInt(EPROM_ACC_Z_POS_1G);

 
          Serial.println("****************   Accelerometer Calibration Values *********************");
          Serial.print(F("EProm X Neg Axis 1g Calibration: "));
          Serial.println(XAxisNegative1gCalibration);
          Serial.print(F("EProm Y Neg Axis 1g Calibration: "));
          Serial.println(YAxisNegative1gCalibration);
          Serial.print(F("EProm Z Neg Axis 1g Calibration: "));
          Serial.println(ZAxisNegative1gCalibration);
          Serial.print(F("EProm X Pos Axis 1g Calibration: "));
          Serial.println(XAxisPositive1gCalibration);
          Serial.print(F("EProm Y Pos Axis 1g Calibration: "));
          Serial.println(YAxisPositive1gCalibration);
          Serial.print(F("EProm Z Pos Axis 1g Calibration: "));
          Serial.println(ZAxisPositive1gCalibration);
          Serial.println("*************************************************************************");
}
//*************************************************************************************
//
//
//    calibrateAccelerometer()
//
//
//************************************************************************************
void calibrateAccelerometer(){

    float Acc[3];
    float prevAcc[3];
   
    
    int XAxis;
    int YAxis;
    int ZAxis;

    int button;

    //
    // Min Max accumulation variables
    //
    int XAxis_min = 0;
    int XAxis_max = 0;
    int YAxis_min = 0;
    int YAxis_max = 0;
    int ZAxis_min = 0;
    int ZAxis_max = 0;
    //
    // Initialize Min Max accumulation variables
    //
    XAxis_min = 0;
    XAxis_max = 0;
    YAxis_min = 0;
    YAxis_max = 0;
    ZAxis_min = 0;
    ZAxis_max = 0;

    /*
     * Intialize to Zero
     */
    for(int i=0; i<3 ; i++){
        Acc[i] = 0;
    }
                             
        // Turn off Lights while setting LEDs
        setLights(SportLight,LOW);
        setLights(TourLight,LOW);

        Serial.println(F("********************* Calibration Routine ****************************"));
        Serial.println(" o  Press Once to see EEprom Calibration & Threshold values ");
        Serial.println(" o  Press Twice To Exit Calibration Mode ");
        Serial.println(" o  Press Three Times To Begin Calibration Mode ");
        Serial.println("    During Calibration - Press Once to Save Calibration Values & Exit Calibration Routine");
        Serial.println(F("**********************************************************************"));
                                              
        /*
                 * Stay in this loop until button is pressed multple times to indicate save
                 */          
                while(1){
                               
                                //read the selector PUSHBUTTON or Rotary and see if they have it in DIAGMODE
                        button = readPushButtonSwitch();
                switch(button){

                            case NOCHANGE:
                            break;

                    case 1:
                                          //
                                          // Ok they want to print current calibration & threshold values
                                          //
                                                  //
                                          Serial.println(F("************************ Thresholds **********************************"));
                          Serial.print(F("Medium Threshold:  "));
                          Serial.println( mediumThreshold);
                          Serial.print(F("Hard Threshold:  "));
                          Serial.println( hardThreshold );
                                          Serial.println(F("**********************************************************************"));
                                                  readPrintEpromValues();
                          Serial.println(" o  Press Once to see EEprom Calibration values ");
                          Serial.println(" o  Press Twice To Exit Calibration Mode ");
                          Serial.println(" o  Press Three Times To Begin Calibration Mode ");
                          Serial.println("    During Calibration - Press Once to Save Calibration Values & Exit Calibration Routine");

                          break;



                    case 2:
                                          //
                                          // Ok they want to Exit
                                          //
                                                 
                          Serial.println(F("********************* Exiting Calibration Routine ****************************"));
                          Serial.println(F("*********************     NO Save Performed       ****************************"));
                                          //
                                          // Blink both lights at together (3 times) to indicate exiting calibration
                                          //
                          blinkBothLights(3,500,1,0,1);
                                
                                  // Restore the previous state of the controller
                                  //
                                          myController.setMode(previousMode);
                                                  return;
                                                  break;

                    case 3:
                                          //
                                          // Ok they want to Calibrate Accelerometer
                                          //
                         
                                                  while(1){
                                                                 
                                                        button = readPushButtonSwitch();
                                                        if (button != NOCHANGE) break;
                                                               
                                for(int i=0; i<3 ; i++){
                                                        prevAcc[i] = Acc[i];
                                        }
                                delay(100);
                                getRawAccelerometer(Acc);

                                        XAxis =Acc[0];
                                        YAxis =Acc[1];
                                        ZAxis =Acc[2];
                                         //
                                         // Check to make sure previous value = current value
                                         // to weed out noise from acc.
                                         //
                                         if(prevAcc[0] == Acc[0]){   
                                           if(Acc[0] < XAxis_min) XAxis_min=XAxis;
                                           if(Acc[0] > XAxis_max) XAxis_max=XAxis;
                                         }

                                         if(prevAcc[1] == Acc[1]){   
                                           if(Acc[1] < YAxis_min) YAxis_min=YAxis;
                                           if(Acc[1] > YAxis_max) YAxis_max=YAxis;
                                         }

                                         if(prevAcc[2] == Acc[2]){   
                                           if(Acc[2] < ZAxis_min) ZAxis_min=ZAxis;
                                           if(Acc[2] > ZAxis_max) ZAxis_max=ZAxis;
                                         }
/*
                                 Serial.print(" X ");
                                 Serial.print(Acc[0] ); 

                                 Serial.print(" Y ");
                                 Serial.print(Acc[1] ); 

                                 Serial.print(" Z ");
                                 Serial.println(Acc[2] ); 
*/
                                 Serial.print("MIN X=");
                                 Serial.print(XAxis_min); 
                                 Serial.print(" ");
                                 Serial.print("Y=");
                                 Serial.print(YAxis_min); 
                                 Serial.print(" ");
                                 Serial.print("Z=");
                                 Serial.print(ZAxis_min); 

                                 Serial.print("  MAX X=");
                                 Serial.print(XAxis_max); 
                                 Serial.print(" ");
                                 Serial.print("Y=");
                                 Serial.print(YAxis_max); 
                                 Serial.print(" ");
                                 Serial.print("Z=");
                                 Serial.println(ZAxis_max); 

                                                  }

                          //
                                                  // ok their done calibrtaing save values
                                                  //
                          XAxis_min = abs(XAxis_min) ;
                          eepromWriteInt(EPROM_ACC_X_NEG_1G , XAxis_min);

                          YAxis_min = abs(YAxis_min) ;
                          eepromWriteInt(EPROM_ACC_Y_NEG_1G , YAxis_min);

                          ZAxis_min = abs(ZAxis_min) ;
                          eepromWriteInt(EPROM_ACC_Z_NEG_1G , ZAxis_min);

                          eepromWriteInt(EPROM_ACC_X_POS_1G , XAxis_max);
                          eepromWriteInt(EPROM_ACC_Y_POS_1G , YAxis_max);
                          eepromWriteInt(EPROM_ACC_Z_POS_1G , ZAxis_max);


                          //
                          // Now Read Back & Print all the Values
                          //
                          Serial.println(F("************************** Thresholds *****************************"));
                          Serial.print(F("Medium Threshold:  "));
                          Serial.println( mediumThreshold);
                          Serial.print(F("Hard Threshold:  "));
                          Serial.println( hardThreshold );
                          Serial.println(F("**********************************************************************"));

                          readPrintEpromValues();

                          Serial.println(F("New Threshold Values have been saved - exiting calibration routine"));

                                          //
                                          // Blink both lights at together (8 times) to indicate saving calibrations & exiting.
                                          //
                          blinkBothLights(8,500,1,0,1);

                                  // Restore the previous state of the controller
                                  //
                                          myController.setMode(previousMode);
                                                  return;
                                                  break;
                                 

                                }
                }
}