#include <Arduino.h>
#include <WiFi.h>
#include <Wire.h>
#include "globals.h"
#include <SPI.h>
#include "MCP3XXX.h"
#include "PCF8574.h"
#include "pio_encoder.h"
#include "dumbdisplay.h"
#include "wifidumbdisplay.h"
#include "zserio.h"
#include "SerialUART.h"

const char* ssid = "TEST";
const char* password = "pink4bubble";
DumbDisplay dumbdisplay(new DDWiFiServerIO(ssid, password), 8192);
LcdDDLayer *optionsdisplay = NULL;
SevenSegmentRowDDLayer *sevenSeg; 
JoystickDDLayer *appjoystick;

packet_t pA, pB, safe;
packet_t *astate, *incoming;
comm_state cs;

// character   0     1     2     3     4     5     6     7     8     9     A     b     C     d     E     F
byte d[] = { 0x7e, 0x30, 0x6d, 0x79, 0x33, 0x5b, 0x5f, 0x70, 0x7f, 0x7b, 0x77, 0x1f, 0x4e, 0x3d, 0x4f, 0x47 };

PCF8574 ioex1(0x20, 20, 21);
PCF8574 ioex2(0x21, 20, 21);
PioEncoder enc1(18); // right
PioEncoder enc2(14); // left

MCP3008 adc;
int count = 0;
int mode = 1;
char left_enabled = 0, right_enabled = 0;
int current_offset[2];
int setup_complete = false;

// driving vars
int target_left_power = 0;
int target_right_power = 0;
int left_power = 0;
int right_power = 0;
int acceleration = 1;
bool turbo = false;
int left_cooldown = 0;
int right_cooldown = 0;
int olddisplay = 99999;

unsigned int getButton(unsigned int num) {
  if (num <= 7) {
    return (astate->btnlo >> num) & 0x01;
  } else if (num > 7 && num <= 15) {
    return (astate->btnhi >> (num - 8)) & 0x01;
  } else {
    return 0;
  }
}
unsigned int getDPad() {
  // four bits: left down right up
  return (astate->btnhi >> 4);
}


void FeedbackHandler(DDLayer* pLayer, DDFeedbackType type, const DDFeedback&) {
    if (pLayer == optionsdisplay) {
        // clicked the "clear" button
        if(turbo) {
          turbo = false;
          pLayer->backgroundColor("lightgray");
        }
        else {
          turbo = true;
          pLayer->backgroundColor("red");
        }
        //delay(100);
    } 
}

void osmc_init() {
  digitalWrite(ALI1, LOW);
  digitalWrite(BLI1, LOW);
  digitalWrite(AHI1, LOW);
  digitalWrite(BHI1, LOW);
  digitalWrite(ALI2, LOW);
  digitalWrite(BLI2, LOW);
  digitalWrite(AHI2, LOW);
  digitalWrite(BHI2, LOW);
  digitalWrite(DENABLE1, HIGH);
  digitalWrite(DENABLE2, HIGH);
  
  pinMode(ALI1, OUTPUT);
  pinMode(AHI1, OUTPUT);
  pinMode(BLI1, OUTPUT);
  pinMode(BHI1, OUTPUT);
  pinMode(ALI2, OUTPUT);
  pinMode(AHI2, OUTPUT);
  pinMode(BLI2, OUTPUT);
  pinMode(BHI2, OUTPUT);
  pinMode(DENABLE1, OUTPUT);
  pinMode(DENABLE2, OUTPUT);
  //pinMode(DREADY1, INPUT);
  //pinMode(DREADY2, INPUT);

  digitalWrite(22, LOW);
  pinMode(22, OUTPUT);
  analogWriteFreq(4000); // set PWM frequency to 16kHz
}

char try_enable_osmc(char enabled, char enablepin, float vbatt,
                     char ali, char bli, char ahi, char bhi) {
  // check that power is present at the OSMC
  if (vbatt > 13) {
    if (!enabled){
      delay(10); //"Short" delay required in order to prevent blowout! 10ms is conservative.
      //delay(1000);
      digitalWrite(enablepin, LOW);
    }
    return 1;
  }
  else { // controller has no power; zero inputs in case we power it again
    digitalWrite(enablepin, HIGH);
    digitalWrite(ali, LOW);
    digitalWrite(bli, LOW);
    digitalWrite(ahi, LOW);
    digitalWrite(bhi, LOW);
    return 0;
  }
}

