//
//   Version 1.1
//
//   Copyright 2016-2018 Lee Djavaherian
//
//   This file is part of TrillSat.
//
//   TrillSat 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, either version 3 of the License, or
//   (at your option) any later version.
//
//   TrillSat 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 TrillSat.  If not, see <http://www.gnu.org/licenses/>.
///////////////////////////////////////////////////////////////////////////////////

//Don't burn huckleberry/sawyer directly, edit trillsat.c and then use trillsat_burnhuck.sh, trillsat_burnsaw.sh to burn.
//It automatically inserts chip name into script during burn.
#define CHIP-NAME-INSERTED-FROM-SCRIPT 1
#define F_CPU 8000000UL

#include<avr/interrupt.h>
#include<avr/sleep.h>
#include<avr/io.h>
#include<util/delay.h>
#include<avr/sfr_defs.h>
#include<math.h> //needed for floating point
#include<avr/eeprom.h> //testing 

#ifdef HUCKLEBERRY

//needed by simtheo.h
#define VIBRATE_DELAY 0
#define SIMTHEO_MODE 1

#define KEY_UP bit_is_set(PINB,PB3)
#define KEY_DOWN bit_is_clear(PINB,PB3)

//enable external interrupt detection
//PB3 is PCINT11 (PCMSK1, PCIE1)

#define KEY_INTERRUPT_ENABLE PCMSK1 |= (1<<PCINT11)
#define KEY_INTERRUPT_DISABLE PCMSK1 &= ~(1<<PCINT11)

#define MORSE_INTERRUPT_VECTOR PCINT1_vect

#define MORSE_ON DDRB &= ~(1<<PB2)
#define MORSE_OFF DDRB |= (1<<PB2);PORTB &= ~(1<<PB2)
#define MORSE_FLIP while (0) {}


#include"/trillsat/SimtheoDecoder/simtheo.h"

#endif
#ifdef SAWYER

//needed by simtheo.h
#define VIBRATE_DELAY 1
#define SIMTHEO_MODE 1

#define KEY_DOWN bit_is_set(PINB,PB2)
#define KEY_UP bit_is_clear(PINB,PB2)

//set interrupt detection for PB2 (MISO/LED pulse line for THUMP haptic motor feedback)
#define KEY_INTERRUPT_ENABLE PCMSK1 |= (1<<PCINT10)
#define KEY_INTERRUPT_DISABLE PCMSK1 &= ~(1<<PCINT10)

#define MORSE_INTERRUPT_VECTOR PCINT1_vect

#define MORSE_ON PORTB |= (1<<PB3);PORTA &= ~(1<<PA6);_delay_ms(30);PORTB &= ~(1<<PB3);PORTA |= (1<<PA6);_delay_ms(30);PORTA &= ~(1<<PA6);PORTB &= ~(1<<PB3)

#define MORSE_OFF PORTA &= ~(1<<PA6);PORTB &= ~(1<<PB3)
#define MORSE_FLIP while (0) {}

#include"/trillsat/SimtheoDecoder/simtheo.h"

#endif

// these are in case I need to read signature table bytes
// #define SIGRD 5 //remove error in library below
//#include<avr/boot.h> //needed to read signature table

//////// SHARED DEFINES ///////////////

#define I2C_ARRAY_SIZE 20

#define ON                1
#define OFF               0

#define UP                1
#define DOWN              0

#define SUCCESS           255
#define JUST_STARTED      254

#define VCC_VOLTAGE       30
#define ATTINY_TEMP       70
#define CTC               55
#define RECEIVE_MORSE     170
#define PULSE_MODE        113
#define THUMP_ENABLED     114
#define SET_HAPTIC_CODE   115

#define BANDGAP_CHANNEL      0b1101
#define TEMP_CHANNEL         0b1110
#define GND_CHANNEL          0b1100


#define NO_JOB                    0
#define PLAIN_ADC_JOB             1
#define VOLTAGE_ADC_JOB           2
#define TEMP_ADC_JOB              3
#define VCC_ADC_JOB               4


//Sawyer pins (shows default states and pullups)

//PA0 OUTPUT/0 L6234 EN1 (U)
//PA1 OUTPUT/0 L6234 IN1 (U)
//PA2 OUTPUT/0 L6234 EN2 (V)
//PA3 OUTPUT/0 L6234 IN2 (V)
//PA4 OUTPUT/0 L6234 EN3 (W)
//PA5 INPUT/P0 ADC2_PHOTOCELL_1
//PA6 OUTPUT/0 0C1B_H-BRIDGE_LEFT
//PA7 OUTPUT/0 L6234 IN3 (W)

//PB0 INPUT/P0 HALL_1
//PB1 I2C/OUTPUT MOSI/SDA_LINE (Pull low to send signal back to Master, otherwise leave in I2C mode)
//PB2 INPUT/P0 MISO_LINE/HALL_2 (shared temporary connection w/level shift for SPI programming)
//PB3 OUTPUT/0 OC1A_H-BRIDGE_RIGHT

//PC0 INPUT/P0 ADC9_PHOTOCELL_2
//PC1 I2C SCK/SCL_LINE
//PC2 INPUT/P1 LM339N/HALL (HU)
//PC3 RESET_LINE
//PC4 INPUT/P1 LM339N/HALL (HV)
//PC5 INPUT/P1 LM339N/HALL (HW)



///////// SAWYER DEFINES //////////////
#ifdef SAWYER

   #define I2C_ADDRESS 0x51
   
   //Global variables for tracking state (0 is used for commands, 1-4 are used for subcommands or misc data)
   #define Phase_of_Day      I2C_Array[6]
 

   //BLDC Shortcuts
   #define UOFF PORTA &= ~(1<<PA0)
   #define UON PORTA |= (1<<PA0)


   #define VOFF PORTA &= ~(1<<PA2)
   #define VON  PORTA |= (1<<PA2)


   #define WOFF PORTA &= ~(1<<PA4)
   #define WON PORTA |= (1<<PA4)
   
   #define ULO  PORTA &= ~(1<<PA1)
   #define UHI  PORTA |= (1<<PA1)
   #define VLO  PORTA &= ~(1<<PA3)
   #define VHI  PORTA |= (1<<PA3)
   #define WLO  PORTA &= ~(1<<PA7)
   #define WHI  PORTA |= (1<<PA7)
   
   #define HU   bit_is_set (PINC, PC2)
   #define HV   bit_is_set (PINC, PC4)
   #define HW   bit_is_set (PINC, PC5)

   #define NHU   bit_is_clear (PINC, PC2)
   #define NHV   bit_is_clear (PINC, PC4) 
   #define NHW   bit_is_clear (PINC, PC5)

/*
uvw
uwv
wvu
wuv
vwu
vuw
*/


   
   #define CDS1_CHANNEL         0b0010
   #define CDS2_CHANNEL         0b1001

   #define BANDGAP_CORRECTION       .000

   #define BLDC_LOCK_MODE       0
   #define BLDC_PWM_MODE        1
   #define BLDC_FAST_MODE       2
   
   #define LOCK_ANTIPHASE       1
   #define SIGN_MAGNITUDE       0
   
   //like clock positions if you are looking at sun to the south (toward the equator)
   #define NOON                 0
   #define SUNRISE              64
   #define SUNSET               192
   #define MORNING              1
   #define AFTERNOON            255
   #define NIGHT                180
   
   //Commands
   #define READ_CDS1            142
   #define READ_CDS2            242
   #define READ_HALL            143
   #define READ_GYROHALL        144
   #define GYROSTOP             147
   #define READ_GYRO_RPM        56
   #define H_BRIDGE_RAMPUP      88
   #define H_BRIDGE_PARK        90
   #define H_BRIDGE_ORBIT       92
   #define PWM_CLEAR            89
   #define H_BRIDGE_PULSE       112
   #define H_BRIDGE_RIGHT_ON    9
   #define H_BRIDGE_RIGHT_OFF   109
   #define H_BRIDGE_LEFT_ON     8
   #define H_BRIDGE_LEFT_OFF    108
   #define GYROTEST             222
   #define BLDCTEST             223
   #define SET_GYRORPM          57
   #define ROTATE               12

   
   //Jobs
   #define GYROTEST_JOB         101
   #define H_BRIDGE_JOB         102
   #define H_BRIDGE_PARK_JOB    104
   #define H_BRIDGE_ORBIT_JOB   105
   #define ROTATE_JOB           103

////SAWYER Function definitions
volatile void GyroTest (uint8_t myrpm);
volatile void Pwm_Clear (void);
volatile void H_Bridge_Up (void);
volatile void H_Bridge_Down (void);
volatile void H_Bridge_Pulse (void);
volatile uint8_t H_Bridge(uint8_t pwm_type, uint8_t acceleration, uint8_t direction, uint8_t speed, uint8_t sustain_time, uint8_t position);




//// SAWYER Global variables
volatile uint8_t BLDC_state = 255;
volatile uint8_t BLDC_delay = 0;
volatile uint8_t BLDC_mode = BLDC_PWM_MODE;


volatile uint32_t Stall_Timer;
volatile uint32_t Delay_Timer;
volatile uint32_t RPM_timer;
volatile uint8_t Hall_Debounce_Timer;
volatile uint8_t Hall_Debounce_Flag;



#endif

//Huckleberry pins (shows default states and pullups)
//Huckleberry device signature = 0x1e9412 - byte 4 is temp data

//PA0 OUTPUT/0 BATT1_7V_BOOST
//PA1 OUTPUT/0 CHARGER_RELAY (Default Batt 2)
//PA2 OUTPUT/0 BATT2_7V_BOOST
//PA3 INPUT/P0 ADC0_SOLAR_CHARGE_VOLTAGE
//PA4 OUTPUT/0 BATT2_5V_BOOST
//PA5 INPUT/P0 ADC2 BATT1_VOLTAGE
//PA6 OUTPUT/1 ESP_LDO
//PA7 OUTPUT/0 SOLAR_CHARGER_SWITCH

//PB0 OUTPUT/0 LATCHING_RELAY_PARALLEL
//PB1 I2C/OUTPUT MOSI/SDA_LINE (Pull low to send signal back to Master, otherwise leave in I2C mode)
//PB2 I2C MISO_LINE (Has level-shift, temporary connection for SPI programming)
//PB3 INPUT/P0 LIS3DH_INTERRUPT (combine this with SDA_LINE above to send back to Master)

//PC0 INPUT ADC9_BATT2_VOLTAGE
//PC1 I2C SCK/SCL_LINE
//PC2 OUTPUT/0 LATCHING_RELAY_SERIES
//PC3 RESET_LINE
//PC4 OUTPUT/0 BATT1_5V_BOOST
//PC5 OUTPUT/0 QI_CHARGER_SWITCH

