// Chad Losey
// 12/10/2023
// Final Project

/*
The display show the battery percentage, whether or not the car
is in sunlight from a window (about 2000 lux), and the distance
of the nearest object in front of the car. The motors go forward
with the pink arrowed motor representing the left wheels and the
blue arrowed motor representing the right wheels. When the light
level is sufficient, the motors stop. If the distance drops below
12 cm, the car backs up, turns right and displays the distance,
turns left and displays the distance, and then faces the direction
with the longer distance. You can change the distance during the
pauses in that process to simulate different readings in each direction
There will be 4 pauses if the right distance (first measurement) was
greater than the left distance (second measurement) and 3 pauses
otherwise, as the car turns an extra time if right is greater.
*/

// lcd setup
#include <LiquidCrystal_I2C.h>
LiquidCrystal_I2C lcd(0x27, 20, 4);

// ddrx registers
unsigned char *portDDRB;
unsigned char *portDDRD;

// portx/pinx registers
unsigned char *portB;
unsigned char *pinB;
unsigned char *portD;

// timer 0 registers
unsigned char *TCCR0A_ESP;
unsigned char *TCCR0B_ESP;
unsigned char *TCNT0_ESP;
unsigned char *OCR0A_ESP;
unsigned char *TIMSK0_ESP;

// timer 1 registers
unsigned char *TCCR1A_ESP;
unsigned char *TCCR1B_ESP;
unsigned char *TCNT1H_ESP;
unsigned char *TCNT1L_ESP;
unsigned char *OCR1AH_ESP;
unsigned char *OCR1AL_ESP;
unsigned char *TIMSK1_ESP;

// adc registers
unsigned char *ADMUX_ESP;
unsigned char *ADCSRA_ESP;
unsigned char *ADCL_ESP;
unsigned char *ADCH_ESP;

// global interupts register
unsigned char *SREG_ESP;


void setup() {
  //////////////////////////////////////////////////
  // initialize the serial port:
  Serial.begin(9600);
  //////////////////////////////////////////////////

  // initialize lcd
  lcd.init();
  lcd.backlight();

  // print info titles
  lcd.print("Battery: ");
  lcd.setCursor(0, 1);
  lcd.print("Light: ");
  lcd.setCursor(0, 2);
  lcd.print("Distance: ");

  // DDRX registers
  portDDRB = (unsigned char*) 0x24;
  portDDRD = (unsigned char*) 0x2A;

  *portDDRB = 0b00010011;
  *portDDRD = 0b11111111;

  // portx/pinx registers
  portB = (unsigned char*) 0x25;
  pinB = (unsigned char *) 0x23;
  portD = (unsigned char*) 0x2B;

  *portB &= ~(00010011);
  *portD &= ~(11111111);

  // timer 0 registers
  TCCR0A_ESP = (unsigned char *) 0x44;
  TCCR0B_ESP = (unsigned char *) 0x45;
  TCNT0_ESP = (unsigned char *) 0x46;
  OCR0A_ESP = (unsigned char *) 0x47;
  TIMSK0_ESP = (unsigned char *) 0x6E;

  *TCCR0A_ESP = 0b00000010; // ctc, normal port operation
  *TCCR0B_ESP = 0b00000010; // ctc, prescalar 8
  *TIMSK0_ESP = 0b00000000; // ctc with ocr0a disabled
  // top, 116 u-sec interval for echo time to cm
  *OCR0A_ESP = 0b01110100;

  // timer 1 registers
  TCCR1A_ESP = (unsigned char *) 0x80;
  TCCR1B_ESP = (unsigned char *) 0x81;
  TCNT1H_ESP = (unsigned char *) 0x85;
  TCNT1L_ESP = (unsigned char *) 0x84;
  OCR1AH_ESP = (unsigned char *) 0x89;
  OCR1AL_ESP = (unsigned char *) 0x88;
  TIMSK1_ESP = (unsigned char *) 0x6F;

  *TCCR1A_ESP = 0b00000000; // ctc, normal port operation
  *TCCR1B_ESP = 0b00001010; // ctc, prescalar 8
  *OCR1AH_ESP = 0b00000001; // top, each number is a
  *OCR1AL_ESP = 0b00001010; // 1 microsecond interval
  *TIMSK1_ESP = 0b00000000; // ctc with ocr1a disabled


  // adc registers
  ADMUX_ESP = (unsigned char*) 0x7c;
  ADCSRA_ESP = (unsigned char*) 0x7a;
  ADCL_ESP = (unsigned char*) 0x78;
  ADCH_ESP = (unsigned char*) 0x79;

  *ADCSRA_ESP |= 0b10000000; // enable the ADC

  // enable global interupts
  SREG_ESP = (unsigned char*) 0x5F;
  *SREG_ESP |= 0b10000000; // enable interupts
}