float get_voltage(byte battery) {
  int read = adc.analogRead(battery);
  //Serial.println(read);
  if (read == 1023)
    return 0.0;
  read = 0;
  for(int i = 0; i < 5; i++) {
    read += adc.analogRead(battery);
    delay(1);
  }
  return (read/5.0) * 5 * 10 / 1024.0;
}

void set_offset() {
  int n = 100;
  current_offset[0] = current_offset[1] = 0;
  for (int i = 0; i < n; i++) {
    current_offset[0] += adc.analogRead(2);
    current_offset[1] += adc.analogRead(3);
    delay(10);
  }
  current_offset[1] /= n;
  current_offset[1] -= 512;
  //Serial.println(current_offset[0]);
  current_offset[0] /= n;
  //Serial.println(current_offset[0]);
  current_offset[0] -= 512;
  //Serial.println(current_offset[0]);
}
float get_current(byte sensor) {
  int read = 0;
  for(int i = 0; i < 20; i++) {
    read += adc.analogRead(sensor + 2);
    delay(1);
    //Serial.println(read/i);
  }
  //Serial.println(read/5.0);
  return (read/20.0 - 512.0 - current_offset[sensor]) / 1.28;
}
// OSMC motor controller stuff
// Low side outputs must be PWM capable and NOT 5 or 6 (on Uno)
// Do not change timer0,
// Pins 7 and 8 use timer4 in phase correct mode
// Pins 11 and 12 use timer1 in phase correct mode
// OSMC ALI and BLI are the low side driver inputs and must ALWAYS be low/zero when the ready signal is not provided
// OSMC AHI and BHI are the high side driver inputs.
/*
 * ----------- Vdd
 *   |     |
 *   AHI   BHI
 *   |     |
 *   ---M---
 *   |     |
 *   ALI   BLI
 *   |     |
 *   --------- GND
 */
void drive_osmc(char enabled, char enablepin, int rawpower, char brake,
                char ali, char bli, char ahi, char bhi) {
  int power = constrain(rawpower, -176, 176); // NOTE - with optocouplers, ~176 becomes 100%

  if (!enabled) {
    digitalWrite(ali, LOW);
    digitalWrite(bli, LOW);
    digitalWrite(ahi, LOW);
    digitalWrite(bhi, LOW);
    digitalWrite(enablepin, HIGH);
    return;
  }
  //Stop!
  if (abs(power) < 5) {
    digitalWrite(ali, LOW);
    digitalWrite(bli, LOW);
    delayMicroseconds(63);
    if (brake != 0) {
      digitalWrite(ahi, HIGH);
      digitalWrite(bhi, HIGH);
    } else {
      digitalWrite(ahi, LOW);
      digitalWrite(bhi, LOW);
    }
    return;
  }
  //Serial.print("Driving OSMC with power ");
  //Serial.println(power);
  //Forward!
  if (power > 0) {
    digitalWrite(bhi, LOW);
    digitalWrite(ali, LOW);
    delayMicroseconds(63);
    digitalWrite(ahi, HIGH);
    analogWrite(bli, power);
  }
  //Reverse!
  if (power < 0) {
    digitalWrite(ahi, LOW); 
    digitalWrite(bli, LOW);
    delayMicroseconds(63);
    digitalWrite(bhi, HIGH);
    analogWrite(ali, abs(power));
  }
}

void set_mosfet(bool pin, bool value) {
  ioex1.digitalWrite(pin, value); // first 2 pins of ioex1 (top) are the mosfets
}

void set_digit(byte digit, byte value) 
{
  Wire.beginTransmission(0x38);
  Wire.write(0x20 + digit);
  Wire.write(d[value]);
  Wire.endTransmission();
  //Serial.print("Set digit ");
  //Serial.print(digit);
  //Serial.print(" to ");
  //Serial.println(value);
  //delay(5000);
}

void set_raw(byte digit, byte value) {
  Wire.beginTransmission(0x38);
  Wire.write(0x20 + digit);
  Wire.write(value);
  Wire.endTransmission();
}
void set_blank() {
  Wire.beginTransmission(0x38);
  Wire.write(0x20);
  Wire.write((byte)0);
  Wire.write((byte)0);
  Wire.endTransmission();
}
void set_hex(byte num) {
  byte digit1 = num; 
  digit1 = digit1 >> 4; // shift right by 4
  //while (digit1 > 15) {
  //  digit1 -= 16;
  //}
  byte digit2 = num;
  while (digit2 > 15) {
    digit2 -= 16;
  }
  set_digit(0, digit1);
  set_digit(1, digit2);
  if(num != olddisplay && dumbdisplay.connected() && millis() % 10 < 1) {
    olddisplay = num;
    sevenSeg->showHexNumber(num);
  }
  set_raw(4, 0x00); // clear second dot
}