/////////////// HUCKLEBERRY DEFINES //////////////
#ifdef HUCKLEBERRY

   #define I2C_ADDRESS 0x52

   //maybe get rid of the 5th place, may interfere with long commands
   //Global variables for tracking state (0 is used for commands, 1-4 are used for subcommands or misc data)
   #define Solar_Charge_Switch I2C_Array[5]
   #define Solar_Charge_Mode   I2C_Array[6]
   #define Solar_Power         I2C_Array[7]
   #define Boost_51            I2C_Array[8]
   #define Boost_52            I2C_Array[9]
   #define Boost_71            I2C_Array[10]
   #define Boost_72            I2C_Array[11]
   #define Charger_Relay       I2C_Array[12]
   #define Batt1_Voltage_High  I2C_Array[13]
   #define Batt1_Voltage_Low   I2C_Array[14]
   #define Batt2_Voltage_High  I2C_Array[15]
   #define Batt2_Voltage_Low   I2C_Array[16]
   #define Batt_Mode           I2C_Array[17]
   #define Qi_Charger          I2C_Array[18]
   #define Esp_Ldo             I2C_Array[19]

   
   #define BATT1_CHANNEL        0b0010
   #define BATT2_CHANNEL        0b1001
   #define SOLAR_CHARGE_CHANNEL 0b0000

   #define BANDGAP_CORRECTION       .081

   
   //Commands
   #define BATT1_VOLTAGE        110
   #define BATT2_VOLTAGE        120
   #define SOLAR_CHARGE_VOLTAGE 152
   #define SERIES_MODE          11
   #define PARALLEL_MODE        1
   #define CHARGER_RELAY        22
   #define BATT1                1
   #define BATT2                2
   #define SOLAR_CHARGE_SWITCH  44
   #define BOOST_51             51
   #define BOOST_52             52
   #define BOOST_71             71
   #define BOOST_72             72
   #define QI_CHARGER           91
   #define ESP_LDO              99
   #define SOLAR_CHARGE_MODE    157
   #define MISO_LED             183
   


// HUCKLEBERRY FUNCTION DECLARATIONS
   volatile uint8_t Switch_Battery_Relay (uint8_t mode);
   volatile uint8_t Switch_Charger_Relay (uint8_t mode);
   volatile uint8_t Switch_Solar_Charge (uint8_t mode);
   volatile uint8_t Switch_Boost_51 (uint8_t mode);
   volatile uint8_t Switch_Boost_52 (uint8_t mode);
   volatile uint8_t Switch_Boost_71 (uint8_t mode);
   volatile uint8_t Switch_Boost_72 (uint8_t mode);
   volatile uint8_t Switch_Qi_Charger (uint8_t mode);
   volatile uint8_t Switch_Esp_Ldo (uint8_t mode);
   volatile void Process_Solar_Charge_Mode (void);


// HUCKLEBERRY GLOBAL VARIABLES

#endif



//////////// MISC SHARED GLOBAL VARIABLES //////////////////

//Jobs are for actions that take a long time to process and need to be run in main loop
volatile uint8_t job = NO_JOB;            //flag to initiate job in main loop
volatile uint8_t job_params[5] = {0, 0, 0, 0, 0}; //max of 5 parameters, mirrors I2C_Array command params section

volatile uint8_t Read_Index = 0;
volatile uint8_t Write_Index = 0;
//create an array of 20 elements (enough to report all status back to trillsat_bot on each I2C read)
volatile uint8_t I2C_Array[I2C_ARRAY_SIZE] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};  //20
volatile uint32_t Timer = 0; //Counter for timing large values using 8-bit timer
volatile uint16_t Big_Timer = 0; //An even bigger timer
volatile uint8_t Small_Timer = 0; //A way to capture TCNT0
volatile uint32_t Morse_Validation_Timer = 0; //used to time input validation on morse interrupts
volatile uint16_t RPM = 0;
volatile uint16_t Set_RPM = 320;
volatile uint8_t phase_count = 0;
volatile uint8_t led_state = 1;
volatile uint8_t received_morse_letter = 61; // + 65 is letter ~
volatile uint8_t actual_morse_letter = 61; // + 65 is letter ~
volatile uint8_t haptic_lock_flag = 0;
volatile uint8_t thump_mode;

//H-Bridge Hall-Effect Sensor states for tracking state changes
//assumes craft is at noon to start (perhaps add function to set to noon to ensure)
volatile uint8_t Hall1 = 0; //a hall of 1 means on triggered
volatile uint8_t Hall2 = 0; //a hall of 1 means on triggered
volatile uint8_t Motor_Direction = DOWN;




////////// SHARED FUNCTION DECLARATIONS /////////////////



volatile void I2C_Command(void);
volatile void I2C_Init(uint8_t Address);

//volatile void pwm_test(void);
volatile uint16_t Plain_ADC_Read(uint8_t channel);
volatile uint16_t Voltage_ADC_Read(uint8_t channel);
volatile uint16_t Vcc_ADC_Read(void);
volatile uint16_t Temp_ADC_Read(void);
volatile void Push_Pull_Job(uint8_t newjob);
volatile void Process_Haptic_Command(uint8_t haptic_command, uint8_t confirmation);




