#include <Adafruit_TiCoServo.h>
#include <PinChangeInterrupt.h>
#include <Adafruit_NeoPixel.h>
#include <LiquidCrystal_I2C.h>

/* 
Note: Arduino Mega 2560 pinout:
https://devboards.info/images/boards/arduino-mega2560-rev3/arduino-mega2560-rev3-pinout.webp

Pin change compatible pins: 
 10, 11, 12, 13, 50, 51, 52, 53, 
 A8 (62), A9 (63), A10 (64), A11 (65), A12 (66), A13 (67), A14 (68), A15 (69)
*/


/***** Pinout *****/
// Ultrasonic distance sensors
static uint8_t pin_dist_trig[] = {34, 36, 38, 40, 42};
static uint8_t pin_dist_echo[] = {A10, A11, A12, A13, A14};
static uint8_t pin_dist_echo_bit[] = {2, 3, 4, 5, 6, 7}; // A10 = PortK:2, A11 = PortK:3, etc.

// Odometry quadrature optical sensors
static uint8_t pin_odom_clk[2] = {A8, A9};
static uint8_t pin_odom_clk_bit[2] = {0, 1};  // A8 = PortK:0, A9 = PortK:1
static uint8_t pin_odom_dat[2] = {30, 32};
static uint8_t odom_last;

// Direction servo -- note: on some ETH shield there's a 3-pin connector "Out5"
static uint8_t pin_servo = 46;

// Propulsion motor / H-bridge PWMs
static uint8_t pin_motor_a = 44;
static uint8_t pin_motor_b = 45;

// Head & taillights (neopixel)
static int pin_rgb_lights = 22;

// Timing debug output
static uint8_t pin_debug = 12;

// State variables: remember port state, and the timing of echo rising edge & high porch duration
static uint8_t portk_last;
static uint16_t last_rising_edge_us[] = {0, 0, 0, 0, 0, 0};
static volatile uint16_t last_high_porch_us[] = {0, 0, 0, 0, 0, 0};

// Odometer position
static volatile uint16_t odom_position[2] = {0};

// Vehicle state
static int16_t motorSpeed = 0;
static int16_t steeringAngle = 0;

Adafruit_TiCoServo steering;

Adafruit_NeoPixel lights_strip(8, pin_rgb_lights, NEO_GRB + NEO_KHZ800);

LiquidCrystal_I2C lcd(0x27,20,4);  // set the LCD address to 0x27 for a 16 chars and 2 line display

enum {
  HEADLIGHTS_OFF, HEADLIGHTS_POSITION, HEADLIGHTS_HIGHBEAM,
  TAILLIGHTS_OFF, TAILLIGHTS_POSITION, TAILLIGHTS_BRAKING
};

enum {
  BLINK_OFF, BLINK_LEFT, BLINK_RIGHT, BLINK_BOTH
};

static struct {
  int headlights;
  int taillights;
  int blinking; 
  unsigned long next_toggle_ms;
} lights_state;

#define COLOR_OFF                 0x000000
#define COLOR_STARTUP             0x000077  // blue at startup
#define COLOR_BLINK               0xff7700  // turn/warning signals: yellow/orange
#define COLOR_HEADLIGHTS_POSITION 0x333333  // position lights: white
#define COLOR_HEADLIGHTS_HIGHBEAM 0xffffff  // high beams: bright white
#define COLOR_TAILLIGHTS_POSITION 0x770000  // tail position lights: red
#define COLOR_TAILLIGHTS_BRAKING  0xff0000  // tail braking indicator


static volatile uint16_t isr_count = 0;
static volatile int isr_max_us = 0;

/**
 * @note: This ISR is critical to ultrasonic sensor measurements, as well as odometer position counting.
 */
