#include <SPI.h>

extern "C" {
#include "hardware.h"
#include "battery.h"
#include "error.h"
#include "logging.h"
#include "slave.h"
#include "tasks.h"
}

#define CLK 52
#define DIN 51
#define CS  53
#define X_SEGMENTS   4
#define Y_SEGMENTS   1
#define NUM_SEGMENTS (X_SEGMENTS * Y_SEGMENTS)
#define X_LEDS_PER_SEGMENT 8
#define Y_LEDS_PER_SEGMENT 8


// a framebuffer to hold the state of the entire matrix of LEDs
// laid out in raster order, with (0, 0) at the top-left
byte fb[X_LEDS_PER_SEGMENT * Y_LEDS_PER_SEGMENT * X_SEGMENTS] =
{
  // 1, 1, 0, 1, 1, 0, 0, 0,
  // 0, 1, 1, 1, 1, 0, 0, 1,
  // 1, 0, 0, 0, 1, 1, 1, 1,
};

void shiftAll(byte send_to_address, byte send_this_data)
{
  digitalWrite(CS, LOW);
  for (int i = 0; i < NUM_SEGMENTS; i++)
  {
    SPI.transfer(send_to_address);
    SPI.transfer(send_this_data);
  }
  digitalWrite(CS, HIGH);
}

void setup()
{
  Serial.begin(115200);
  pinMode(CLK, OUTPUT);
  pinMode(DIN, OUTPUT);
  pinMode(CS, OUTPUT);
  SPI.beginTransaction(SPISettings(16000000, MSBFIRST, SPI_MODE0));

  // Setup each MAX7219
  shiftAll(0x0f, 0x00); //display test register - test mode off
  shiftAll(0x0b, 0x07); //scan limit register - display digits 0 thru 7
  shiftAll(0x0c, 0x01); //shutdown register - normal operation
  shiftAll(0x0a, 0x0f); //intensity register - max brightness
  shiftAll(0x09, 0x00); //decode mode register - No decode

  hal_init();
  slave_initialize();
  BMS_battery_closeInterlock();
  BMS_tasks_init();
  BMS_battery_stats.reads = 10;

  for (int iCell, iSlave = 0; iSlave < TOTAL_SLAVES; iSlave++)
  {
      for (iCell = 0; iCell < CELL_VOLTAGES_PER_SLAVE; iCell++)
      {
          BMS_battery_stats.slave[iSlave].cv[iCell] = random(35000, 35300);
      }
  }

  Serial.println("setup done");
}

#define X_SEGMENTS   4
#define Y_SEGMENTS   1
#define NUM_SEGMENTS (X_SEGMENTS * Y_SEGMENTS)
#define X_LEDS_PER_SEGMENT 8
#define Y_LEDS_PER_SEGMENT 8

void updateMatrix()
{
  static int y, segment, n, x, i;
  static byte flag, shift;

  for (y = 0; y < Y_LEDS_PER_SEGMENT; y++)
  {
    digitalWrite(CS, LOW); // start of row

    for (segment = 0; segment < X_SEGMENTS; segment++)
    {
      flag = 0;

      for (n = 0; n < X_LEDS_PER_SEGMENT; n++)
      {
        x = X_LEDS_PER_SEGMENT * segment + n;
        i = Y_LEDS_PER_SEGMENT * x + y;
        shift = X_LEDS_PER_SEGMENT - 1 - n;
        flag |= (fb[i] ? 1 : 0) << shift;
      }

      SPI.transfer(y+1);
      SPI.transfer(flag);
      // Serial.println(segment); 
    }

    digitalWrite(CS, HIGH); // end of row
  }
}

void updateFramebuffer()
{
  static int iSlave, iCell;
  static byte isDischarging;

  for (iSlave = 0; iSlave < TOTAL_SLAVES; iSlave++)
  {
      for (iCell = 0; iCell < CELL_VOLTAGES_PER_SLAVE; iCell++)
      {
          isDischarging = (BMS_battery_stats.slave[iSlave].cellDischarge >> iCell) & 1;
          fb[CELL_VOLTAGES_PER_SLAVE*iSlave+iCell] = isDischarging;

          if (isDischarging)
            BMS_battery_stats.slave[iSlave].cv[iCell] -= 1;

          //Serial.println(BMS_battery_stats.slave[iSlave].cv[iCell]);
      }
  }
}

void loop()
{
  BMS_tasks_run();
  updateFramebuffer();
  updateMatrix();
  delay(100);

  static int iSlave, iCell;
  // for (iSlave = 0; iSlave < TOTAL_SLAVES; iSlave++)
  // {
  //     for (iCell = 0; iCell < CELL_VOLTAGES_PER_SLAVE; iCell++)
  //     {
  //         dischargeStates |= (iSlave == 8) << iCell;
  //     }

  //     BMS_battery_stats.slave[iSlave].cellDischarge = dischargeStates;
  // }
}