int main(void){

   
   ///////// SAWYER DEFAULT STATES AND PULLUPS //////////
#ifdef SAWYER

    Phase_of_Day = NIGHT;  //motor should be told to move to NOON later to set default position
    
    
    //outputs for Port A
    //L6234 EN1, L6234 IN1, L6234 EN2, L6234 IN2, L6234 EN3, OC1B_H-BRIDGE_LEFT, L6234 IN3
    DDRA |= (1<<PA0)|(1<<PA1)|(1<<PA2)|(1<<PA3)|(1<<PA4)|(1<<PA6)|(1<<PA7);
    
    //set to 0, off
    PORTA &= ~(1<<PA0);
    PORTA &= ~(1<<PA1);
    PORTA &= ~(1<<PA2);
    PORTA &= ~(1<<PA3);
    PORTA &= ~(1<<PA4);  
    PORTA &= ~(1<<PA6);  
    PORTA &= ~(1<<PA7);

       
    //Set this Port A ADC to input
    DDRA &= ~(1<<PA5);    //ADC2_PHOTOCELL_1
    //set pullup to 0, off
    PUEA &= ~(1<<PA5);
        
    //outputs for Port B (ignore MOSI SDA feedback for now)
    //OC1A_H-BRIDGE_RIGHT
    DDRB |= (1<<PB3);
    //set to 0, off
    PORTB &= ~(1<<PB3);
    
    //Set these Port B inputs
    //HALL_1, HALL2/MISO_LINE_SHARED
    DDRB &= ~(1<<PB0);   
    DDRB &= ~(1<<PB2);   //this is shared with temporary MISO Line for SPI programming
                         //can't read HALL2 until MISO Line is removed and jumper to sensor is plugged in
                         //only send out MISO output briefly if hall not sinking, then return to input mode
    //set pullups to 0, off
    PUEB &= ~(1<<PB0);
    PUEB &= ~(1<<PB2);

    //this must be added to allow SIMTHEO.h interrupts to work
    //Enable pin change interrupt for range of pins (PCINT11 through PCINT8) and pin change mask for just PB3
    GIMSK |= (1<<PCIE1);KEY_INTERRUPT_ENABLE;

      
    //outputs for Port C (none)   
        
    //Set these Port C inputs
    DDRC &= ~(1<<PC0);    //ADC9 PHOTOCELL_2
    DDRC &= ~(1<<PC2);    //LM339N/HALL (HU)
    DDRC &= ~(1<<PC4);    //LM339N/HALL (HV)
    DDRC &= ~(1<<PC5);    //LM339N/HALL (HW)
    
    //enable external interrupts for the 3 hall sensors HU, HV, HW
    //Since HU is PCINT14, HV is PCINT16, HW is PCINT17, these fall under PCMSK2, PCIE2
    
    //don't enable interrupt until actually needed
    //GIMSK |= (1<<PCIE2);  //Enable pin change interrupt for that range of pins (PCINT17 through PCINT12)
    PCMSK2 |= (1<<PCINT14) | (1<<PCINT16) | (1<<PCINT17);//Enable pin change mask for just those 3 pins

    //set photocell pullup to 0, off
    PUEC &= ~(1<<PC0);
    
    //set gyro hall pullups to 0, off since 10k pullups are already present    
    PUEC &= ~(1<<PC2);
    PUEC &= ~(1<<PC4);
    PUEC &= ~(1<<PC5);

    morse_echo = 0; //disable immediate key echo on output
    morse_replay = 0; //disable replay output of received input
   
#endif

///////// HUCKLEBERRY DEFAULT STATES AND PULLUPS //////////
#ifdef HUCKLEBERRY

        
    //outputs for Port A
    //BATT2_5V_BOOST, BATT2_7V_BOOST, BATT1_7V_BOOST, SOLAR_CHARGER_SWITCH, CHARGER_RELAY, ESP_LDO
    DDRA |= (1<<PA4)|(1<<PA2)|(1<<PA0)|(1<<PA7)|(1<<PA1)|(1<<PA6);
    
    //set to 0, off
    PORTA &= ~(1<<PA4);
    PORTA &= ~(1<<PA2);
    PORTA &= ~(1<<PA0);
    PORTA &= ~(1<<PA7);
    PORTA &= ~(1<<PA1);      

    //set to 1, on
    PORTA |= (1<<PA6);   //drive ESP_LDO high, already a 10k on ESP LDO to 5v but helps.
                         //Enable pin input impedance is really high, microamps so should be fine 
    
    //Set these Port A ADCs to inputs
    DDRA &= ~(1<<PA3);    //ADC0 SOLAR_CHARGE_VOLTAGE
    DDRA &= ~(1<<PA5);    //ADC2 BATT1_VOLTAGE

    //set pullups to 0, off
    PUEA &= ~(1<<PA3);
    PUEA &= ~(1<<PA5);
    PUEA &= ~(1<<PA6);  //don't need pullup on ESP_LDO since driving high directly 

    //outputs for Port B (ignore MOSI SDA feedback for now)
    //LATCHING RELAY PARALLEL
    DDRB |= (1<<PB0);
    //set to 0, off
    PORTB &= ~(1<<PB0);
    
    //Set these Port B inputs
    DDRB &= ~(1<<PB3);    

    DDRB &= ~(1<<PB2);    //MISO Line

    //set interrupt pullup to 0, off (or else it will overvolt the LIS3DH 3.3v logic)
    PUEB &= ~(1<<PB3);
    //set MISO pullup to 0, off
    PUEB &= ~(1<<PB2);
    
    //only send out MISO output briefly if hall not sinking, then return to input mode


  //this must be added to allow SIMTHEO.h interrupts to work

//Enable pin change interrupt for range of pins (PCINT11 through PCINT8) and pin change mask for just PB3
 GIMSK |= (1<<PCIE1);PCMSK1 |= (1<<PCINT11);




      
    //outputs for Port C
    //LATCHING_RELAY_SERIES, BATT1_5V_BOOST, QI_CHARGER_SWITCH
    DDRC |= (1<<PC2)|(1<<PC4)|(1<<PC5);
    //set to 0, off
    PORTC &= ~(1<<PC2);
    PORTC &= ~(1<<PC4);
    PORTC &= ~(1<<PC5);     
        
    //Set this Port C ADC to input
    DDRC &= ~(1<<PC0);    //ADC9 BATT2_VOLTAGE
    //set pullup to 0, off
    PUEC &= ~(1<<PC0);  

    morse_echo = 1; //enable immediate key echo on output
    morse_replay = 1; //enable replay output of received input 
    MORSE_ON; //on boot, keep LED on to allow saywer programming 


#endif

  ////////// INITIALIZE ADC ////////////////
   //Disable power reduction for the ADC to make sure it is enabled
   PRR &= ~(1<<PRADC);

   //Set voltage reference to Vcc (approx 5 volts in actuality)
   ADMUX &= ~(1<<REFS1);
   ADMUX &= ~(1<<REFS0);
   
   ADCSRA |= (1<<ADEN); // Enable ADC
   ADCSRA |= 0b111;  //Enable all 3 bits of ADPS prescaler bits to set prescaler to CLK/128
   ADCSRB &= ~(1<<ADLAR); // Set to 10-bit resolution (no left shift result)

//put main code in while loop later--don't put chips to sleep since it needs
//use global variables to maintain state
//use a just-powered-on variable to track if a sudden reset occurred
//allow xmpp to poll and keep status updates of states of huckleberry and sawyer

//add battery charging circuitry to keep topped up if charge flag is set

//////// INITIALIZE I2C ////////////////////

//set_sleep_mode(SLEEP_MODE_PWR_DOWN);

TWSCRA |= _BV(TWDIE);  //enable data interrupt
TWSCRA |= _BV(TWASIE); //enable address/stop interrupt
TWSCRA |= _BV(TWEN);   //enable slave interface
TWSCRA |= _BV(TWSIE);  //enable stop interrupt
TWSCRA |= _BV(TWSHE); //hold time enable  
TWSA = I2C_ADDRESS<<1; //set high 7 bits to slave address (set in defines for Huck and Saw)


///////// INITIALIZE 8-BIT TIMER 0 FOR INTERNAL TIMING /////////

  //TCCR0B |= (1 << CS00) | (1 << CS02); // Start timer 0 at max prescaler of Fcpu/1024 
  //OCR0A values of 90 to 254 are visibly detectable via LED
  //For 8-bit timer, the time-to-overflow is 256 x (Prescaler/8000000) = 0.032768, or ~33 ms
  //Shortest time that can be read is that divided by 256 = 0.000128, or 128 us
  //max pulses per minute (related to RPM) is 60/0.000128 = 468750, for gyro divide by 36 = 13020.8 max speed
  //(gyro 13020 / 10 pwm pulses) / 2 = 651 max rpm

  TCCR0B |= (1 << CS01); // Start timer 0 at max prescaler of Fcpu/8
  //time-to-overflow is 0.000256 or 256 us
  //shortest time is 0.000001 or 1 us
  //so if you take overflow x 3906.25 = 1 second.

  //TCCR0B |= (1 << CS00) | (1 << CS01); // Start timer 0 at max prescaler of Fcpu/64
  //time-to-overflow is 0.002048, or ~ 2 ms
  //shortest time is 0.000008, or 8 us

  //max pulses per minute is 7500000, gyro 208333
  //(gyro 208333 / 10 pwm pulses) / 2 = 10416 max rpm

  //TCCR0B |= (1 << CS02);    //Start timer 0 at max prescaler of Fcpu/256
  //time-to-overflow is 0.008192 seconds, or ~8 ms
  //shortest time segment is 0.000032 seconds, or 32 us
  //for a 36-step BLDC gyro at 10000 max rpm, there is 167 us



  TCCR0A |= (1 << COM0A1);  //Clear OC0A on Compare Match

  //Set Waveform Mode 3 Fast PWM -- good enough for gyro
  TCCR0A |= (1 << WGM00) | (1 << WGM01);

  //Enable both interrupts for both (up/down) phases of PWM, and for general clocking
  TIMSK |= (1 << TOIE0);  // Overflow interrupt (MAX), this also allows me to increment Big_Timer variable
  TIMSK |= (1 << OCIE0A); // CTC interrupt (BOTTOM), which can also run things at a fraction of 255
  
  //This value acts like a compare voltage on a sawtooth waveform
  //If it is set high, only the peaks are matched, and the on pulse is very narrow
  //So the duty cycle is very low
  OCR0A = 175; //Bigger values, lower voltage, 210 is just about too slow, lower is faster

  // Note that OC0A/OC0B wont toggle real pin since they are set to inputs on both Huckleberry and Sawyer
  //read Big_Timer for long time, read TCNT0 if you just need a smaller time


   

  sei(); //  Enable global interrupts (needed for I2C, 8-bit timer CTC interrupt, and pin change interrupts)
   
I2C_Array[0] = JUST_STARTED;
uint16_t two_bytes;

//set haptic lock flag if eeprom code is valid [A-Z][A-Z]
if (eeprom_read_byte((uint8_t*)0) >= 65 && eeprom_read_byte((uint8_t*)0) <= 90 && eeprom_read_byte((uint8_t*)1) >= 65 && eeprom_read_byte((uint8_t*)1) <= 90) {
           haptic_lock_flag = 1;
}

//set 16-bit timer to 1024 prescaler for THUMP
TCCR1B |= (1 << CS10) | (1 << CS12);

KEY_INTERRUPT_ENABLE; //enable thump mode on boot
thump_mode = 1;

while (1) {

///////////// MAIN LOOP ///////////////// 
      
//jobs take priority


switch (job) {

 
   
      case PLAIN_ADC_JOB:
          two_bytes = Plain_ADC_Read(job_params[0]);
          I2C_Array[1] = two_bytes & 0x00ff; //low byte
          I2C_Array[2] = two_bytes >> 8;     //high byte
          job = SUCCESS;
          break;

      case VCC_ADC_JOB:
          two_bytes = Vcc_ADC_Read();
          I2C_Array[1] = two_bytes & 0x00ff; //low byte
          I2C_Array[2] = two_bytes >> 8;     //high byte
          job = SUCCESS;
          break;

       case TEMP_ADC_JOB:
          two_bytes = Temp_ADC_Read();
          I2C_Array[1] = two_bytes & 0x00ff; //low byte
          I2C_Array[2] = two_bytes >> 8;     //high byte
          job = SUCCESS;
          break;


    
///////// HUCKLEBERRY MAIN LOOP - JOBS /////////////
#ifdef HUCKLEBERRY

       case VOLTAGE_ADC_JOB:
          two_bytes = Voltage_ADC_Read(job_params[0]);
          I2C_Array[1] = two_bytes & 0x00ff; //low byte
          I2C_Array[2] = two_bytes >> 8;     //high byte
          job = SUCCESS;
          break;

#endif

////////////// SAWYER MAIN LOOP - JOBS /////////////////
#ifdef SAWYER

       case GYROTEST_JOB:
          GyroTest(job_params[0]);
          job = SUCCESS;
          break;
          
          
       //note that this only waits for function to complete and considers completion a success
       //before returning its own success
       //it really should check the output of the function to see if there was another condition (such as motor stall)
       //I made H_Bridge function return a variable, so this can be looked at if needed - fix later 
       //stops at position noon, unless otherwise set - fix later
       case H_BRIDGE_JOB:

          //temporarily disable thump interrupt if thump is enabled
          if (thump_mode == 1) {KEY_INTERRUPT_DISABLE;}
         
          H_Bridge(job_params[0],job_params[1],job_params[2],job_params[3],job_params[4],NOON);

          //now re-enable THUMP interrupt and set 16-bit timer to 1024 prescaler if thump is enabled
          if (thump_mode == 1) {TCCR1B |= (1 << CS10) | (1 << CS12);KEY_INTERRUPT_ENABLE;}

          job = SUCCESS;
          break;          

       case H_BRIDGE_PARK_JOB:

          //temporarily disable thump interrupt if thump is enabled
          if (thump_mode == 1) {KEY_INTERRUPT_DISABLE;}
        
          //Here are the 3 Trident Orbital Mechanics
          //These values need to be tweaked in real-world use
          //to ensure correct speed and torque
      
          Motor_Direction = DOWN;
          H_Bridge(SIGN_MAGNITUDE, 15, Motor_Direction, 20, 100, SUNRISE);

          //then, ramp down towards noon, but don't reach it
          H_Bridge(SIGN_MAGNITUDE, 253, Motor_Direction, 20, 10, NOON); 

          //then, ramp up in reverse to lift craft and go to sunset
          Motor_Direction = UP;
          H_Bridge(SIGN_MAGNITUDE, 15, Motor_Direction, 20, 100, SUNSET);   
  
          //continue past sunset to noon (you can make speed changes here if needed)
          H_Bridge(SIGN_MAGNITUDE, 0, Motor_Direction, 20, 100, NOON);  
       
          //at noon, switch to locked antiphase and slow to SUNRISE
          H_Bridge(LOCK_ANTIPHASE, 250, Motor_Direction, 20, 100, SUNRISE);  

          //now re-enable THUMP interrupt and set 16-bit timer to 1024 prescaler if thump is enabled
          if (thump_mode == 1) {TCCR1B |= (1 << CS10) | (1 << CS12);KEY_INTERRUPT_ENABLE;}

          job = SUCCESS;
          break;

       case H_BRIDGE_ORBIT_JOB:          
          //temporarily disable thump interrupt if thump is enabled
          if (thump_mode == 1) {KEY_INTERRUPT_DISABLE;}

          //two params:  [direction][# of orbits]
          Motor_Direction = job_params[0]; //this global variable is required for interrupt to compute positions correctly

          //pwm_type, acceleration, direction, speed, sustain_time, position
          //need to use LOCK_ANTIPHASE to slow it down on downhills, then really ramp it up on uphills
          //make separate function so it can be called by THUMP and then Qi charger auto turns on

          if (Motor_Direction == DOWN) {
             H_Bridge(SIGN_MAGNITUDE, 25, DOWN, 150, 100, NOON);  //first, ramp to NOON, boosting power if needed

             for (uint8_t orbcnt = 0; orbcnt < job_params[1]; orbcnt++) { //job_params[1] is the number of orbits 
               H_Bridge(SIGN_MAGNITUDE, 0, DOWN, 80, 100, SUNSET);   //no accel, since it is already moving
               H_Bridge(SIGN_MAGNITUDE, 0, DOWN, 70, 100, SUNRISE);  //no accel, but boost speed on lifting mass
               H_Bridge(SIGN_MAGNITUDE, 0, DOWN, 40, 100, NOON); //no accel, since it is already moving
             }
          } else { //if UP   
               H_Bridge(SIGN_MAGNITUDE, 25, UP, 150, 100, NOON);  //first, ramp to NOON, boosting power if needed

               for (uint8_t orbcnt = 0; orbcnt < job_params[1]; orbcnt++) { //job_params[1] is the number of orbits
                  H_Bridge(SIGN_MAGNITUDE, 0, UP, 40, 100, SUNRISE);   //no accel, since it is already moving
                  H_Bridge(SIGN_MAGNITUDE, 0, UP, 70, 100, SUNSET);  //boost power on uphill
                  H_Bridge(SIGN_MAGNITUDE, 0, UP, 40, 100, NOON);  //no accel, since it is already moving
               }
          }  

          //now re-enable THUMP interrupt and set 16-bit timer to 1024 prescaler if thump is enabled
          if (thump_mode == 1) {TCCR1B |= (1 << CS10) | (1 << CS12);KEY_INTERRUPT_ENABLE;}

          job = SUCCESS;
          break;

       case ROTATE_JOB:
          //temporarily disable thump interrupt if thump is enabled
          if (thump_mode == 1) {KEY_INTERRUPT_DISABLE;}

          Motor_Direction = job_params[0]; //this global variable is required for interrupt to compute positions correctly
          //Only inputs direction and position, sustains for 10 seconds or until position is reached
          H_Bridge(0,7,Motor_Direction,50,50,job_params[1]); 

          //now re-enable THUMP interrupt and set 16-bit timer to 1024 prescaler if thump is enabled
          if (thump_mode == 1) {TCCR1B |= (1 << CS10) | (1 << CS12);KEY_INTERRUPT_ENABLE;}

          job = SUCCESS;
          break;

#endif
}

///////// HUCKLEBERRY MAIN LOOP - OTHER /////////////
#ifdef HUCKLEBERRY

   //Don't run if existing job, since I2C might not have had a chance to read the variables
      //yet and this can overwrite them
      //if I2C creates new job, it will catch it when done
          

        if (job == NO_JOB && Solar_Charge_Mode == ON && Solar_Power == ON) {
           Process_Solar_Charge_Mode();
        }

        // if no solar power switch to night mode
        // if batteries too low, switch to low power mode
        // if batteries extremely low, shut off all power to them


        //Update solar power status (this slows loop), put in timer later
        //if voltage is greater than half, 2.5 volts, is considered on
        if (job == NO_JOB & Plain_ADC_Read(SOLAR_CHARGE_CHANNEL) > 512) 
           {Solar_Power = ON; 
        } else {Solar_Power = OFF;}

        //flash led using 8-bit timer overflow as test of speed

//Can check TCNT0 for quick timing
//Or check Big_Timer for larger timing
//Can also set main loop functions to operate on set times instead of constantly
/*
        if (TCNT0 % 5 == 0) {

           //DO SOMETHING 

        }  
*/


#endif
////////////// SAWYER MAIN LOOP - OTHER /////////////////

#ifdef SAWYER


#endif
  
}

}

