/*
 * Stairs lighting
 * Copyright 2017 (c) Mark van Renswoude
 *
 * https://git.x2software.net/pub/Stairs
*/
#include <RS485_non_blocking.h>
#include <SoftwareSerial.h>
#define SERIAL_COMMANDS_DEBUG
#include <SerialCommands.h>
#include "protocol.h"


const uint8_t PinRS485Receive = 2;
const uint8_t PinRS485Transmit = 3;
const uint8_t PinRS485WriteEnable = 4;

const uint32_t RS485BaudRate = 76800;


/*
 * RS485 communication
 */
SoftwareSerial rs485(PinRS485Receive, PinRS485Transmit);

size_t doWrite(const byte what)
{
  return rs485.write (what);  
}

int doAvailable()
{
  return rs485.available ();  
}

int doRead()
{
  return rs485.read ();  
}

RS485 comm(doRead, doAvailable, doWrite, 20);
  


/*
 * Serial communication (user input)
 */
SerialCommand cmdPing("ping", handlePingCommand);
SerialCommand cmdDisplay("display", handleDisplayCommand);
SerialCommand cmdLink("link", handleLinkCommand);
SerialCommand cmdLinkAssign("assign", handleLinkAssignCommand);
SerialCommand cmdLinkStop("stop", handleLinkStopCommand);
SerialCommand cmdSetPWM("setPWM", handlesetPWMCommand);
SerialCommand cmdGetSensors("getSensors", handlegetSensorsCommand);


char commandBuffer[32];
SerialCommands serialCommands(&Serial, commandBuffer, sizeof(commandBuffer), "\r\n", " ");

void cmdUnrecognized(SerialCommands* sender, const char* cmd)
{
  sender->GetSerial()->print("ERROR: Unrecognized command [");
  sender->GetSerial()->print(cmd);
  sender->GetSerial()->println("]");
}


bool inLink = false;


void showCommands()
{
  Serial.println("");
  Serial.println("Available commands:");
  Serial.println("  ping [moduleIndex]");
  Serial.println("  display");
  Serial.println("  link");
  Serial.println("  setPWM [moduleIndex] [flags] [value1] [value2]");
  Serial.println("  getSensors [moduleIndex]");
  Serial.println("");  
}

void setup()
{
  pinMode(PinRS485WriteEnable, OUTPUT); 
  
  rs485.begin(RS485BaudRate); 
  comm.begin();

  Serial.begin(115200);
  serialCommands.AddCommand(&cmdPing);
  serialCommands.AddCommand(&cmdDisplay);
  serialCommands.AddCommand(&cmdLink);
  serialCommands.AddCommand(&cmdLinkAssign);
  serialCommands.AddCommand(&cmdLinkStop);
  serialCommands.AddCommand(&cmdSetPWM);
  serialCommands.AddCommand(&cmdGetSensors);
  serialCommands.SetDefaultHandler(&cmdUnrecognized);

  Serial.println("Stairs module Mock Master ready");
  showCommands();
}


void loop ()
{
  serialCommands.ReadSerial();

  if (comm.update())
    handleCommMessage();
}


void sendCommMessage(const byte* data, size_t len)
{
  Serial.print("Raw out: ");
  byte* ptr = data;
  for (size_t i = 0; i < len; i++, ptr++)
  {
    Serial.print(*ptr, HEX);
    Serial.print(' ');
  }
  Serial.println("");
  
  
  digitalWrite(PinRS485WriteEnable, HIGH);
  comm.sendMsg(data, len);
  digitalWrite(PinRS485WriteEnable, LOW);
}


void handlePingCommand(SerialCommands* sender)
{
  if (inLink) return;
  
  int moduleIndex = atoi(sender->Next());
  
  sender->GetSerial()->println("> Ping");
  const byte msg[] = { CommandPing, moduleIndex };
  sendCommMessage(msg, sizeof(msg));
}


void handleDisplayCommand(SerialCommands* sender)
{
  if (inLink) return;

  sender->GetSerial()->println("> DisplayModuleIndex");
  const byte msg[] = { CommandDisplayModuleIndex };
  sendCommMessage(msg, sizeof(msg));
}


void handleLinkCommand(SerialCommands* sender)
{
  if (inLink) return;

  sender->GetSerial()->println("> Link");
  const byte msg[] = { CommandStartLink };
  sendCommMessage(msg, sizeof(msg));
    
  sender->GetSerial()->println("");  
  sender->GetSerial()->println("Available commands:");
  sender->GetSerial()->println("  assign [moduleIndex]");
  sender->GetSerial()->println("  stop");
  sender->GetSerial()->println("");

  inLink = true;
}


void handleLinkAssignCommand(SerialCommands* sender)
{
  if (!inLink) return;

  int moduleIndex = atoi(sender->Next());

  sender->GetSerial()->println("> LinkRequest response");
  const byte msg[] = { ResponseRequestLink, moduleIndex };
  sendCommMessage(msg, sizeof(msg));  
}


void handleLinkStopCommand(SerialCommands* sender)
{
  if (!inLink) return;

  sender->GetSerial()->println("> StopLink");
  const byte msg[] = { CommandStopLink };
  sendCommMessage(msg, sizeof(msg));

  inLink = false;
  showCommands();
}


void handlesetPWMCommand(SerialCommands* sender)
{
  if (inLink) return;

  int moduleIndex = atoi(sender->Next());
  int flags = atoi(sender->Next());
  int value1 = atoi(sender->Next());
  int value2 = atoi(sender->Next());
  
  sender->GetSerial()->println("> SetPWM");
  const byte msg[] = { CommandSetPWM, moduleIndex, flags, lowByte(value1), highByte(value1), lowByte(value2), highByte(value2) };
  sendCommMessage(msg, sizeof(msg));  
}


void handlegetSensorsCommand(SerialCommands* sender)
{
  if (inLink) return;

  int moduleIndex = atoi(sender->Next());
  
  sender->GetSerial()->println("> GetSensors");
  const byte msg[] = { CommandGetSensors, moduleIndex };
  sendCommMessage(msg, sizeof(msg));  
}


void handleCommMessage()
{
  uint8_t* data = comm.getData();
  uint8_t length = comm.getLength();

  Serial.print("Raw in: ");
  byte* ptr = data;
  for (size_t i = 0; i < length; i++, ptr++)
  {
    Serial.print(*ptr, HEX);
    Serial.print(' ');
  }
  Serial.println("");  

  if (length == 0)
    return;

  uint8_t command = *data; data++; length--;
  switch (command)
  {
    case ResponsePing: 
      if (length > 0)
        Serial.println("< Ping response, module index = " + String(*data));
      else
        Serial.println("< Ping response, module index = <missing>");
        
      break;

    case CommandRequestLink:
      Serial.println("< Request link");
      break;
      
    case ResponseSetPWM:
      Serial.println("< Set PWM response, module index = " + String(*data));
      break;

    case ResponseGetSensors:
      Serial.println("< Get sensors response, module index = " + String(*data)); data++; length--;
      if (length >= 2)
      {
        Serial.println("  Sensor 1 value: " + String(*reinterpret_cast<uint16_t*>(data))); data+=2; length-=2;
      }
      else
        Serial.println("  Sensor 1 value: <missing>");
        
      if (length >= 2)
      {
        Serial.println("  Sensor 2 value: " + String(*reinterpret_cast<uint16_t*>(data))); data+=2; length-=2;
      }
      else
        Serial.println("  Sensor 2 value: <missing>");
        
      break;

    case ResponseUhmWhat:
      Serial.println("< Uhm, what? module index = " + String(*data));
      break;
  }
}