void loop() {
  bool brightEnough;
  // 111 photoresistor adc is about sunlight through a window
  if (adcReadLight() < 112)
    brightEnough = true;
  else
    brightEnough = false;

  uint8_t batteryPercentage = adcReadBattery();
  uint16_t cmDistance = readDistance();
  // display various readings
  displayInfo(batteryPercentage, brightEnough, cmDistance);

  // when not in sunlight, determine route and move
  if (!brightEnough) {
    if (cmDistance < 12) {
      // turns car to avoid obstacles
      determineDirection(batteryPercentage, brightEnough);
    }
      
    // move forwards
    for (uint8_t i = 0; i < 4; ++i) {
      rightForwards();
      leftForwards();
    }
  }
}


// face the direction with the further the distance
void determineDirection (uint8_t batteryPercentage, bool brightEnough) {
  // move backwards slightly
  for (uint8_t i = 0; i < 20; ++i) {
    rightBackwards();
    leftBackwards();
  }
  for (uint16_t i = 0; i < 15000; ++i) {
    myDelay(65000);
  }

  // read distance to the right
  for (uint8_t i = 0; i < 20; ++i) {
    rightBackwards();
    leftForwards();
  }
  uint16_t right = readDistance();
  displayInfo(batteryPercentage, brightEnough, right);
  for (uint16_t i = 0; i < 15000; ++i) {
    myDelay(65000);
  }

  // read the distance to the left
  for (uint8_t i = 0; i < 40; ++i) {
    leftBackwards();
    rightForwards();
  }
  uint16_t left = readDistance();
  displayInfo(batteryPercentage, brightEnough, left);
  for (uint16_t i = 0; i < 15000; ++i) {
    myDelay(65000);
  }

  // face the further distance
  if (right > left) {
    for (uint8_t i = 0; i < 40; ++i) {
      rightBackwards();
      leftForwards();
    }
    displayInfo(batteryPercentage, brightEnough, right);
    for (uint16_t i = 0; i < 15000; ++i) {
      myDelay(65000);
    }
  }
}


// turn right wheel backwards about 3.5 degrees
void rightBackwards () {
  // change pins in order required by stepper motor
  *portD |= 0b00100000;
  myDelay(10000);
  *portD &= ~(0b00001000);

  myDelay(10000);
  *portD |= 0b00001000;
  myDelay(10000);
  *portD &= ~(0b00010000);

  myDelay(10000);
  *portD |= 0b00010000;
  myDelay(10000);
  *portD &= ~(0b00000100);
    
  myDelay(10000);
  *portD |= 0b00000100;
  myDelay(10000);
  *portD &= ~(0b00100000);
  myDelay(10000);
}


// turn right wheel backwards about 3.5 degrees
void rightForwards () {
  // change pins in order required by stepper motor
  myDelay(10000);
  *portD &= ~(0b00000100);
  *portD |= 0b00010000;

  myDelay(10000);
  *portD &= ~(0b00010000);
  *portD |= 0b00001000;
    
  myDelay(10000);
  *portD &= ~(0b00001000);
  *portD |= 0b00100000;
  
  myDelay(10000);
  *portD &= ~(0b00100000);
  *portD |= 0b00000100;
}


// turn left wheel backwards about 3.5 degrees
void leftBackwards () {
  // change pins in order required by stepper motor
  myDelay(10000);
  *portD &= ~(0b10000000);
  *portB |= 0b00000010;

  myDelay(10000);
  *portB &= ~(0b00000001);
  *portD |= 0b10000000;

  myDelay(10000);
  *portD &= ~(0b01000000);
  *portB |= 0b00000001;
  
  myDelay(10000);
  *portB &= ~(0b00000010);
  *portD |= 0b01000000;
}


