#include <SPI.h>
#include "mcp_can.h"
#include <mcp_can_dfs.h>

unsigned char login[8] = {0x16, 0x36, 0x71, 0x07, 0x03, 0x76, 0x00, 0x00}; //this is for logging into your flatpack. Must use your serial number.

const int SPI_CS_PIN = 10;
MCP_CAN CAN(SPI_CS_PIN);

#define VOLTAGE 4800
#define CURRENT 450

uint8_t outset[8] = {CURRENT & 0xff, (CURRENT >> 8) & 0xff, VOLTAGE & 0xFF, (VOLTAGE >> 8) & 0xFF ,VOLTAGE & 0xFF, (VOLTAGE >> 8) & 0xFF ,0x3E, 0x17};
volatile int got_cb_data = 0;
unsigned char setdefaultvolt[5] = {0x29, 0x15, 0x00, 0x80, 0x16};
float potvolts;

void setup()
{
   Serial.begin(115200);
   pinMode(3,INPUT);
   pinMode(12,INPUT);
START_INIT:
   if(CAN_OK == CAN.begin(MCP_ANY, CAN_125KBPS, MCP_8MHZ))
   {
       \\ can bus comms OK
   }
else
   {
   Serial.println("CAN BUS Shield init fail");
   delay(1000);
   goto START_INIT;
   }  
CAN.setMode(0);    
delay(200);
CAN.sendMsgBuf(0x05004804, 1, 8, login);    // id = 1 so XX = 04    
potvolts = 54.0;
}


void loop() {
   unsigned char len = 0;
   unsigned char buf[8] ;
   int rv;
   float a,i,v;
   unsigned int ui;
   got_cb_data = 0;
   rv = CAN.checkReceive();
   if(rv == CAN_MSGAVAIL )
   {  
       CAN.readMsgBuf(&len, buf);
       uint32_t canId = CAN.getCanId();
       if (canId == 0x05014004 || canId == 0x05014008)
         {
         i = buf[2]*255*0.1+buf[1]*0.1;
         v = buf[4]*255*0.01+buf[3]*0.01;
         Serial.print (potvolts); Serial.print(" ");Serial.print (v); Serial.print(" ");Serial.print(i);Serial.println(" 0 60 ");
         }
      //send request for new update
      CAN.sendMsgBuf(0x05004804, 1, 8, login);     // XX = 04
      a = analogRead(0)/660.0;           // pot is connected to 3.3V so need this scale for 0 - 1 range. 5V pin is used by canbus adaptor
      potvolts = 40.0 + 20.0*a;           // set demand from 40 to 60V. flatpack has output limits smaller than this range.
      if (potvolts > 57.4)        potvolts = 57.4;
      if (potvolts < 42.7)        potvolts = 42.7;
      ui = int(potvolts * 100.0);
      // make changes to control word with new Vout
      outset[2] = outset[4] = ui & 0xff;     // low 8 bits of scaled x 100 voltage
      outset[3] = outset[5] = (ui >> 8) & 0xff;  // high 8 bits
      CAN.sendMsgBuf(0x05FF4004, 1, 8, outset);    
  }
}