/**
  Text with ANSI colors in the Serial Terminal, using Arduino's ANSI library. 
  See "diagram.json" for an example how to configure the Serial Terminal.

  https://wokwi.com/arduino/projects/308893120796295745

  Copyright (c) 2021 Uri Shaked
*/

#include <SoftwareSerial.h>

SoftwareSerial Serial1(11,10);

#define BCD_SIZE 5
int bcdNumbers[BCD_SIZE];
unsigned char yaesuPacket[BCD_SIZE];


void decimalToBCD(int decimalNumber, int bcdArray[]) 
{ 
  for (int i = 0; i < 4; i++) 
  { 
    bcdArray[i] = decimalNumber % 10; 
    decimalNumber = decimalNumber / 10; 
  } 
} 

unsigned char * to_bcd(unsigned char bcd_data[], unsigned  long freq, unsigned bcd_len)
{
	int i;
	unsigned char a;

	/* '450'/4-> 5,0;0,4 */
	/* '450'/3-> 5,0;x,4 */

	for (i=0; i < bcd_len/2; i++) {
		a = freq%10;
		freq /= 10;
		a |= (freq%10)<<4;
		freq /= 10;
		bcd_data[i] = a;
	}
	if (bcd_len&1) {
		bcd_data[i] &= 0xf0;
		bcd_data[i] |= freq%10;	/* NB: high nibble is left uncleared */
	}

	return bcd_data;
}

void convertFrequencyToBCD(double frequency, unsigned char* output) {
    char freqStr[9]; // Include space for null terminator

    // Convert frequency to a padded string with the corrected factor
    sprintf(freqStr, "%08lu", (unsigned long)(frequency * 100000)); // Corrected multiplication factor

    // Convert each digit to BCD and store in array
    for (int i = 0; i < 4; i++) {
        int index = i * 2;
        byte high = freqStr[index] - '0';   // High nibble
        byte low = freqStr[index + 1] - '0'; // Low nibble
        output[3 - i] = (high << 4) | low;   // Store in reverse order
    }
}

String bcdBytesToHexString(byte* bcdBytes) {
    String hexString = "";
    for (int i = 0; i < 4; i++) {
        if (bcdBytes[i] < 0x10) hexString += "0"; // Add leading zero if needed
        hexString += String(bcdBytes[i], HEX);
    }
    hexString.toUpperCase(); // Convert to uppercase
    return hexString;
}

void printHexArray(const unsigned char array[5]) {
    for (int i = 0; i < 5; i++) {
        Serial.print("0x"); // Optional prefix for clarity
        Serial.print(array[i], HEX); // Print the value in hexadecimal
        Serial.print(' '); // Add a space between each value for readability
    }
    Serial.println(); // Newline after printing the array
}

void YaesuCommand(int command, unsigned char bcdArray[])
{
  yaesuPacket[0] = (char)bcdArray[3];
  yaesuPacket[1] = (char)bcdArray[2];
  yaesuPacket[2] = (char)bcdArray[1];
  yaesuPacket[3] = (char)bcdArray[0];
  yaesuPacket[4] = (char)command;
}
void setup() {
  Serial.begin(115200);
  Serial.println("Hello, I'm in a terminal!");
  Serial.println();
 // bcdNumbers[4] = 0x0A;

long testNumber = 00001425000;

unsigned char bcdArray[5];
 
 // printHexArray(to_bcd(bcdArray, 000014250000L/10, 8)); 
 
convertFrequencyToBCD((float)000014250000L/100000.0, bcdArray);
Serial.println(bcdBytesToHexString(bcdArray));
Serial.println(printHexArray(YaesuCommand(0x0a,bcdArray)));
  
}

const char *loopChars = "-\\|/";
int i;

void loop() {
  Serial1.print("ABCDE");  delay(1000);
}