//////////// PUSH PULL JOB ///////////////
volatile void Push_Pull_Job(uint8_t newjob) {
   
      if (job == SUCCESS) {
        //done, return success code back to pi so it knows to read results
        job = NO_JOB;
        job_params[0] = 0;job_params[1] = 0; job_params[2] = 0; job_params[3] = 0; job_params[4] = 0;  //clear params
        I2C_Array[0] = SUCCESS;
      } else if (job == NO_JOB) {
         //start a new job
         job = newjob;
         I2C_Array[0] = newjob;
      } else if (job == newjob) {
         I2C_Array[0] = newjob;
      }   
      
  
}

////////////// PLAIN ADC READ FUNCTION ////////////////////
volatile uint16_t Plain_ADC_Read(uint8_t channel) { //returns 10-bit value from ADC, as precise as possible
   
      uint16_t ten_bit_adc;
      uint16_t sum = 0; //this is large enough to store 64 reads for a sum if needed (but I only need 24)
      uint8_t low_byte;
      uint8_t high_byte;
         
      ADMUX &= ~0b1111; //clear last 4 bits     
      ADMUX |= GND_CHANNEL; // drain off any energy from capacitor

      _delay_ms(3);

      ADCSRA |= (1<<ADSC);  // Start conversion by setting ADSC bit high
      while (ADCSRA & (1<<ADSC)) {} //Endless loop until ADSC bit goes low to confirm conversion complete
       
      _delay_ms(10);

      ADMUX &= ~0b1111; //clear last 4 bits
      ADMUX |= channel; 
 
      _delay_ms(500);

      // now perform 24 reads and average the last 12
      for( uint8_t cnt = 0; cnt < 24; cnt = cnt + 1 ) {
          ADCSRA |= (1<<ADSC);  // Start conversion by setting ADSC bit high
          while (ADCSRA & (1<<ADSC)) {} //Endless loop until ADSC bit goes low to confirm conversion complete
          
      low_byte = ADCL; //low byte must be read first
      high_byte = ADCH;

      ten_bit_adc = (ADCH<<8) | low_byte; 
      if (cnt > 11 ) sum = sum + ten_bit_adc;
      _delay_ms(10);
      }  
      ten_bit_adc = sum / 12;

      return ten_bit_adc;

}

///////////////////// VCC ADC READ FUNCTION ///////////////
volatile uint16_t Vcc_ADC_Read (void) { //converts ADC Vcc to decimal voltage value in 2-byte integer form

          //fix the math later to remove float since function below is simple
          float vcc;
          uint16_t decimal_vcc;
          
          //first compare 1.1 ref voltage against Vcc to compute actual Vcc voltage
          vcc = ((1.1 - BANDGAP_CORRECTION ) * 1024 / (Plain_ADC_Read(BANDGAP_CHANNEL) - 1)); 

          //move decimal to keep precision of 4 decimal points, works well since max volts wont exceed 6.5536
          decimal_vcc = vcc * 10000;

          return decimal_vcc;
}


///////////////////// TEMP ADC READ FUNCTION ///////////////
volatile uint16_t Temp_ADC_Read (void) { //converts ADC to crude temp value in 2-byte integer form

       uint16_t decimal_temp;

      //first set voltage reference to internal 1.1 v regulator
      ADMUX |= (1<<REFS1);
      ADMUX &= ~(1<<REFS0);
      
      decimal_temp = Plain_ADC_Read(TEMP_CHANNEL);
      
      //Set voltage reference back to normal 5v Vcc
      ADMUX &= ~(1<<REFS1);
      ADMUX &= ~(1<<REFS0);

      //just return value, let python handle computation   
      return decimal_temp;
}



/////////// I2C (2-wire) Interrupt //////////////////
ISR(TWI_SLAVE_vect) {
  if (bit_is_set(TWSSRA,TWASIF)) {               // if address or stop received
    //cli();
    
    if (bit_is_clear(TWSSRA,TWAS)) {             // if stop
         Read_Index = 0; Write_Index = 0;
         TWSCRB = (3 << TWCMD0);                 // complete

         //do something with array before going to sleep again
         I2C_Command();

         //turn off sleep since huckleberry and sawyer need to be running/monitoring all the time
         //but keep interrupts enabled
         //sleep_enable();                         // only put to sleep after a stop or it won't wake up fast enough
         //sei();
         //sleep_cpu();
         //sleep_disable();
    }

    else if (bit_is_set(TWSSRA,TWDIR)) {         // if address, master read
      TWSCRB = 3<<TWCMD0;                        // ACK behavior
      
    } else {                                     // if address, master write
      TWSCRB = 3<<TWCMD0;                        // ACK behavior
    }
 //sei();  

  } else if (bit_is_set(TWSSRA,TWDIF)) {         // Data interrupt
     cli();
     if (bit_is_set(TWSSRA,TWDIR)) {             // data read
      if (Read_Index < I2C_ARRAY_SIZE) {         //this check takes too long at 100 Khz, cutting off the highest data bit on TWSD, so I had
                                                 //to slow down to 50 Khz to allow this to work around bug
        TWSD = I2C_Array[Read_Index++];          
        TWSCRB = 3<<TWCMD0;                      // ACK behavior
        
      } else 
TWSCRB = (2 << TWCMD0);             // complete
    } else {                                     // data write
      if (Write_Index < I2C_ARRAY_SIZE) {
      	I2C_Array[Write_Index++] = TWSD;
      	TWSCRB = 3<<TWCMD0;                     // ACK behavior
      }      
      else TWSCRB = (1 << TWAA) | (2 << TWCMD0); // NACK behavior and complete
    }
    //sei();
  } 
}

/////////////////////////////////////////////////////
  // ADC Interrupt Is Used To Wake Up CPU From Sleep Mode (NOT CURRENTLY USED)
//ISR(ADC_vect) { }
/////////////////////////////////////////////////////
#ifdef HUCKLEBERRY
//Set pin change interrupt and pin change mask for the key pin

ISR(MORSE_INTERRUPT_VECTOR) { 
KEY_INTERRUPT_DISABLE;  //disable morse key interrupts
sei(); //this is necessary to allow other interrupts (like I2C) to continue to work

uint8_t temp_morse_letter = 61;

if ((Timer > Morse_Validation_Timer) && (Timer < Morse_Validation_Timer + 5000000)) { //5 second delay
   temp_morse_letter = read_morse();

   //if unlocked, check for command sequence
   if (haptic_lock_flag == 0) { 
     if (temp_morse_letter == 26 || temp_morse_letter == 27) { //if F2 "validation" key is read, see if in time frame to validate to actual
        actual_morse_letter = received_morse_letter;
        received_morse_letter = 61;
     }
   //if locked, check for unlock sequence 
   //if confirmation is [A-Z] and not F1 or F2, check eeprom to see if unlock sequence matches
   } else if (temp_morse_letter >= 0 && temp_morse_letter <= 25) {        
        if (eeprom_read_byte((uint8_t*)0) == received_morse_letter + 65 && eeprom_read_byte((uint8_t*)1) == temp_morse_letter + 65) {
           haptic_lock_flag = 0; //set to unlocked (until chip reboots)
        } 
   }
  
} else {
     //just store value, clear actual, and reset timer
     received_morse_letter = read_morse();
     actual_morse_letter = 61;
     Morse_Validation_Timer = Timer;
} 

if (morse_replay == 0) {
  for (uint8_t delcnt = 0; delcnt < 5; delcnt++) {
    _delay_ms (1000); //5 second delay if pulsemode = 1 to prevent sawyer's vibration from sending command to huckleberry
  } 
  Morse_Validation_Timer = Timer;  //reset timer again to allow enough time to enter command
}

//do something with actual, then clear actual
if (actual_morse_letter != 61) {
  Process_Haptic_Command(actual_morse_letter, temp_morse_letter); 
}

KEY_INTERRUPT_ENABLE; //now re-enable morse key interrupts
}
#endif

//////////////////////////////////////////////////////

#ifdef SAWYER

//External pin interrupt for Sawyer to pulse motor in response to low-to-high transitions on PB2 (MISO/LED)