void set_dec(byte num) {
  byte digit1 = num / 10; 
  //while (digit1 > 9) {
  //  digit1 -= 10;
  //}
  byte digit2 = num;
  while (digit2 > 9) {
    digit2 -= 10;
  }
  set_digit(0, digit1);
  set_digit(1, digit2);
  if(num != olddisplay && dumbdisplay.connected() && millis() % 10 < 1) {
    olddisplay = num;
    sevenSeg->showNumber(num);
  }
  set_raw(4, 0x02); // set second dot
}


void setup() {
  WiFi.noLowPowerMode();
  rp2040.enableDoubleResetBootloader();

  pinMode(LED_BUILTIN, OUTPUT);
  pinMode(32+1, OUTPUT);
  digitalWrite(32+1, LOW); // set SMPS to full power mode (pin connected thru wifi chip)
  digitalWrite(LED_BUILTIN, HIGH);
  Serial.begin(115200);
  //Serial.println("hello!");
  delay(2000);
  Serial.println("Initializing RIB subsystems..");


  Serial.print("Enabling LED driver..");
  Wire.setSDA(20);
  Wire.setSCL(21);
  Wire.begin();
  Wire.beginTransmission(0x38);
  Wire.write(0x01); // register: decode mode
  Wire.write(0x00); // disable decode mode for all digits
  Wire.write(0x3f); // intensity max
  Wire.write(0x03); // scan limit 3
  Wire.write(0x01); // normal operation
  Wire.endTransmission();
  Wire.beginTransmission(0x38);
  Wire.write(0x07); // display mode register
  Wire.write(0x01); // display test mode
  Wire.endTransmission();
  delay(100);

  Wire.beginTransmission(0x38);
  Wire.write(0x07);
  Wire.write(0x00); // disable display test mode
  Wire.endTransmission();
  Serial.println(" done");
  

  Serial.print("Initializing ADC..");
  set_hex(0x1);
  SPI1.setRX(12);
  SPI1.setCS(13);
  SPI1.setTX(11);
  SPI1.setSCK(10);
  adc.begin(13);
  //pinMode(13, OUTPUT);
  //pinMode(11, OUTPUT);
  //pinMode(10, OUTPUT);
  //digitalWrite(13, HIGH);
  //digitalWrite(11, HIGH);
  //digitalWrite(10, HIGH);
  //adc.begin(13,11,12,10);
  Serial.println(" done");
  delay(20);

  Serial.print("Initializing OSMCs..");
  set_hex(0x2);
  osmc_init();
  Serial.println(" done");
  delay(20);
  //delay(2000);
  

  bool ioex1p, ioex2p = false;
  Serial.print("Initializing I/O expanders..");
  set_hex(0x3);
  if(ioex1.begin()) {
    delay(200);
    Serial.print(" 1");
    ioex1p = true;
  } else {
    delay(20);
  }
  set_hex(0x4);
  if(ioex2.begin()) {
    delay(20);
    Serial.print(" 2");
    ioex2p = true;
  } else {
    delay(20);
  }

  Serial.println(" done");
  delay(20);

  Serial.print("Initializing encoders..");
  set_hex(0x5);
  enc1.begin();
  enc2.begin();
  Serial.println(" done");
  delay(20);

  Serial.print("Initializing xBee radio..");
  set_hex(0x6);
  SerComm.setRX(17);
  SerComm.setTX(16);
  SerComm.begin(57600);
  comm_init(); //Initialize the communication FSM
  safe.stickX = 127;
  safe.stickY = 127;
  safe.btnhi = 0;
  safe.btnlo = 0;
  safe.cksum = 0b1000000010001011;

  Serial.println(" done");
  delay(20);



  Serial.println("Initialization complete.");
  Serial.println("Running self-tests..");
  byte pass = 0;
  set_hex(0x7);
  Serial.print("Checking LED driver..");
  Wire.beginTransmission(0x38);
  if(Wire.endTransmission() != 0) {
    Serial.println(" WARNING: LED driver not detected");
    set_hex(0xF7);
    delay(500);
  } else {
    Serial.println(" done");
    delay(20);
    pass++;
  }
  
  // TODO

  set_hex(0x8);
  Serial.print("Checking ADC..");
  byte startpass = pass;
  for (size_t i = 0; i < adc.numChannels(); ++i)
  {
    if (adc.analogRead(i) != 0 && adc.analogRead(i) != 1023) {
      pass = startpass+1; // check that at least one reading is successful to confirm MCP3008 is responding
    }
  }
  if (pass == startpass+1) {
    Serial.println(" done");
    delay(20);
  } else {
    Serial.println(" WARNING: ADC not detected");
    set_hex(0xF8);
    delay(500);
  }
  Serial.print("Calibrating current sensors..");
  set_offset();
  Serial.println("done");

  Serial.print("Checking OSMC 1..");
  set_hex(0x9);
  if (get_voltage(0) > 13) {
    Serial.println(" done");
    delay(20);
    pass++;
  } else {
    Serial.println(" WARNING: OSMC 1 battery too low or OSMC not present");
    set_hex(0xF9);
    delay(500);
  }
  set_hex(0xA);
  Serial.print("Checking OSMC 2..");
  if (get_voltage(1) > 13) {
    Serial.println(" done");
    delay(20);
    pass++;
  } else {
    Serial.println(" WARNING: OSMC 2 battery too low or OSMC not present");
    set_hex(0xFA);
    delay(500);
  }

  set_hex(0xB);
  Serial.print("Checking I/O expander 1..");
  for (int i = 0; i < 8; i++) {
    ioex1.pinMode(i,OUTPUT, LOW);
  }
  if(ioex1p == false) {
    Serial.println(" WARNING: I/O expander not detected");
    set_hex(0xFB);
    delay(500);
  } else {
    Serial.println(" done");
    delay(20);
    pass++;
  }
  set_hex(0xC);
  Serial.print("Checking I/O expander 2..");
  for (int i = 0; i < 8; i++) {
    ioex2.pinMode(i,OUTPUT, LOW);
  }
  if(!ioex2p == false) {
    Serial.println(" WARNING: I/O expander not detected");
    set_hex(0xFC);
    delay(500);
  } else {
    Serial.println(" done");
    delay(20);
    pass++;
  }

  Serial.print("Self tests complete: ");
  Serial.print(pass);
  Serial.println("/6 tests passed.");
  Serial.println("RIB is ready to go. Starting program.");
  set_blank();
  /*dumbdisplay.recordLayerSetupCommands();
  sevenSeg = dumbdisplay.create7SegmentRowLayer(2); // 2 digits
  appjoystick = dumbdisplay.createJoystickLayer(255, "lr+tb", 1); // max, directions, scale in UI
  appjoystick->autoRecenter(true);
  appjoystick->moveToCenter();
  optionsdisplay = dumbdisplay.createLcdLayer(5, 1);   
  optionsdisplay->setFeedbackHandler(FeedbackHandler);
  optionsdisplay->backgroundColor("lightgray");
  optionsdisplay->print("TURBO");
  dumbdisplay.configAutoPin(DD_AP_HORI_2(
                                appjoystick->getLayerId(),
                                DD_AP_VERT_2(
                                    sevenSeg->getLayerId(),
                                    optionsdisplay->getLayerId())));
  dumbdisplay.playbackLayerSetupCommands("basic");*/
  //dumbdisplay.configAutoPin(DD_AP_VERT); // auto vertical pin layout
  setup_complete = true;
  rp2040.wdt_begin(500); // start watchdog with 500ms limit. Safety feature; reset during crash to disable motors!
}

