#include "command-handler.h"

// Initialize empty, global command context
volatile command_context_t context = {UNKNOWN_COMMAND, SRC_NONE, FAN_NONE,
                                      CNF_NONE};

void parse_command(uint8_t command[], uint8_t command_len) {
  ///////////////////////
  // Command selection //
  ///////////////////////

  // Validate that we have a command
  if (command_len < 1) {
    context.command = UNKNOWN_COMMAND;
  }

  // Extract the first byte, which contains the command
  uint8_t command_byte = command[0];

  // Figure out which command to run
  switch (command_byte) {
  case 0x11: // Read config
    context.command = READ_CONFIG;
    break;
  case 0x12: // Read voltage
    context.command = READ_VOLTAGE;
    break;
  case 0x13: // Read temperature
    context.command = READ_TERMPERATURE;
    break;
  case 0x14: // Read current fan speed
    context.command = READ_FAN_SPEED;
    break;
  case 0x15: // Read bulk fan speed
    context.command = READ_BULK_FAN_SPEED;
    break;
  case 0x21: // Write config
    context.command = WRITE_CONFIG;
    break;
  case 0x22: // Clear stored fan speed data
    context.command = CLEAR_BULK_FAN_SPEED;
    break;
  default: // Unrecognized command
    context.command = UNKNOWN_COMMAND;
    break;
  }

  ///////////////////////////////
  // First parameter selection //
  ///////////////////////////////

  // Check if the command does not require a parameter. If it does not, return.
  if (context.command == READ_TERMPERATURE) {
    return;
  }

  // Validate that we have a second parameter, else requirements for command are
  // not fulfilled. return unknown command.
  if (command_len < 2) {
    context.command = UNKNOWN_COMMAND;
    return;
  }

  // Store the parameter
  uint8_t param = command[1];

  // Extract the parameter. Parameter is dependent on command
  switch (command[0]) {
  // Configuration parameters
  case 0x11: // Read config
  case 0x21: // Write config
      if (param == 0x01) {
          context.conf = SAMPLE_TIME;
      } else {
          context.conf = CNF_NONE;
      }
    break;

  // Voltage parameters
  case 0x12: // Read voltage
    if (param == 0x01) {
      context.src_voltage = SRC_INTERNAL;
    } else if (param == 0x02) {
      context.src_voltage = SRC_EXTRNAL;
    } else if (param == 0x03) {
      context.src_voltage = SRC_THERMISTOR;
    } else {
      context.src_voltage = SRC_NONE;
    }
    break;

  // Fan parameters
  case 0x14: // Read current fan speed
  case 0x15: // Read bulk fan speed
  case 0x22: // Clear stored fan speed data
    if (param == 0x01) {
      context.fan = FAN1;
    } else if (param == 0x02) {
      context.fan = FAN2;
    } else {
      context.fan = FAN_NONE;
    }
    break;

  // This should never be reached
  default: // Unrecognized command
    context.command = UNKNOWN_COMMAND;
    break;
  }

  /////////////////////////////////
  //  Third parameter selection  //
  /////////////////////////////////

  // Check if the command does not require a second parameter. If it does not,
  // return. Only config write requires a second parameter.
  if (context.command != WRITE_CONFIG) {
    return;
  }

  // Validate that we have a third parameter, else requirements for command are
  // not fulfilled. return unknown command.
  if (command_len < 3) {
    context.command = UNKNOWN_COMMAND;
    return;
  }

  // Store the parameter
  param = command[2];

  context.conf = param;
  

  ////////////////////////////////
  // Fourth parameter selection //
  ////////////////////////////////
  
  // Validate that we have a fourth parameter, else requirements for command are
  // not fulfilled. return unknown command. Second parameter is u16, thus two bytes.
  if (command_len < 5) {
    context.command = UNKNOWN_COMMAND;
    return;
  }
  
  // Extract the value
  union {
      uint16_t value;
      uint8_t bytes[2];
  } config_value;
  
  config_value.bytes[0] = command[3];
  config_value.bytes[1] = command[4];
  
  // Store the value
  context.conf_val = config_value.value;
  
  // exit
  return;
}

uint8_t route_command(int pos) {
 switch (context.command) {
    case WRITE_CONFIG:
      return WRITE_CONFIG;
    case READ_CONFIG:
    {
        // Validate that pos is within the valid range
        if (pos >= 2) { return 0x00; }
        
        // Config only has one parameter so we sent that parameter
        // Create a union to store the data
        union {
            uint16_t value;
            uint8_t bytes[2];
        } config_value;
        config_value.value = config.ms_fanspeed_sample_rate;
        
        uint8_t data = config_value.bytes[1-pos];
        
        // Return the corresponding data byte
        return data;
    }
        break;
      
    case READ_VOLTAGE:
    {
        // Create a union to store the data
        union {
            int16_t v;
            uint8_t bytes[2];
        } voltage;
        
        // Figure out which voltage source to read
        switch (context.src_voltage) {
            case SRC_INTERNAL:
                voltage.v = internal_voltage_read();
                break;
            case SRC_EXTRNAL:
                voltage.v = external_voltage_read();
                break;
            case SRC_THERMISTOR:
                voltage.v = thermistor_voltage_read();
                break;
            default:
                return 0xFF;
                break;
        }
        
        // Send the data
        return voltage.bytes[pos];
    }
    case READ_TERMPERATURE:
    {
        uint16_t v_therm = thermistor_voltage_read();
        union {
            int16_t temp;
            uint8_t bytes[2];
        } temperature;
        temperature.temp = (int16_t) ( calculate_thermistor_temp(v_therm) * 1000 );
        return temperature.bytes[pos];
    }
        break;
    case READ_FAN_SPEED:
        if (context.fan == FAN1) {
            return fan1_history[0];
        } else if (context.fan == FAN2) {
            return fan2_history[0];
        } else {
            return 0;
        }
        break;        
    case READ_BULK_FAN_SPEED:
        if (context.fan == FAN1) {
            return fan1_history[pos];
        } else if (context.fan == FAN2) {
            return fan2_history[pos];
        } else {
            return 0;
        }
        break;
    case CLEAR_BULK_FAN_SPEED:
        // Overwrite the content of the desired fan array with zeroes
        if (context.fan == FAN1) {
            memset(fan1_history, 0, sizeof(fan1_history));
        } else if (context.fan == FAN2) {
            memset(fan2_history, 0, sizeof(fan2_history));
        }
        break;
    case UNKNOWN_COMMAND:
    default:
      return 0xFF;
  }
}