ISR(MORSE_INTERRUPT_VECTOR) { 
  KEY_INTERRUPT_DISABLE;  //disable morse key interrupts
  sei(); //this is necessary to allow other interrupts (like I2C) to continue to work


  //since it was a pin change, send a short haptic pulse
  //however, this will cause problems during programming Huckleberry since Sawyer will see the signal fluctuations

  uint8_t temp_morse_letter = 61;

if ((Timer > Morse_Validation_Timer) && (Timer < Morse_Validation_Timer + 5000000)) { //5 second delay
   temp_morse_letter = read_morse();

   //if unlocked, check for command sequence
   if (haptic_lock_flag == 0) { 
     if (temp_morse_letter == 26 || temp_morse_letter == 27) { //if F2 "validation" key is read, see if in time frame to validate to actual
        actual_morse_letter = received_morse_letter;
        received_morse_letter = 61;
     }
   //if locked, check for unlock sequence 
   //if confirmation is [A-Z] and not F1 or F2, check eeprom to see if unlock sequence matches
   } else if (temp_morse_letter >= 0 && temp_morse_letter <= 25) {        
        if (eeprom_read_byte((uint8_t*)0) == received_morse_letter + 65 && eeprom_read_byte((uint8_t*)1) == temp_morse_letter + 65) {
           haptic_lock_flag = 0; //set to unlocked (until chip reboots)
         }  

   }
  
} else {
     //just store value, clear actual, and reset timer
     received_morse_letter = read_morse();
     actual_morse_letter = 61;
     Morse_Validation_Timer = Timer;
} 

if (morse_replay == 0) {
  for (uint8_t delcnt = 0; delcnt < 5; delcnt++) {
    _delay_ms (1000); //5 second delay if pulsemode = 0 to prevent sawyer from listening to huckleberry's replays
  } 
  Morse_Validation_Timer = Timer;  //reset timer again to allow enough time to enter command
}


//do something with actual, then clear actual
if (actual_morse_letter != 61) {
  Process_Haptic_Command(actual_morse_letter, temp_morse_letter); 
}


  KEY_INTERRUPT_ENABLE; //now re-enable morse key interrupts
}

/////////////////////////////////////////////////////

//need to pull from 16 bit hardware timer since I can't get accurate time when in this interrupt

//External pin interrupt for the 3 gyro hall sensors
ISR(PCINT2_vect) { //PCINT2 triggers for PCINT17 through PCINT12, which covers the 3 sensors
Small_Timer = TCNT0;
Stall_Timer = Timer + Small_Timer; //anytime pin change occurs, reset stall detection

//time 6 36-step revolutions
if (phase_count < 216) {
   phase_count++;
} else {
phase_count = 0;
RPM = 60000000/(Timer + Small_Timer - RPM_timer);RPM_timer = Timer + Small_Timer;
//try to dynamically, slowly adjust speed every 6 revs if needed

  if (BLDC_mode == BLDC_LOCK_MODE) {
    if (RPM < Set_RPM) {
       if (BLDC_delay > 0) BLDC_delay--;
    } else {
       if (BLDC_delay < 255) BLDC_delay++;
    }

  } else if (BLDC_mode == BLDC_PWM_MODE) {
    if (RPM < Set_RPM) {
       if (OCR0A > 0) OCR0A--;
    } else {
       if (OCR0A < 254) OCR0A++; //Doesn't reach 255 to shut off
    }


  }

   
}

//BLDC_delay keeps state held until its value passes, a value of 50 is very slow        
//each delay value is 256 us, or .2 ms  
//real gyrohall readings are (u,v,w counter clockwise):
//101
//100
//110
//010
//011
//001

//120 degree hall

 
    if (BLDC_state != 255) {

     if (BLDC_mode != BLDC_LOCK_MODE) { //if PWM or FAST mode, simply pulse stator directly
       //note that state varibles slow the routine
       if (HU && NHV && HW) {
             BLDC_state = 1;
             UHI;UON;VLO;VON;WLO;WOFF;
       } else if (HU && NHV && NHW) {
             BLDC_state = 2;  
             UHI;UON;VLO;VOFF;WLO;WON;
       } else if (HU && HV && NHW) {
             BLDC_state = 3;   
             UHI;UOFF;VHI;VON;WLO;WON;
       } else if (NHU && HV && NHW) {
             BLDC_state = 4;
             ULO;UON;VHI;VON;WHI;WOFF;
       } else if (NHU && HV && HW) {
             BLDC_state = 5;
             ULO;UON;VHI;VOFF;WHI;WON;
       } else if (NHU && NHV && HW) {
             BLDC_state = 6;
             ULO;UOFF;VLO;VON;WHI;WON;   
       }

    } else { //in LOCK mode just change the state, have overflow interrupt add delay and pulse stator
      if (HU && NHV && HW) {
             BLDC_state = 1;
       } else if (HU && NHV && NHW) {
             BLDC_state = 2;  
       } else if (HU && HV && NHW) {
             BLDC_state = 3;   
       } else if (NHU && HV && NHW) {
             BLDC_state = 4;
       } else if (NHU && HV && HW) {
             BLDC_state = 5;
       } else if (NHU && NHV && HW) {
             BLDC_state = 6;
            
       }
    
     }




    }


}
#endif
//////////////////////////////////////////////
//8-bit Timer Overflow interrupt
//This is used for longer term accurate clocking updates
ISR(TIMER0_OVF_vect) // BOTTOM in mode 5 Phase Correct PWM
{

//1000000 = 1 second
//if this gets too long it will delay pin change interrupt also


if (Timer != 4294967040) {
    Timer += 256;
} else {
    Timer = 0;
    if (Big_Timer != 65535) {
        Big_Timer++;
    } else {
    Big_Timer = 0;
    } 
}


#ifdef SAWYER


if (Hall_Debounce_Flag == ON) {
   Hall_Debounce_Timer = Hall_Debounce_Timer + 1;
   if (Hall_Debounce_Timer == 255) {Hall_Debounce_Timer = 0; Hall_Debounce_Flag = OFF;}

} else if (Hall1 != bit_is_set(PINB, PB0) || Hall2 != bit_is_set(PINB, PB2)) { //by anding the pin and port, we can get bit state 1 or 0, a 0 is a trip.
   //we have a mismatch--hall states have changed (doesn't normally occur when gyro is in use so shouldn't delay gyro much
   //now take more time to determine what is the current state
   Hall1 = bit_is_set(PINB, PB0); Hall2 = bit_is_set(PINB, PB2);  //slight delay here since pins are evaluated twice--could cause timing error

   //if state change, activate debounce delay to slow it down by 256 times... (about 1/15th of a second delay)
   Hall_Debounce_Flag = ON;   

   
   if (Phase_of_Day == NOON) {
      if (bit_is_set(PINB, PB0) && bit_is_set(PINB, PB2)) {  //if previous was noon, but now only 1 hall is tripped, disregard, is still "stuck on noon" so to speak
         if (Motor_Direction == DOWN) {Phase_of_Day = AFTERNOON;}
         else if (Motor_Direction == UP) {Phase_of_Day = MORNING;}
      } 

   //Add catch-all to reset to noon, for whatever reason, if not noon (makes later checks for noon redudant--remove later)
   }  else if (bit_is_clear(PINB, PB0) && bit_is_clear(PINB, PB2)) {
        Phase_of_Day = NOON;
      
   }  else if (Phase_of_Day == AFTERNOON) {
        if (Motor_Direction == UP) {
           if (bit_is_clear(PINB, PB0) && bit_is_clear(PINB, PB2)) {Phase_of_Day = NOON;}
        } else if (Motor_Direction == DOWN) {
           if (bit_is_clear(PINB, PB0)) {Phase_of_Day = SUNSET;}  //Hall1 is the sunset position
        }   
        
        
   }  else if (Phase_of_Day == MORNING) {
        if (Motor_Direction == DOWN) {
           if (bit_is_clear(PINB, PB0) && bit_is_clear(PINB, PB2)) {Phase_of_Day = NOON;}
        } else if (Motor_Direction == UP) {
           if (bit_is_clear(PINB, PB2)) {Phase_of_Day = SUNRISE;} //Hall2 is the sunrise position
        } 

   }  else if (Phase_of_Day == SUNSET) {
        if (Motor_Direction == UP) {
           if (bit_is_set(PINB, PB0) && bit_is_set(PINB, PB2)) {Phase_of_Day = AFTERNOON;}
        } else if (Motor_Direction == DOWN) {
           if (bit_is_set(PINB, PB0) && bit_is_set(PINB, PB2)) {Phase_of_Day = NIGHT;}
        }   
        
        
   }  else if (Phase_of_Day == SUNRISE) {
        if (Motor_Direction == DOWN) {
           if (bit_is_set(PINB, PB0) && bit_is_set(PINB, PB2)) {Phase_of_Day = MORNING;}
        } else if (Motor_Direction == UP) {
           if (bit_is_set(PINB, PB0) && bit_is_set(PINB, PB2)) {Phase_of_Day = NIGHT;}
        } 
   }  else if (Phase_of_Day == NIGHT) {
        if (Motor_Direction == DOWN) {
           if (bit_is_clear(PINB, PB2)) {Phase_of_Day = SUNRISE;} 
        } else if (Motor_Direction == UP) {
           if (bit_is_clear(PINB, PB0)) {Phase_of_Day = SUNSET;}
        } 
   }   

   
}

   
if (BLDC_mode != BLDC_FAST_MODE) {
//Turn L6234 half bridges OFF for PWM chopping, 50 khz max
UOFF;VOFF;WOFF;
}

//shut off gyro if stall condition detected
//500000 timer ticks is 1/2 second
//don't set lower than BLDC_delay or it will consider the delay a stall condition
if (BLDC_state != 255 && BLDC_state != 0) { //if gyro is running
   if (Timer + TCNT0 > Stall_Timer + 500000) {
     GIMSK &= ~(1<<PCIE2);  //disable gyro hall pin interrupts
     BLDC_state = 255;
     RPM = 0;
     UOFF;VOFF;WOFF;
   }
}  

 if (BLDC_state != 255 && BLDC_state != 0 && BLDC_mode == BLDC_LOCK_MODE) {
    if (Timer + TCNT0 > Delay_Timer + BLDC_delay * 100) {

       if (BLDC_state == 1) {
          UHI;UON;VLO;VON;WLO;WOFF;
          Delay_Timer = Timer + TCNT0;
       } else if (BLDC_state == 2) {
          UHI;UON;VLO;VOFF;WLO;WON;
          Delay_Timer = Timer + TCNT0;
       } else if (BLDC_state == 3) {
          UHI;UOFF;VHI;VON;WLO;WON;
          Delay_Timer = Timer + TCNT0;
       } else if (BLDC_state == 4) {
          ULO;UON;VHI;VON;WHI;WOFF;
          Delay_Timer = Timer + TCNT0;
       } else if (BLDC_state == 5) {
          ULO;UON;VHI;VOFF;WHI;WON; 
          Delay_Timer = Timer + TCNT0;
       } else if (BLDC_state == 6) {
          ULO;UOFF;VLO;VON;WHI;WON;
          Delay_Timer = Timer + TCNT0;
       }
   }
 } else if (BLDC_state == 0) {
       Delay_Timer = 0;
       OCR0A = 100; //increase voltage for start
       sei();
       PCINT2_vect();
       OCR0A = 175; //decrease voltage after running
 }

#endif

//turn off LED
//DDRB |= (1<<PB2);
//set MISO to low
//PORTB &= ~(1<<PB0);



//Timer += TCNT0;
}