void pin_change_isr() {
  digitalWrite(pin_debug, HIGH);
  uint16_t now_us = micros();
  
  // read port K, i.e. pins A8 - A15, all at once
  uint8_t portk = PINK;
  // what pin has changed state since last isr?
  uint8_t portk_diff = portk ^ portk_last;

  // process HC-SR04 distance sensors echo signal
  if (portk_diff & 0xFC) {
    for (int i = 0; i < 6; i++) {
      uint8_t mask = 1 << pin_dist_echo_bit[i];
      if (portk_diff & mask) { 
        // this bit has changed -> edge detected!
        if (portk & mask) {  
          // rising edge: remember time 
          last_rising_edge_us[i] = now_us;
        } else { 
          // falling edge: compute high porch duration
          last_high_porch_us[i] = now_us - last_rising_edge_us[i];
        }
      }
    }
  }

  // process odometry sensors
  if (portk_diff & 0x03) {
    for (int i = 0; i < 2; i++) {
      uint8_t mask = 1 << pin_odom_clk_bit[i];
      if (portk_diff & mask) {
        // edge detected at pin_odom_clk[i]
        uint8_t cur_clk = (portk & mask) ? HIGH : LOW;
        uint8_t cur_dat = digitalRead(pin_odom_dat[i]);
        if (cur_clk ^ cur_dat) {
          ++ odom_position[i];
        } else {
          -- odom_position[i];
        }
      }
    }
  }

  portk_last = portk;
  isr_count++;

  // measure ISR duration
  uint16_t end_us = micros();
  if (end_us - now_us > isr_max_us) {
    isr_max_us = end_us - now_us;
  }
  digitalWrite(pin_debug, LOW);
}

void setup() {
  Serial.begin(115200);
  Serial.println("Application " __FILE__ " compiled " __DATE__ " at " __TIME__ "");
  
  pinMode(pin_debug, OUTPUT);
  digitalWrite(pin_debug, HIGH);

  // Configure I2C LCD display
  lcd.init();
  lcd.backlight();
  lcd.clear();
  lcd.home();
  lcd.print("Starting!");

  // Configure head/taillights
  lights_strip.begin();
  lights_strip.setBrightness(255);
  lights_strip.fill(COLOR_STARTUP, 0, 8);
  lights_strip.show();

  // Configure ultrasonic distance sensors
  for (int i = 0; i < 6; i++) {
    pinMode(pin_dist_echo[i], INPUT);
    attachPCINT(digitalPinToPCINT(pin_dist_echo[i]), pin_change_isr, CHANGE);
  }

  // Configure odometry sensors
  for (int i = 0; i < 2; i++) {
    pinMode(pin_odom_clk[i], INPUT);
    pinMode(pin_odom_dat[i], INPUT);
    attachPCINT(digitalPinToPCINT(pin_odom_clk[i]), pin_change_isr, CHANGE);
  }

  // Configure motors
  pinMode(pin_motor_a, OUTPUT);
  pinMode(pin_motor_b, OUTPUT);
  pinMode(pin_servo, OUTPUT);
  steering.attach(pin_servo);
  steering.write(90);

  // Ready !
//  lcd.clear();
//  lcd.home();
//  lcd.print("Ready");
}

void loop() {  
  task_simulate();
  task_move();
  task_trigger_sensors();
  task_headlights();
  task_report();
  delay(10);
}

void task_move() {
  // Activate at 20Hz
  const int period_ms = 50;
  static unsigned int last_trigger_ms = 0;
  unsigned int now_ms = millis();
  if (now_ms - last_trigger_ms >= period_ms) {
    last_trigger_ms = now_ms;

    set_motor_speed(motorSpeed);
    steering.write(90 + steeringAngle);
  }
}

void task_simulate() {
  int d0 = last_high_porch_us[0] / 59;
  motorSpeed = map(d0, 0, 300, -255, 255);
  int d1 = last_high_porch_us[1] / 59;
  steeringAngle = map(d1, 0, 300, -90, 90);

  const int period_ms = 1000;
  static unsigned int last_trigger_ms = 0;
  unsigned int now_ms = millis();
  if (now_ms - last_trigger_ms >= period_ms) {
    last_trigger_ms = now_ms;

    static int state = 0;
    switch(state) {
    case 0:
      lights_state.headlights = HEADLIGHTS_OFF;
      lights_state.taillights = TAILLIGHTS_OFF;
      break;
    case 1:
      lights_state.headlights = HEADLIGHTS_POSITION;
      lights_state.taillights = TAILLIGHTS_POSITION;
      break;
    case 2:
      lights_state.headlights = HEADLIGHTS_HIGHBEAM;
      lights_state.taillights = TAILLIGHTS_BRAKING;
      break;
    case 3:
      lights_state.headlights = HEADLIGHTS_HIGHBEAM;
      lights_state.taillights = TAILLIGHTS_BRAKING;
      lights_state.blinking = BLINK_LEFT;
      break;      
    case 4:
      lights_state.headlights = HEADLIGHTS_HIGHBEAM;
      lights_state.taillights = TAILLIGHTS_BRAKING;
      lights_state.blinking = BLINK_RIGHT;
      break;
    default:
      state = -1;
    }
    state = (state + 1) % 8;
  }
}