// turn left wheel foward about 3.5 degrees
void leftForwards () {
  // change pins in order required by stepper motor
  myDelay(10000);
  *portD &= ~(0b01000000);
  *portB |= 0b00000001;

  myDelay(10000);
  *portB &= ~(0b00000001);
  *portD |= 0b10000000;

  myDelay(10000);
  *portD &= ~(0b10000000);
  *portB |= 0b00000010;

  myDelay(10000);
  *portB &= ~(0b00000010);
  *portD |= 0b01000000;
}


// delays by the input amount of microseconds
bool timerDone;
void myDelay (uint16_t microSeconds) {
  // one ocr value is one microsecond
  uint8_t *OCR1AH_ESP = ((microSeconds >> 8) & 0b11111111);
  uint8_t *OCR1AL_ESP = (microSeconds & 0b11111111);

  timerDone = false;
  *TCNT1H_ESP = 0b00000000;
  *TCNT1L_ESP = 0b00000000;
  *TIMSK1_ESP |= 0x02;
  
  // wait for timer to match
  // you need to put empty serial prints in
  // some while loops to fix a wokwi bug
  while (!timerDone) {Serial.print("");}
  *TIMSK1_ESP &= ~(0x02);
}


// specified delay is over
ISR (TIMER1_COMPA_vect) {
  timerDone = true;
}

// returns the number of cm to objects sensed
uint16_t cmCount;
uint16_t readEcho () {
  cmCount = 0;
  bool echoDone = false;

  // wait for echo to start
  // you need to put empty serial prints in
  // some while loops to fix a wokwi bug
  while ((*pinB & 0x08) != 0x08) {Serial.print("");}

  *TCNT0_ESP = 0b00000000;
  *TIMSK0_ESP |= 0x02;

  // wait for echo to end
  while (!echoDone) {
    Serial.print("");
    if ((*pinB & 0x08) != 0x08) {
      echoDone = true;
    }
  }

  *TIMSK0_ESP &= (0x02);
  // The distance comes out offset by 5 cm
  return (cmCount - 5);
}


// each ctc match is equivelent to 1 cm
ISR (TIMER0_COMPA_vect) {
  ++cmCount;
}

// reads distance with ultrasonic sensor
uint16_t readDistance () {
  // send trigger
  *portB &= ~(0x10); // trigger off
  myDelay(4);
  *portB |= 0x10; // trigger on
  myDelay(10);
  *portB &= ~(0x10); // trigger off
  
  // read echo
  return readEcho();
}


// displays various info
void displayInfo (uint8_t batteryLevel, bool brightEnough, uint16_t cm) {
  // battery info
  lcd.setCursor(9, 0);
  lcd.print(batteryLevel);
  lcd.print("%  ");

  // sunlight info
  lcd.setCursor(7, 1);
  if (brightEnough)
    lcd.print("In sunlight ");
  else
    lcd.print("Not sunlight");

  // distance info
  lcd.setCursor(10, 2);
  lcd.print(cm);
  lcd.print(" cm    ");
}


// reads the battery level
uint8_t adcReadBattery () {
  // uses avcc as ref and internal v as adc input
  *ADMUX_ESP = 0b01001110;

  // first result must be thrown away to get
  // an accurate reading
  uint16_t adcValue;
  for (uint8_t i = 0; i < 2; ++i) {
    // you must wait 1 ms to get an accurate reading
    myDelay(1000);
    // start conversion
    *ADCSRA_ESP |= 0b01000000;

    // get result
    unsigned char cl = *ADCL_ESP;
    unsigned char ch = *ADCH_ESP;
    adcValue = (ch << 8) | cl;
  }

  // use formula from data sheet
  float currentVolts = (1.1 * 1024) / adcValue;
  // min and max volts of the battery
  const float minVolts = 3.3;
  const float maxVolts = 5.0;

  // return current percentage of total battery
  return (uint8_t) (100 * (currentVolts
                    - minVolts) / (maxVolts - minVolts));
}


// reads the light level
uint16_t adcReadLight () {
  // use a0 as adc input
  *ADMUX_ESP = 0b00000000;

  // first result must be thrown away to get
  // an accurate reading
  uint16_t adcValue;
  for (uint8_t i = 0; i < 2; ++i) {
    // you must wait 1 ms to get an accurate reading
    myDelay(1000);
    // start conversion
    *ADCSRA_ESP |= 0b01000000;

    // get result
    unsigned char cl = *ADCL_ESP;
    unsigned char ch = *ADCH_ESP;
    adcValue = (ch << 8) | cl;
  }

  return adcValue;
}