////////////////////////////////////////////////////
  //8-bit Timer CTC interrupt
  //This is used for certain actions at only a fraction of 255

ISR(TIMER0_COMPA_vect) // in Mode 1 Phase Correct PWM
{
#ifdef SAWYER

//Turn L6234 half bridges ON for PWM chopping, 50 khz max

switch (BLDC_state) {
  case 1:
     UON;VON;
     break;
  case 2:
     UON;WON;
     break;
  case 3:
     VON;WON;
     break;
  case 4:
     UON;VON;
     break;
  case 5: 
     UON;WON;
     break;
  case 6:
     VON;WON;


}
#endif

//turn on on LED, set MISO back to input
//DDRB &= ~(1<<PB2);      


//later - add scheduled routines to launch here instead of constant main loop
/*
if (Timer % 5 == 0)  {
     // do something
}
*/

}

///////////// FROM I2C INTERRUPT //////////////

//////////// SHARED I2C COMMANDS //////////////
volatile void I2C_Command(void) {
  //ADC READS 
  
 switch (I2C_Array[0]) {
    case VCC_VOLTAGE:
      Push_Pull_Job (VOLTAGE_ADC_JOB);
      break;
      
    case ATTINY_TEMP:
      Push_Pull_Job (TEMP_ADC_JOB);
      break;

    case CTC:
      OCR0A = I2C_Array[1];
      I2C_Array[0] = SUCCESS;
      break;

    case RECEIVE_MORSE:
      I2C_Array[1] = actual_morse_letter;
      I2C_Array[0] = SUCCESS;
      break;

    case THUMP_ENABLED:
      if (I2C_Array[1] == 1) {KEY_INTERRUPT_ENABLE;thump_mode = 1;}
      if (I2C_Array[1] == 0) {KEY_INTERRUPT_DISABLE;thump_mode = 0;}
      I2C_Array[0] = SUCCESS;
      break;

    case SET_HAPTIC_CODE:
      eeprom_update_byte((uint8_t*)0, I2C_Array[1]);  //write first code letter to EEPROM byte number 0
      eeprom_update_byte((uint8_t*)1, I2C_Array[2]);  //write second code letter to EEPROM byte number 1
      //return bytes written for confirmation
      I2C_Array[1] = eeprom_read_byte((uint8_t*)0);
      I2C_Array[2] = eeprom_read_byte((uint8_t*)1);
      if (I2C_Array[1] >= 65 && I2C_Array[1] <= 90 && I2C_Array[2] >=65 && I2C_Array[2] <= 90) {
        haptic_lock_flag = 1;
      } else {
        haptic_lock_flag = 0;
      }
      I2C_Array[0] = SUCCESS;
      break;

//////////// SAWYER I2C COMMANDS //////////////
#ifdef SAWYER  

  case PULSE_MODE:
      if (I2C_Array[1] == 1) {morse_echo = 0;morse_replay = 1;}
      if (I2C_Array[1] == 0) {morse_echo = 0;morse_replay = 0;}
      I2C_Array[0] = SUCCESS;
      break;

  
  //ADC READS
  case READ_CDS1:
    job_params[0] = CDS1_CHANNEL;
    Push_Pull_Job (PLAIN_ADC_JOB);
    break;
    
  case READ_CDS2:

    job_params[0] = CDS2_CHANNEL;
    Push_Pull_Job(PLAIN_ADC_JOB);
    break;
      
  //INPUT READS



    case READ_HALL: //read hall
      if bit_is_set (PINB, PB0) {I2C_Array[1] = 1;} //hall 1
        else I2C_Array[1] = 0;
      if bit_is_set (PINB, PB2) {I2C_Array[2] = 1;} //hall 2 on miso line
        else I2C_Array[2] = 0;
      I2C_Array[0] = SUCCESS;
      break;
      
    case READ_GYROHALL: 
      if bit_is_set (PINC, PC2) {I2C_Array[1] = 1;} //HU
        else I2C_Array[1] = 0;
      if bit_is_set (PINC, PC4) {I2C_Array[2] = 1;} //HV
        else I2C_Array[2] = 0;
      if bit_is_set (PINC, PC5) {I2C_Array[3] = 1;} //HW
        else I2C_Array[3] = 0;
      I2C_Array[0] = SUCCESS;
      break;

    case READ_GYRO_RPM:
      cli();
      I2C_Array[1] = RPM & 0x00ff; //low byte
      I2C_Array[2] = RPM >> 8;     //high byte
      sei();
      I2C_Array[0] = SUCCESS;
      break;

    case BLDCTEST:
      // U-V
      if (I2C_Array[1] == 1) {   
         UHI;UON;VLO;VON;WLO;WOFF;
         _delay_ms(2000);
      // -U-V
      } else if (I2C_Array[1] == 11) {
         ULO;UON;VHI;VON;WLO;WOFF;
         _delay_ms(2000);
      // U-W
      } else if (I2C_Array[1] == 2) {   
         UHI;UON;VLO;VOFF;WLO;WON;
         _delay_ms(2000);
      // -U-W
      } else if (I2C_Array[1] == 22) {
         ULO;UON;VLO;VOFF;WHI;WON;
         _delay_ms(2000);
      // V-W
      } else if (I2C_Array[1] == 3) {   
         ULO;UOFF;VHI;VON;WLO;WON;
         _delay_ms(2000);
      // -V-W
      } else if (I2C_Array[1] == 33) {
         ULO;UOFF;VLO;VON;WHI;WON;
         _delay_ms(2000);
      }
      UOFF;VOFF;WOFF;
      I2C_Array[0] = SUCCESS;
      break;

    case H_BRIDGE_RAMPUP:
    
      job_params[0] = I2C_Array[1];
      job_params[1] = I2C_Array[2];
      job_params[2] = I2C_Array[3];
      job_params[3] = I2C_Array[4];
      job_params[4] = I2C_Array[5];
      Push_Pull_Job (H_BRIDGE_JOB);
      break;

    case H_BRIDGE_PARK:
    
      Push_Pull_Job (H_BRIDGE_PARK_JOB);
      break;

    case H_BRIDGE_ORBIT:

      job_params[0] = I2C_Array[1];
      job_params[1] = I2C_Array[2];    
      Push_Pull_Job (H_BRIDGE_ORBIT_JOB);
      break;

    case ROTATE:
    
      job_params[0] = I2C_Array[1];  //Direction
      job_params[1] = I2C_Array[2];  //Position
      Push_Pull_Job (ROTATE_JOB);
      break;
   
  
  case PWM_CLEAR:

    Pwm_Clear();
    I2C_Array[0] = SUCCESS;
    break;

  case H_BRIDGE_PULSE:
    //hopefully it is short enough to work without a job
    H_Bridge_Pulse();
    I2C_Array[0] = SUCCESS;
    break;

/// H-BRIDGE CONTROL 
  case H_BRIDGE_RIGHT_ON:
      PORTB |= (1<<PB3);
      I2C_Array[0] = SUCCESS; 
      break;
 
  case H_BRIDGE_RIGHT_OFF:
      PORTB &= ~(1<<PB3);
      I2C_Array[0] = SUCCESS;
      break;
      
  case H_BRIDGE_LEFT_ON:
      PORTA |= (1<<PA6);
      I2C_Array[0] = SUCCESS;
      break;
      
  case H_BRIDGE_LEFT_OFF:
      PORTA &= ~(1<<PA6);
      I2C_Array[0] = SUCCESS;
      break;
      
  case GYROTEST:
      Push_Pull_Job(GYROTEST_JOB);
      job_params[0] = I2C_Array[1];
      job_params[1] = I2C_Array[2];
      job_params[2] = I2C_Array[3];
      job_params[3] = I2C_Array[4];
      job_params[4] = I2C_Array[5];
      break;
      
  case GYROSTOP:
      GIMSK &= ~(1<<PCIE2);  //disable gyro hall pin interrupts
      BLDC_state = 255;
      UOFF;VOFF;WOFF;
      break;

  case SET_GYRORPM:
      Set_RPM = I2C_Array[1] * 40;
      if (Set_RPM >= 1360) {BLDC_mode = BLDC_PWM_MODE;TIMSK |= (1 << OCIE0A);} //if rpm byte is 34 or greater, make sure ctc interrupt is working
      else if (Set_RPM >= 1520) {BLDC_mode = BLDC_FAST_MODE;TIMSK &= ~(1 << OCIE0A);} //if rpm byte is 38 or greater, disables ctc interrupt to speed pin change interrupts
      else {BLDC_mode = BLDC_LOCK_MODE;OCR0A = 175;TIMSK |= (1 << OCIE0A);}
      I2C_Array[0] = SUCCESS;
      break;


#endif
  
  

#ifdef HUCKLEBERRY


 //////////// HUCKLEBERRY I2C COMMANDS /////////////// 

    case PULSE_MODE:
      if (I2C_Array[1] == 0) {morse_echo = 1;morse_replay = 1;}  
      if (I2C_Array[1] == 1) {morse_echo = 1;morse_replay = 0;}  
      I2C_Array[0] = SUCCESS;
      break;

  
    //ADC READS
    case BATT1_VOLTAGE:
      job_params[0] = BATT1_CHANNEL;
      Push_Pull_Job(VOLTAGE_ADC_JOB);
     // ADCSRA |= 1 << ADIE;
   
     // set_sleep_mode (SLEEP_MODE_ADC);
     // sleep_enable();
     // sei();
     // sleep_cpu();
     //sleep_disable();
     // set_sleep_mode(SLEEP_MODE_PWR_DOWN);
     break;
  
    case BATT2_VOLTAGE:
       job_params[0] = BATT2_CHANNEL;
       Push_Pull_Job(VOLTAGE_ADC_JOB);
       break;
       
    case SOLAR_CHARGE_VOLTAGE:
       job_params[0] = SOLAR_CHARGE_CHANNEL;
       Push_Pull_Job(VOLTAGE_ADC_JOB);
       break;
           
   //OUTPUT

   //need to add code to move to 5.2 if on 5.1 or it will lose power in series!
   case SERIES_MODE:
      I2C_Array[0] = Switch_Battery_Relay(SERIES_MODE);
      break;
      
   case PARALLEL_MODE:
      I2C_Array[0] = Switch_Battery_Relay(PARALLEL_MODE);
      break;
      
   case CHARGER_RELAY:
      I2C_Array[0] = Switch_Charger_Relay(I2C_Array[1]);
      break;
   
   case SOLAR_CHARGE_SWITCH: 
      I2C_Array[0] = Switch_Solar_Charge(I2C_Array[1]);
      break;
  
   case BOOST_51: 
      I2C_Array[0] = Switch_Boost_51(I2C_Array[1]);
      break;
      
  case BOOST_52:
      I2C_Array[0] = Switch_Boost_52(I2C_Array[1]);
      break;
      
 case BOOST_71:
      I2C_Array[0] = Switch_Boost_71(I2C_Array[1]);
      break;
      
  case BOOST_72:
      I2C_Array[0] = Switch_Boost_72(I2C_Array[1]);
      break;
      
  case QI_CHARGER:
      I2C_Array[0] = Switch_Qi_Charger(I2C_Array[1]);
      break;
  
  case ESP_LDO:
      I2C_Array[0] = Switch_Esp_Ldo(I2C_Array[1]);
      break;

  case MISO_LED:
      if (I2C_Array[1] == ON) {
         //turn LED spotlight on (as long as MISO line is free and Pi and Esp turned LED on)
         DDRB &= ~(1<<PB2);
      } else { //turn LED spotlight off
         DDRB |= (1<<PB2);PORTB &= ~(1<<PB2);
      }
      I2C_Array[0] = SUCCESS;
      break;
      
  case SOLAR_CHARGE_MODE:
      //if OFF command, quickly turn off relay and charger
      if (I2C_Array[1] == OFF) {
         Switch_Charger_Relay (BATT2);
         Switch_Solar_Charge (OFF);  
      }
      Solar_Charge_Mode = I2C_Array[1];
      I2C_Array[0] = SUCCESS;
   
#endif
  }
  
}