void task_trigger_sensors() {
  const int period_ms = 300;
  static unsigned int last_trigger_ms = 0;
  unsigned int now_ms = millis();
  if (now_ms - last_trigger_ms >= period_ms) {
    last_trigger_ms = now_ms;
    
    for (int i = 0; i < 6; i ++) {
      pinMode(pin_dist_trig[i], OUTPUT);    
      digitalWrite(pin_dist_trig[i], HIGH);
    }
    for (int i = 0; i < 6; i ++) {
      digitalWrite(pin_dist_trig[i], LOW);
    }
  }
}

void task_report() {
  const int period_ms = 500;
  static unsigned int next_trigger_ms = period_ms;
  if ((int)(millis() - next_trigger_ms) >= 0) {
    next_trigger_ms += period_ms;
    for (int i = 0; i < 6; i++) {
        Serial.print(last_high_porch_us[i] / 59); 
        Serial.print(",");
    }
    Serial.print(odom_position[0]);
    Serial.print(", ");
    Serial.print(odom_position[1]);
    Serial.print(", ");
    Serial.print("isr max (us) ");
    Serial.println(isr_max_us);

    lcd.clear();
    lcd.home();
    for (int i = 0; i < 6; i++) {
        lcd.print(last_high_porch_us[i] / 59);         
        lcd.print(",");
    }
    lcd.print(odom_position[0]);
    lcd.print(",");
    lcd.print(odom_position[1]);
    lcd.print(".");

  }
}

void task_headlights() {
  const int period_ms = 100;
  static unsigned int next_trigger_ms = period_ms;
  if ((int)(millis() - next_trigger_ms) >= 0) {
    next_trigger_ms += period_ms;
    
    // Update headlights color
    switch (lights_state.headlights) {
    case HEADLIGHTS_POSITION:
      lights_set_color(1, COLOR_HEADLIGHTS_POSITION);
      lights_set_color(2, COLOR_HEADLIGHTS_POSITION);
      break;
    case HEADLIGHTS_HIGHBEAM:
      lights_set_color(1, COLOR_HEADLIGHTS_HIGHBEAM);
      lights_set_color(2, COLOR_HEADLIGHTS_HIGHBEAM);    
      break;
    case HEADLIGHTS_OFF:
    default: // fallthrough  
      lights_set_color(1, COLOR_OFF);
      lights_set_color(2, COLOR_OFF);
    }

    // Update taillights color
    switch(lights_state.taillights) {
    case TAILLIGHTS_POSITION:
      lights_set_color(5, COLOR_TAILLIGHTS_POSITION);
      lights_set_color(6, COLOR_TAILLIGHTS_POSITION);
      break;
    case TAILLIGHTS_BRAKING:
      lights_set_color(5, COLOR_TAILLIGHTS_POSITION);
      lights_set_color(6, COLOR_TAILLIGHTS_POSITION);    
      break;
    case TAILLIGHTS_OFF: 
    default: // fallthrough
      lights_set_color(5, COLOR_OFF);
      lights_set_color(6, COLOR_OFF);
    }

    // Update blinkers status
    static int blink_state = 0;
    if ((long)(millis() - lights_state.next_toggle_ms) >= 0) {
      lights_state.next_toggle_ms += 300;
      blink_state = !blink_state;
    }

    if (blink_state && 
        ((lights_state.blinking == BLINK_LEFT) || 
         (lights_state.blinking == BLINK_BOTH))) {
      lights_set_color(0, COLOR_BLINK);
      lights_set_color(4, COLOR_BLINK);
    } else {
      lights_set_color(0, COLOR_OFF);
      lights_set_color(4, COLOR_OFF);
    }
    if (blink_state && 
        ((lights_state.blinking == BLINK_RIGHT) || 
         (lights_state.blinking == BLINK_BOTH))) {
      lights_set_color(3, COLOR_BLINK);
      lights_set_color(7, COLOR_BLINK);      
    } else {
      lights_set_color(3, COLOR_OFF);
      lights_set_color(7, COLOR_OFF);      
    }
    lights_strip.show();
  }
}

void lights_set_color(int pixel, uint32_t color) {
  lights_strip.setPixelColor(pixel, color);
}

void set_motor_speed(int speed) {
  // Set PWM for motor speed
  speed = constrain(speed, -255, 255);
  if (speed == 0) {
    digitalWrite(pin_motor_a, LOW);
    digitalWrite(pin_motor_b, LOW);
  } else if (speed > 0) {
    digitalWrite(pin_motor_a, LOW);
    analogWrite(pin_motor_b, speed);
  } else {
    analogWrite(pin_motor_a, -speed);
    digitalWrite(pin_motor_b, LOW);
  }
}