void setup1() {
  while(!setup_complete)
    delay(100);
}
void print_status() {
  Serial.print(get_voltage(0));
  Serial.print("V  ");
  Serial.print(get_current(0));
  Serial.print("A  ENC1: ");
  Serial.print(enc1.getCount());
  Serial.print("  ENC2: ");
  Serial.println(enc2.getCount());

  SerComm.print(get_voltage(0));
  SerComm.print("V  ");
  SerComm.print(get_current(0));
  SerComm.print("A  ENC1: ");
  SerComm.print(enc1.getCount());
  SerComm.print("  ENC2: ");
  SerComm.println(enc2.getCount());


}


void loop() {
  rp2040.wdt_reset();
  comm_parse();
  if (getButton(SHOULDER_TOP_RIGHT))
    turbo = true;
  else 
    turbo = false;
  //const DDFeedback*  fb;
  /*if (!dumbdisplay.connected() || !WiFi.isConnected()) {
    target_left_power = 0;
    target_right_power = 0;
    Serial.print("Connection lost");
    
  } else
    fb = appjoystick->getFeedback();*/
  
  int zeroed_power =    -1 * ((int)(astate->stickX) - 127);
  int zeroed_turn =      ((int)(astate->stickY) - 127);
  
  
  if (true) { //fb != NULL) {
    //int x = fb->x - 127;
    //int y = - fb->y + 127;
    int x = zeroed_turn;
    int y = zeroed_power;
    //Serial.print(x);
    //Serial.print(" ");
    //Serial.println(y);
    
    double rawdriveangle = atan2(x, y);
    double driveangle = rawdriveangle * 180 / 3.1415926;
    target_left_power = y;
    target_right_power = y;
    
    target_left_power += x;
    target_right_power += -x;
    target_left_power = constrain(target_left_power, -127, 127);
    target_right_power = constrain(target_right_power, -127, 127);
    if(turbo) {
      target_left_power *= 2;
      target_right_power *= 2;
    }
    target_left_power = target_left_power * 0.675;
    target_right_power = target_right_power * 0.675;


  

  }
  
  if(turbo)
    acceleration = 8;
  else
    acceleration = 3;

  if(left_cooldown > 0)
    left_cooldown --;

  if(abs(target_left_power) <= 4 && abs(left_power) > 5) {
    left_power = 0;
    left_cooldown = 2;
  }
  else if(target_left_power >= left_power + acceleration && left_cooldown == 0)
    left_power += acceleration;
  else if(acceleration > target_left_power - left_power && left_cooldown == 0)
    left_power = target_left_power;
  else if(target_left_power <= left_power - acceleration && left_cooldown == 0)
    left_power -= acceleration;
  else if(acceleration > left_power - target_left_power && left_cooldown == 0)
    left_power = target_left_power;

  if(right_cooldown > 0)
    right_cooldown --;

  if(abs(target_right_power) <= 4 && abs(right_power) > 5) {
    right_power = 0;
    right_cooldown = 2;
  }
  else if(target_right_power >= right_power + acceleration && right_cooldown == 0)
    right_power += acceleration;
  else if(acceleration > target_right_power - right_power && right_cooldown == 0)
    right_power = target_right_power;
  else if(target_right_power <= right_power - acceleration && right_cooldown == 0)
    right_power -= acceleration;
  else if(acceleration > right_power - target_right_power && right_cooldown == 0)
    right_power = target_right_power;

  int avg_speed = (abs(right_power) + abs(left_power))/2;
  //SerComm.println();
  set_hex(avg_speed);

  drive_right(right_enabled, right_power);
  drive_left(left_enabled, -left_power);
  SerComm.println(" L: " + String(left_power) + " LT: " + String(target_left_power) + " R: " + String(right_power) + " RT: " + String(target_right_power) + " MEM FREE: "+ String(rp2040.getFreeHeap()));

  //if(left_power != target_left_power || right_power != target_right_power)

  
  
  //delay(1000);
  //set_digit(0, 6);
  //set_digit(0, 10);
  //set_digit(1, 9);
  //set_digit(1, 10);
  //set_digit(2, 8);
  //set_digit(2, 10);
  //set_digit(3, 8);
  //set_digit(3, 10);
  //set_digit(4, 8);
  //set_digit(4, 10);
  /*if (mode == 0) {
    set_raw(count / 8, count % 8);
    if (count < 39) {
      count ++;
    } else {
      count = 0;
      mode = 1;
      delay(100);
    }
  }*/
  //print_status();
  //drive_right(right_enabled, 10);
  //drive_left(left_enabled, 10);
  /*if (millis() % 3000 > 1500) {
    set_mosfet(0, LOW);
    set_mosfet(1, LOW);
    //ioex2.digitalWrite(7, LOW);
  }
  if (millis() % 3000 < 1500) {
    set_mosfet(0, HIGH);
    set_mosfet(1, HIGH);
    //ioex2.digitalWrite(7, HIGH);

  }*/
  /*if (mode == 1) {
    set_dec(count);
    drive_right(right_enabled, count);
    //set_hex(count);
    if (count < 40) {
      count += 5;
    } else {
      //count = 0;

      mode = 2;
    }
  }
  if (mode == 2) {
    set_dec(count);
    drive_right(right_enabled, count);
    //set_hex(count);
    if (count > 5) {
      count -= 5;
    } else {
      //count = 0;
      
      mode = 1;
    }
  }*/
  //delay(200);
  delay(50);
  //DDYield();

}
int loopcount = 0;
void loop1() {
  rp2040.wdt_reset();
  //digitalWrite(LED_BUILTIN, HIGH);  
  if(loopcount == 20) {
    //print_status();
    loopcount = 0;
    delay(25);
  }
  else {
    delay(25);
    loopcount++;
  }
  //SerComm.println("update");
  left_enabled = try_enable_left(left_enabled, get_voltage(1));
  right_enabled = try_enable_right(right_enabled, get_voltage(0));
  //digitalWrite(LED_BUILTIN, LOW);
  delay(25);
}