///////////////// SHARED FUNCTIONS ////////////////

volatile void Process_Haptic_Command (uint8_t haptic_command, uint8_t confirmation) {

//confirmation is 26 for F1, 27 for F2, but not currently used
#ifdef HUCKLEBERRY

  //X - Disable all power to the craft (.   ....)
  if (actual_morse_letter == 23) {
      _delay_ms(1000); //first wait 1 second for Sawyer to disable Gyro
      Switch_Qi_Charger(OFF);
      Switch_Battery_Relay(PARALLEL_MODE);
      Switch_Charger_Relay (BATT2);
      Switch_Solar_Charge (OFF);
      Switch_Esp_Ldo(OFF);
      Switch_Boost_71(OFF);
      Switch_Boost_72(OFF);
      Switch_Boost_51(OFF);
      Switch_Boost_52(OFF);

  //P - Power up 5.2 boost_converter and ESP8266 (.--.     ..   .   .)
  } else if (actual_morse_letter == 15) {
      Switch_Boost_52(OFF);
      Switch_Esp_Ldo(ON);
      actual_morse_letter = 61;  //rmorse can't read it now
  
  //L - Turn on ESP8266 LDO                      (.-..     ..   ..)
  } else if (actual_morse_letter == 11) {
      Switch_Esp_Ldo(ON);
      actual_morse_letter = 61;  //rmorse can't read it now
  
  //B - Turn on 5.2 boost converter              (-...     .   ...)
  } else if (actual_morse_letter == 1) {
    Switch_Boost_52(ON);
    actual_morse_letter = 61;  //rmorse can't read it now
  
  //Q - Turn on Qi charger                       (--.-     .   .   ...)
  } else if (actual_morse_letter == 16) {
    Switch_Qi_Charger(ON);
    actual_morse_letter = 61;  //rmorse can't read it now
  }   
  


#endif
#ifdef SAWYER
  //X - Disable all power to the craft (.   ....)
  if (actual_morse_letter == 23) {
      //Make sure Gyro is shut down since Huckleberry is shutting off all power
      //This protects Sawyer from being damaged by L6234 I/O pins
      GIMSK &= ~(1<<PCIE2);  //disable gyro hall pin interrupts
      BLDC_state = 255;
      UOFF;VOFF;WOFF;
      actual_morse_letter = 61;  //rmorse can't read it now
  }

#endif



}

#ifdef SAWYER
///////////////// SAWYER FUNCTIONS /////////////////

volatile void GyroTest (uint8_t myrpm) {
// make sure battery is in series mode or damage to Sawyer will result!

GIMSK |= (1<<PCIE2); //Enable gyro hall pin interrupts
BLDC_delay = 200;//larger values slow down gyro by forcing magnetic hold
Set_RPM = myrpm * 40; 
if (Set_RPM >= 1360) {BLDC_mode = BLDC_PWM_MODE;TIMSK |= (1 << OCIE0A);} // if rpm byte is 34 or greater
else if (Set_RPM >= 1520) {BLDC_mode = BLDC_FAST_MODE;TIMSK &= ~(1 << OCIE0A);} //if rpm byte is 38 or greater
else {BLDC_mode = BLDC_LOCK_MODE;OCR0A = 175;TIMSK |= (1 << OCIE0A);}
BLDC_state = 0;  //turn gyro on

}

/////////////
volatile void Pwm_Clear (void) {
   //disable pwm and return ports back to normal, off    
    TCCR1A &= ~(1<<COM1A1);
    TCCR1A &= ~(1<<COM1A0);
    TCCR1A &= ~(1<<COM1B1); 
    TCCR1A &= ~(1<<COM1B0);

    //OC1A_H-BRIDGE_RIGHT
    DDRB |= (1<<PB3);
    //set to 0, off
    PORTB &= ~(1<<PB3);
    //OC1A_H-BRIDGE_LEFT
    DDRA |= (1<<PA6);
    //set to 0, off  
    PORTA &= ~(1<<PA6);


}

////////////////////
volatile void H_Bridge_Up (void) {
  // H_BRIDGE right on
  PORTB |= (1<<PB3);
  // H_BRIDGE left off
  PORTA &= ~(1<<PA6);
}
//////////////////////


volatile void H_Bridge_Down (void) {
  // H_BRIDGE right off
  PORTB &= ~(1<<PB3);
  // H_BRIDGE left on
  PORTA |= (1<<PA6);
}

////////////////////
volatile void H_Bridge_Pulse (void) {
  //3 back and forth full current cycles
  H_Bridge_Up();
  _delay_ms(30);
  H_Bridge_Down();
  _delay_ms(30);
  H_Bridge_Up();
  _delay_ms(30);
  H_Bridge_Down();
  _delay_ms(30);
  Pwm_Clear();
}


////////////////////// H-BRIDGE //////////////////////
volatile uint8_t H_Bridge (uint8_t pwm_type, uint8_t acceleration, uint8_t direction, uint8_t speed, uint8_t sustain_time, uint8_t position) {



    //now set 16-bit timer accordingly

    //16 bit pwm phase and frequency correct mode 8
    TCCR1B |= (1<<WGM13);
    TCCR1B &= ~(1<<WGM12);
    TCCR1A &= ~(1<<WGM11);
    TCCR1A &= ~(1<<WGM10);
	 
    //set to 001 - NO prescaler (to allow max duty cycle resolution in frequencies I need)
    TCCR1B &= ~(1<<CS12); TCCR1B &= ~(1<<CS11); TCCR1B |= (1<<CS10);
  
    //Equation:  8000000 / (2 x clk scaler x ICR1 value) = pwm frequency
    //lowering the value of ICR1 (which is called TOP) increases max frequency
    
    //set frequency
    ICR1 = 0x0400; // 1024 decimal - 3906.25 hz

    //if you increase top it has to be higher than all compare registers or it will never match
    //max duty cycle is 0x0400
    //half of 400 is 200 - 512 decimal

    //ICR1 = 0x0300; //decimal 768, 5208.33333 Hz
    //max duty cycle is 0x0300
    //half of 300 is 180 - 384 decimal
   
   //top cannot exceed ICR1, which is currently set to 1024 above, this determines max speed
   uint16_t top = speed * 4; //let user determine max speed in 8-bits, by being a chunk of top (255 x 4 = 1024 max)
   uint16_t start; 
   if (pwm_type == LOCK_ANTIPHASE) {
      start = ICR1 / 2;
   } else {
      start = 0;
   }

    //now turn on pins
 
    if (pwm_type == LOCK_ANTIPHASE) {
       //set not inverted pwm for A 1 0
       TCCR1A |= (1<<COM1A1);TCCR1A &= ~(1<<COM1A0);

       //set inverted pwm for B 1 1
       TCCR1A |= (1<<COM1B1);TCCR1A |= (1<<COM1B0);

    } else {
       //sign magnitude drive
       //disconnect OC1B from port
       TCCR1A &= ~(1<<COM1B1); TCCR1A &= ~(1<<COM1B0);

       if (direction == UP) {
          //set not inverted pwm for A 1 0
          TCCR1A |= (1<<COM1A1);TCCR1A &= ~(1<<COM1A0);

          //set OC1B pin to 0 if sign magnitude direction is up
          PORTA &= ~(1<<PA6);

       } else { //if DOWN
          //set inverted pwm for A 1 0
          TCCR1A |= (1<<COM1A1);TCCR1A |= (1<<COM1A0);

          //set OC1B pin to 1 if sign magnitude direction is down
          PORTA |= (1<<PA6);
       }
     
    }
    
  

   if (acceleration > 0 && acceleration < 128) {

     //ramp speed slow to fast
     //break out if position is met (don't continue to speed up) and set top speed to where it was last set

     if (pwm_type == LOCK_ANTIPHASE && direction == DOWN) {

     for( uint16_t mcount = start; mcount > (ICR1 - top) / 2; mcount = mcount - 1 ) {
       //delay between speed changes to slowly ramp
       if (Phase_of_Day == position) {Pwm_Clear(); return SUCCESS;}
       for( uint8_t del = 0; del < acceleration * 2; del = del + 1 ) {
         if (Phase_of_Day == position) {Pwm_Clear(); return SUCCESS;}
         _delay_ms(1);
       }
       OCR1A = mcount;
       //if locked antiphase drive, add inverted
       if (pwm_type == LOCK_ANTIPHASE) {
       OCR1B = mcount;
       }
     } 

     } else { // normal
     if (pwm_type == LOCK_ANTIPHASE) {top = (top + ICR1) / 2;}
     for( uint16_t mcount = start; mcount < top; mcount = mcount + 1 ) {
       //delay between speed changes to slowly ramp
       if (Phase_of_Day == position) {Pwm_Clear(); return SUCCESS;}
       for( uint8_t del = 0; del < acceleration * 2; del = del + 1 ) {
         if (Phase_of_Day == position) {Pwm_Clear(); return SUCCESS;}
         _delay_ms(1);
       }
       OCR1A = mcount;
       //if locked antiphase drive, add inverted
       if (pwm_type == LOCK_ANTIPHASE) {
       OCR1B = mcount;
       } else {mcount = mcount + 1;} // reduce resolution to match lock-antiphase
     } 

     } 

      //sustain speed for number of 10ths of a second (0-255 up to 25.5 seconds), break out if position is met
      for( uint8_t del = 0; del < sustain_time; del = del + 1 ) {
        if (Phase_of_Day == position) {Pwm_Clear();return SUCCESS;}
        _delay_ms(100);
      }
     

   } else if (acceleration >= 128) {

       OCR1A = top;
       //if locked antiphase drive, add inverted
       if (pwm_type == LOCK_ANTIPHASE) {
         if (direction == DOWN) {
           OCR1A = (ICR1 - top)/ 2;
           OCR1B = (ICR1 - top)/ 2;
         } else {
           OCR1A = (top + ICR1) / 2;
           OCR1B = (top + ICR1) / 2;
         }
       }

      //sustain speed for number of 10ths of a second (0-255 up to 25.5 seconds), break out if position is met
      for( uint8_t del = 0; del < sustain_time; del = del + 1 ) {
        if (Phase_of_Day == position) {Pwm_Clear();return SUCCESS;}
        _delay_ms(100);
      }
     
    if (pwm_type == LOCK_ANTIPHASE && direction == DOWN) {

     //ramp speed fast to slow
     for( uint16_t mcount = (ICR1 - top)/ 2; mcount < start; mcount = mcount + 1 ) {
       //delay between speed changes to slowly ramp
       if (Phase_of_Day == position) {Pwm_Clear();return SUCCESS;} 
 
       for( uint8_t del = 0; del < 255 - acceleration; del = del + 1 ) {
       _delay_ms(1);
       }
       OCR1A = mcount;
       //if locked antiphase drive, add inverted
       if (pwm_type == LOCK_ANTIPHASE) {
       OCR1B = mcount;
       }
     }
   }  else {
      if (pwm_type == LOCK_ANTIPHASE) {top = (top + ICR1) / 2;}
     //ramp speed fast to slow
     for( uint16_t mcount = top; mcount > start; mcount = mcount - 1 ) {
       //delay between speed changes to slowly ramp
       if (Phase_of_Day == position) {Pwm_Clear();return SUCCESS;} 
 
       for( uint8_t del = 0; del < 255 - acceleration; del = del + 1 ) {
       _delay_ms(1);
       }
       OCR1A = mcount;
       //if locked antiphase drive, add inverted
       if (pwm_type == LOCK_ANTIPHASE) {
       OCR1B = mcount;
       } else {mcount = mcount - 1;} //reduce resolution to match lock-antiphase
     }
   }
  } else { //acceleration is zero

       OCR1A = top;
       //if locked antiphase drive, add inverted
       if (pwm_type == LOCK_ANTIPHASE) {
         if (direction == DOWN) {
           OCR1A = (ICR1 - top)/ 2;
           OCR1B = (ICR1 - top)/ 2;
         } else {
           OCR1A = (top + ICR1) / 2;
           OCR1B = (top + ICR1) / 2;
         }
       }

      //sustain speed for number of 10ths of a second (0-255 up to 25.5 seconds), break out if position is met
      for( uint8_t del = 0; del < sustain_time; del = del + 1 ) {
        if (Phase_of_Day == position) {Pwm_Clear();return SUCCESS;}
        _delay_ms(100);
      }
}





   Pwm_Clear();



 return SUCCESS;

}




