text: /*
   * The MIT License (MIT)
   *
   * Copyright (c) 2016  B. Stultiens
   *
   * Permission is hereby granted, free of charge, to any person obtaining a copy
   * of this software and associated documentation files (the "Software"), to
   * deal in the Software without restriction, including without limitation the
   * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
   * sell copies of the Software, and to permit persons to whom the Software is
   * furnished to do so, subject to the following conditions:
   *
   * The above copyright notice and this permission notice shall be included in
   * all copies or substantial portions of the Software.
   *
   * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
   * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
   * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
   * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
   * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
   * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
   * IN THE SOFTWARE.
   */
  #include "fast_hsv2rgb.h"
  
  #if defined(HSV_USE_ASSEMBLY) && !defined(__AVR_ARCH__)
  #warning "Only AVR assembly is implemented. Other architectures use C fallback."
  #undef HSV_USE_ASSEMBLY
  #endif
  
  void fast_hsv2rgb_32bit(uint16_t h, uint8_t s, uint8_t v, uint8_t *r, uint8_t *g , uint8_t *b)
  {
  #ifndef HSV_USE_ASSEMBLY
  	HSV_MONOCHROMATIC_TEST(s, v, r, g, b);	// Exit with grayscale if s == 0
  
  	uint8_t sextant = h >> 8;
  
  	HSV_SEXTANT_TEST(sextant);		// Optional: Limit hue sextants to defined space
  
  	HSV_POINTER_SWAP(sextant, r, g, b);	// Swap pointers depending which sextant we are in
  
  	*g = v;		// Top level
  
  	// Perform actual calculations
  
  	/*
  	 * Bottom level: v * (1.0 - s)
  	 * --> (v * (255 - s) + error_corr + 1) / 256
  	 */
  	uint16_t ww;		// Intermediate result
  	ww = v * (255 - s);	// We don't use ~s to prevent size-promotion side effects
  	ww += 1;		// Error correction
  	ww += ww >> 8;		// Error correction
  	*b = ww >> 8;
  
  	uint8_t h_fraction = h & 0xff;	// 0...255
  	uint32_t d;			// Intermediate result
  
  	if(!(sextant & 1)) {
  		// *r = ...slope_up...;
  		d = v * (uint32_t)((255 << 8) - (uint16_t)(s * (256 - h_fraction)));
  		d += d >> 8;	// Error correction
  		d += v;		// Error correction
  		*r = d >> 16;
  	} else {
  		// *r = ...slope_down...;
  		d = v * (uint32_t)((255 << 8) - (uint16_t)(s * h_fraction));
  		d += d >> 8;	// Error correction
  		d += v;		// Error correction
  		*r = d >> 16;
  	}
  
  #else /* HSV_USE_ASSEMBLY */
  
  #ifdef __AVR_ARCH__
  	/*
  	 * Function arguments passed in registers:
  	 *   h = r25:r24
  	 *   s = r22
  	 *   v = r20
  	 *   *r = r19:r18
  	 *   *g = r17:r16
  	 *   *b = r15:r14
  	 */
  	asm volatile (
  		MOVW("r27", "r26", "r19", "r18")	// r -> X
  		MOVW("r29", "r28", "r17", "r16")	// g -> Y
  		MOVW("r31", "r30", "r15", "r14")	// b -> Z
  
  		"cpse	r22, __zero_reg__\n\t"	// if(!s) --> monochromatic
  		"rjmp	.Lneedcalc%=\n\t"
  
  		"st	X, r20\n\t"		// *r = *g = *b = v;
  		"st	Y, r20\n\t"
  		"st	Z, r20\n\t"
  		"rjmp	.Lendoffunc%=\n"	// return
  
  	".Lneedcalc%=:\n\t"
  
  		"cpi	r25, lo8(6)\n\t"	// if(hi8(h) > 5) hi8(h) = 5;
  		"brlo	.Linrange%=\n\t"
  		"ldi	r25,lo8(5)\n"
  	".Linrange%=:\n\t"
  
  		"sbrs	r25, 1\n\t"		// if(sextant & 2) swapptr(r, b);
  		"rjmp	.Lsextno1%=\n\t"
  		MOVW("r19", "r18", "r27", "r26")
  		MOVW("r27", "r26", "r31", "r30")
  		MOVW("r31", "r30", "r19", "r18")
  	"\n"
  	".Lsextno1%=:\n\t"
  
  		"sbrs	r25, 2\n\t"		// if(sextant & 4) swapptr(g, b);
  		"rjmp	.Lsextno2%=\n\t"
  		MOVW("r19", "r18", "r29", "r28")
  		MOVW("r29", "r28", "r31", "r30")
  		MOVW("r31", "r30", "r19", "r18")
  	"\n"
  	".Lsextno2%=:\n\t"
  
  		"ldi	r18, lo8(6)\n\t"
  		"and	r18, r25\n\t"		// if(!(sextant & 6))
  		"brne	.Lsext2345%=\n\t"
  
  		"sbrc	r25, 0\n\t"		// if(!(sextant & 6) && !(sextant & 1)) --> doswasp
  		"rjmp	.Ldoneswap%=\n"
  	".Lsext0%=:\n\t"
  		MOVW("r19", "r18", "r27", "r26")
  		MOVW("r27", "r26", "r29", "r28")
  		MOVW("r29", "r28", "r19", "r18")
  		"rjmp	.Ldoneswap%=\n"
  
  	".Lsext2345%=:\n\t"
  		"sbrc	r25, 0\n\t"		// if((sextant & 6) && (sextant & 1)) --> doswap
  		"rjmp	.Lsext0%=\n"
  	".Ldoneswap%=:\n\t"
  
  		/* Top level assignment first to free up Y register (r29:r28) */
  		"st	Y, r20\n\t"		// *g = v
  
  		"ldi	r18, 0\n\t"		// Temporary zero reg (r1 is used by mul)
  		"ldi	r19, 1\n\t"		// Temporary one reg
  
  		/*
  		 * Do bottom level next so we may use Z register (r31:r30).
  		 *
  		 *	Bottom level: v * (1.0 - s)
  		 *	--> (v * (255 - s) + error_corr + 1) / 256
  		 *	1 bb = ~s;
  		 *	2 ww = v * bb;
  		 *	3 ww += 1;
  		 *	4 ww += ww >> 8;	// error_corr for division 1/256 instead of 1/255
  		 *	5 *b = ww >> 8;
  		 */
  		"mov	r23, r22\n\t"		// 1 use copy of s
  		"com	r23\n\t"		// 1
  		MUL("r23", "r20", "a")		// 2 r1:r0 = v *  ~s
  		"add	r0, r19\n\t"		// 3 r1:r0 += 1
  		"adc	r1, r18\n\t"		// 3
  		"add	r0, r1\n\t"		// 4 r1:r0 += r1:r0 >> 8
  		"adc	r1, r18\n\t"		// 4
  		"st	Z, r1\n\t"		// 5 *b = r1:r0 >> 8
  
  		/* All that is left are the slopes */
  
  		"sbrc	r25, 0\n\t"		// if(sextant & 1) --> slope down
  		"rjmp	.Lslopedown%=\n\t"
  
  		/* Slope up:
  		 *	d = v * ((255 << 8) - (s * (256 - h_fraction)));
  		 *	d += d >> 8;	// Error correction
  		 *	d += v;		// Error correction
  		 *	*r = d >> 16;
  		 */
  		"ldi	r28, 0\n\t"		// 0 r19:r28 = 256 (0x100)
  		"sub	r28, r24\n\t"		// 0 256 - h_fraction
  		"sbc	r19, r18\n\t"
  		MUL("r22", "r28", "b")		// r1:r0 = s * lo8(256 - h_fraction)
  		"sbrc	r19, 0\n\t"		// if(256 - h_fraction == 0x100)
  		"add	r1, r22\n\t"		//   r1:r0 += s << 8
  		"rjmp	.Lslopecommon%=\n\t"	// r1:r0 holds inner multiplication
  
  		/*
  		 * Slope down:
  		 *	d = v * ((255 << 8) - (s * h_fraction));
  		 *	d += d >> 8;	// Error correction
  		 *	d += v;		// Error correction
  		 *	*r = d >> 16;
  		 */
  	"\n"
  	".Lslopedown%=:\n\t"
  		MUL("r22", "r24", "b")		// r1:r0 = s * h_fraction
  	"\n"
  	".Lslopecommon%=:\n\t"
  		"ldi	r31, 255\n\t"		// Z = 255 << 8
  		"mov	r30, r18\n\t"
  		"sub	r30, r0\n\t"		// Z = (255 << 8) - (s * (...))
  		"sbc	r31, r1\n\t"
  
  		/*
  		 * Multiply r20 * r31:r30:
  		 * r29:r28	= r20 * r31	lo8 * hi8
  		 * r1:r0	= r20 * r30	lo8 * lo8
  		 * Sum:
  		 *	r29 : r28 : (0)
  		 *	(0) : r1  : r0
  		 *	--------------- +
  		 *	r29 : r28 : r0
  		 */
  		MUL("r31", "r20", "c")
  		MOVW("r29", "r28", "r1", "r0")	// Y = hi8(Z) * v
  		MUL("r30", "r20", "d")		// r1:r0 = lo8(Z) * v
  		"add	r28, r1\n\t"
  		"adc	r29, r18\n\t"		// r29:r28:r0 = Y * v
  		"add	r0, r28\n\t"		// Error correction r29:r28:r0 += 0:r29:r28
  		"adc	r28, r29\n\t"
  		"adc	r29, r18\n\t"
  		"add	r0, r20\n\t"		// Error correction r29:r28:r0 += v
  		"adc	r28, r18\n\t"
  		"adc	r29, r18\n\t"
  		"st	X, r29\n\t"		// *r = slope result >> 16
  
  		"clr	__zero_reg__\n"		// Restore zero reg
  
  	".Lendoffunc%=:\n"
  	:
  	:
  	: "r31", "r30", "r29", "r28", "r27", "r26", "r25", "r24", "r23", "r22", "r19", "r18"
  #ifndef __AVR_HAVE_MUL__
  	 , "r17", "r16"
  #endif
  	);
  
  #else /* __AVR_ARCH__ */
  #error "No assembly version implemented for architecture"
  #endif
  #endif
  }
  