#endif
#ifdef HUCKLEBERRY
//////////////// HUCKLEBERRY FUNCTIONS //////////////

volatile uint8_t Switch_Battery_Relay (uint8_t mode) {
  switch (mode){

    case SERIES_MODE:
      PORTC |= (1<<PC2);      //turn on
      _delay_ms(333);         //pulse for 1/3 second
      PORTC &= ~(1<<PC2);     //turn off coil power
      Batt_Mode = SERIES_MODE;
      break;
      
    case PARALLEL_MODE:
      PORTB |= (1<<PB0);      //turn on
      _delay_ms(333);         //pulse for 1/3 second
      PORTB &= ~(1<<PB0);     //turn off coil power
      Batt_Mode = PARALLEL_MODE;
  }
  return SUCCESS;
}


      
volatile uint8_t Switch_Charger_Relay (uint8_t mode) {
  switch (mode) {
    
    case BATT1:
        PORTA |= (1<<PA1);
        Charger_Relay = ON;
        break;

    case BATT2:
        PORTA &= ~(1<<PA1);
        Charger_Relay = OFF;
  }
  return SUCCESS;
}


volatile uint8_t Switch_Solar_Charge (uint8_t mode) {
  switch (mode) {

    case ON:
      PORTA |= (1<<PA7);
      Solar_Charge_Switch = ON;
      break;

    case OFF:
      PORTA &= ~(1<<PA7);
      Solar_Charge_Switch = OFF;
  }   
  I2C_Array[0] = SUCCESS;
}


volatile uint8_t Switch_Boost_51 (uint8_t mode) {
  switch (mode) {
  
    case ON:
      PORTC |= (1<<PC4);
      Boost_51 = ON;
      break;

    case OFF:
      PORTC &= ~(1<<PC4);
      Boost_51 = OFF;
  }
  I2C_Array[0] = SUCCESS;
}
      

volatile uint8_t Switch_Boost_52 (uint8_t mode) {
  switch (mode) {

    case ON:
      PORTA |= (1<<PA4);
      Boost_52 = ON;
      break;

    case OFF:
      PORTA &= ~(1<<PA4);
      Boost_52 = OFF;
  }  
  I2C_Array[0] = SUCCESS;
}

volatile uint8_t Switch_Boost_71 (uint8_t mode) {
  switch (mode) {
      
    case ON:
      PORTA |= (1<<PA0);
      Boost_71 = ON;
      break;

    case OFF:
      PORTA &= ~(1<<PA0);
      Boost_71 = OFF; 
  }
  I2C_Array[0] = SUCCESS;
}
      

volatile uint8_t Switch_Boost_72 (uint8_t mode) {
  switch (mode) {

    case ON:
      PORTA |= (1<<PA2); 
      Boost_72 = ON;
      break;

    case OFF:
      PORTA &= ~(1<<PA2); 
      Boost_72 = OFF;
  }
  I2C_Array[0] = SUCCESS;
}
      

volatile uint8_t Switch_Qi_Charger (uint8_t mode) {
  switch (mode) {

    case ON:
      PORTC |= (1<<PC5);
      Qi_Charger = ON;
      break;

    case OFF:
      PORTC &= ~(1<<PC5);
      Qi_Charger = OFF;
  }
  I2C_Array[0] = SUCCESS;
}

  
volatile uint8_t Switch_Esp_Ldo (uint8_t mode) {
  switch (mode) {

    case ON:
      PORTA |= (1<<PA6);
      Esp_Ldo = ON;
      break;
    case OFF:
      PORTA &= ~(1<<PA6);
      Esp_Ldo = OFF;
  }
  I2C_Array[0] = SUCCESS;
}

volatile void Process_Solar_Charge_Mode(void) {
           //be sure to add commands to shut off stuff when solar charge mode is off--fix
           //Don't activate if series mode is enabled
           //Sawyer will have to carefully turn off gyro and series first
           //pi should be in charge of handling higher functions, too

           //if batt draining too fast, both batts will gradually lower from max voltage
           //otherwise, they will gradually raise to max voltage
           //add check to stop charging if top voltages reached

           uint16_t batt1_voltage;
           uint16_t batt2_voltage;
           
           //first, read current batt voltages
           //on huckleberry, Voltage_ADC_Read function returns integer, decimal, medium precision voltage
           batt1_voltage = Voltage_ADC_Read(BATT1_CHANNEL);  //this automatically stores batt voltage in I2C_Array
           batt2_voltage = Voltage_ADC_Read(BATT2_CHANNEL);  //this automatically stores batt voltage in I2C_Array

           //.1 volt is 1000, switch banks if difference is more than .2 v
           //integer shouldn't overflow unless batt voltage reaches 63536 (+2000), 6.3 volts, which is impossible

           //cycling at around 40% of max capacity is best for longevity
           //keeping top at around 3.9 volts increases longevity (but max capacity is lowered for night)
           //so late in the day, the batteries should be topped off before nightfall

           if (batt1_voltage > (batt2_voltage + 2000)) {
              
              //make sure solar charge switch is off
              if (Solar_Charge_Switch == ON) Switch_Solar_Charge (OFF);

              //if on batt power, switch to batt1
              if (Boost_52 == ON) {
                Switch_Boost_51 (ON);
                _delay_ms(333);
                Switch_Boost_52 (OFF);
              }

              if (Boost_72 == ON) {
                Switch_Boost_71 (ON);
                _delay_ms(333);
                Switch_Boost_72 (OFF);
              }

              //now charge batt2
              Switch_Charger_Relay(BATT2);
              Switch_Solar_Charge(ON);  
           }
           else if (batt2_voltage > (batt1_voltage + 2000)) {
             
             //make sure solar charge switch is off
              if (Solar_Charge_Switch == ON) Switch_Solar_Charge(OFF);

               //if on batt power, switch to batt2
              if (Boost_51 == ON) {
                Switch_Boost_52 (ON);
                _delay_ms(333);
                Switch_Boost_51 (OFF);
              }

              if (Boost_71 == ON) {
                Switch_Boost_72 (ON);
                _delay_ms(333);
                Switch_Boost_71 (OFF);
              }

              //now charge batt1
              Switch_Charger_Relay (BATT1);
              Switch_Solar_Charge (ON);  

           }
           //if batteries close in value, but sun in still on, make sure charger still on (experimental)
           //else if (Solar_Power == ON && Solar_Charge_Switch == OFF) Switch_Solar_Charge(ON);

}

///////////////////// VOLTAGE ADC READ FUNCTION ///////////////
volatile uint16_t Voltage_ADC_Read (uint8_t channel) { //converts ADC to decimal voltage value in 2-byte integer form

          float vcc;
          float voltage;
          uint16_t decimal_voltage;
   
         
          //first compare 1.1 ref voltage against Vcc to compute actual Vcc voltage
          vcc = ((1.1 - BANDGAP_CORRECTION) * 1024 / (Plain_ADC_Read(BANDGAP_CHANNEL) - 1));
          //make sure to perform "minus one USB" on reference as in datasheet pg. 195
          //1.1 reference has tolerance of .1 volt.  I had to adjust for such error on huckleberry.
   
          voltage = (vcc * (Plain_ADC_Read(channel) - 1))/1024.0;
          //move decimal to keep precision of 4 decimal points, works well since max volts wont exceed 6.5536
          decimal_voltage = voltage * 10000;

          //If the voltage happens to be one of the batteries, might as well keep global vars updated
          if (channel == BATT1_CHANNEL) {
            Batt1_Voltage_High = decimal_voltage >> 8; 
            Batt1_Voltage_Low = decimal_voltage & 0x00ff;
          } else if (channel == BATT2_CHANNEL) {
            Batt2_Voltage_High = decimal_voltage >> 8; 
            Batt2_Voltage_Low = decimal_voltage & 0x00ff;
          }


          return decimal_voltage;
}


#endif


